Fix/path smoothing robot radius (#1231)

* Fix: Include robot_radius in path_smoothing collision check

* Test: Add unit test to verify smoothed path respects robot_radius

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -51,30 +51,93 @@ def get_target_point(path, targetL):
     return [x, y, ti]
 
 
-def line_collision_check(first, second, obstacleList):
-    # Line Equation
+def is_point_collision(x, y, obstacle_list, robot_radius):
+    """
+    Check whether a single point collides with any obstacle.
 
-    x1 = first[0]
-    y1 = first[1]
-    x2 = second[0]
-    y2 = second[1]
+    This function calculates the Euclidean distance between the given point (x, y)
+    and each obstacle center. If the distance is less than or equal to the sum of
+    the obstacle's radius and the robot's radius, a collision is detected.
 
-    try:
-        a = y2 - y1
-        b = -(x2 - x1)
-        c = y2 * (x2 - x1) - x2 * (y2 - y1)
-    except ZeroDivisionError:
-        return False
+    Args:
+        x (float): X-coordinate of the point to check.
+        y (float): Y-coordinate of the point to check.
+        obstacle_list (List[Tuple[float, float, float]]): List of obstacles defined as (ox, oy, radius).
+        robot_radius (float): Radius of the robot, used to inflate the obstacles.
 
-    for (ox, oy, size) in obstacleList:
-        d = abs(a * ox + b * oy + c) / (math.hypot(a, b))
-        if d <= size:
-            return False
-
-    return True  # OK
+    Returns:
+        bool: True if the point is in collision with any obstacle, False otherwise.
+    """
+    for (ox, oy, obstacle_radius) in obstacle_list:
+        d = math.hypot(ox - x, oy - y)
+        if d <= obstacle_radius + robot_radius:
+            return True  # Collided
+    return False
 
 
-def path_smoothing(path, max_iter, obstacle_list):
+def line_collision_check(first, second, obstacle_list, robot_radius=0.0, sample_step=0.2):
+    """
+    Check if the line segment between `first` and `second` collides with any obstacle.
+    Considers the robot_radius by inflating the obstacle size.
+
+    Args:
+        first (List[float]): Start point of the line [x, y]
+        second (List[float]): End point of the line [x, y]
+        obstacle_list (List[Tuple[float, float, float]]): Obstacles as (x, y, radius)
+        robot_radius (float): Radius of robot
+        sample_step (float): Distance between sampling points along the segment
+
+    Returns:
+        bool: True if collision-free, False otherwise
+    """
+    x1, y1 = first[0], first[1]
+    x2, y2 = second[0], second[1]
+
+    dx = x2 - x1
+    dy = y2 - y1
+    length = math.hypot(dx, dy)
+
+    if length == 0:
+        # Degenerate case: point collision check
+        return not is_point_collision(x1, y1, obstacle_list, robot_radius)
+
+    steps = int(length / sample_step) + 1  # Sampling every sample_step along the segment
+
+    for i in range(steps + 1):
+        t = i / steps
+        x = x1 + t * dx
+        y = y1 + t * dy
+
+        if is_point_collision(x, y, obstacle_list, robot_radius):
+            return False  # Collision found
+
+    return True  # Safe
+
+
+def path_smoothing(path, max_iter, obstacle_list, robot_radius=0.0):
+    """
+    Smooths a given path by iteratively replacing segments with shortcut connections,
+    while ensuring the new segments are collision-free.
+
+    The algorithm randomly picks two points along the original path and attempts to
+    connect them with a straight line. If the line does not collide with any obstacles
+    (considering the robot's radius), the intermediate path points between them are
+    replaced with the direct connection.
+
+    Args:
+        path (List[List[float]]): The original path as a list of [x, y] coordinates.
+        max_iter (int): Number of iterations for smoothing attempts.
+        obstacle_list (List[Tuple[float, float, float]]): List of obstacles represented as
+            (x, y, radius).
+        robot_radius (float, optional): Radius of the robot, used to inflate obstacle size
+            during collision checking. Defaults to 0.0.
+
+    Returns:
+        List[List[float]]: The smoothed path as a list of [x, y] coordinates.
+
+    Example:
+        >>> smoothed = path_smoothing(path, 1000, obstacle_list, robot_radius=0.5)
+    """
     le = get_path_length(path)
 
     for i in range(max_iter):
@@ -94,7 +157,7 @@ def path_smoothing(path, max_iter, obstacle_list):
             continue
 
         # collision check
-        if not line_collision_check(first, second, obstacle_list):
+        if not line_collision_check(first, second, obstacle_list, robot_radius):
             continue
 
         # Create New path
@@ -119,14 +182,16 @@ def main():
         (3, 10, 2),
         (7, 5, 2),
         (9, 5, 2)
-    ]  # [x,y,size]
+    ]  # [x,y,radius]
     rrt = RRT(start=[0, 0], goal=[6, 10],
-              rand_area=[-2, 15], obstacle_list=obstacleList)
+              rand_area=[-2, 15], obstacle_list=obstacleList,
+              robot_radius=0.3)
     path = rrt.planning(animation=show_animation)
 
     # Path smoothing
     maxIter = 1000
-    smoothedPath = path_smoothing(path, maxIter, obstacleList)
+    smoothedPath = path_smoothing(path, maxIter, obstacleList,
+                                  robot_radius=rrt.robot_radius)
 
     # Draw final path
     if show_animation: