From bbe005af148d89ecd37674b0e3164fb3324c6600 Mon Sep 17 00:00:00 2001 From: shibo Date: Mon, 9 Jun 2025 11:47:03 +0800 Subject: [PATCH 1/2] Fix: Include robot_radius in path_smoothing collision check --- PathPlanning/RRT/rrt_with_pathsmoothing.py | 86 +++++++++++++++------- 1 file changed, 59 insertions(+), 27 deletions(-) diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 2ed6a366d1..b59a486b28 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -51,30 +51,60 @@ def get_target_point(path, targetL): return [x, y, ti] -def line_collision_check(first, second, obstacleList): - # Line Equation - - x1 = first[0] - y1 = first[1] - x2 = second[0] - y2 = second[1] - - try: - a = y2 - y1 - b = -(x2 - x1) - c = y2 * (x2 - x1) - x2 * (y2 - y1) - except ZeroDivisionError: - return False - - 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 - - -def path_smoothing(path, max_iter, obstacle_list): +def line_collision_check(first, second, obstacle_list, robot_radius=0.0): + """ + Check if the line segment between `first` and `second` collides with any obstacle. + Considers the robot_radius by inflating the obstacle size. + """ + 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: + return True # Degenerate case + + steps = int(length / 0.2) + 1 # Sampling every 0.2m along the segment + + for i in range(steps + 1): + t = i / steps + x = x1 + t * dx + y = y1 + t * dy + + for (ox, oy, size) in obstacle_list: + d = math.hypot(ox - x, oy - y) + if d <= size + robot_radius: + return False # Collision + + 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 +124,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 @@ -121,12 +151,14 @@ def main(): (9, 5, 2) ] # [x,y,size] 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: From f57fb4ff1ac2028a769966f46207d671b5a84600 Mon Sep 17 00:00:00 2001 From: shibo Date: Mon, 9 Jun 2025 11:47:27 +0800 Subject: [PATCH 2/2] Test: Add unit test to verify smoothed path respects robot_radius --- PathPlanning/RRT/rrt_with_pathsmoothing.py | 49 +++++++++++++++++---- tests/test_rrt_with_pathsmoothing_radius.py | 48 ++++++++++++++++++++ 2 files changed, 89 insertions(+), 8 deletions(-) create mode 100644 tests/test_rrt_with_pathsmoothing_radius.py diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index b59a486b28..ac68efe23f 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -51,10 +51,44 @@ def get_target_point(path, targetL): return [x, y, ti] -def line_collision_check(first, second, obstacle_list, robot_radius=0.0): +def is_point_collision(x, y, obstacle_list, robot_radius): + """ + Check whether a single point collides with any obstacle. + + 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. + + 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. + + 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 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] @@ -64,19 +98,18 @@ def line_collision_check(first, second, obstacle_list, robot_radius=0.0): length = math.hypot(dx, dy) if length == 0: - return True # Degenerate case + # Degenerate case: point collision check + return not is_point_collision(x1, y1, obstacle_list, robot_radius) - steps = int(length / 0.2) + 1 # Sampling every 0.2m along the segment + 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 - for (ox, oy, size) in obstacle_list: - d = math.hypot(ox - x, oy - y) - if d <= size + robot_radius: - return False # Collision + if is_point_collision(x, y, obstacle_list, robot_radius): + return False # Collision found return True # Safe @@ -149,7 +182,7 @@ 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, robot_radius=0.3) diff --git a/tests/test_rrt_with_pathsmoothing_radius.py b/tests/test_rrt_with_pathsmoothing_radius.py new file mode 100644 index 0000000000..a1159255b5 --- /dev/null +++ b/tests/test_rrt_with_pathsmoothing_radius.py @@ -0,0 +1,48 @@ +import conftest +import math + +from PathPlanning.RRT import rrt_with_pathsmoothing as rrt_module + +def test_smoothed_path_safety(): + # Define test environment + obstacle_list = [ + (5, 5, 1.0), + (3, 6, 2.0), + (3, 8, 2.0), + (3, 10, 2.0), + (7, 5, 2.0), + (9, 5, 2.0) + ] + robot_radius = 0.5 + + # Disable animation for testing + rrt_module.show_animation = False + + # Create RRT planner + rrt = rrt_module.RRT( + start=[0, 0], + goal=[6, 10], + rand_area=[-2, 15], + obstacle_list=obstacle_list, + robot_radius=robot_radius + ) + + # Run RRT + path = rrt.planning(animation=False) + + # Smooth the path + smoothed = rrt_module.path_smoothing(path, max_iter=1000, + obstacle_list=obstacle_list, + robot_radius=robot_radius) + + # Check if all points on the smoothed path are safely distant from obstacles + for x, y in smoothed: + for ox, oy, obs_radius in obstacle_list: + d = math.hypot(x - ox, y - oy) + min_safe_dist = obs_radius + robot_radius + assert d > min_safe_dist, \ + f"Point ({x:.2f}, {y:.2f}) too close to obstacle at ({ox}, {oy})" + + +if __name__ == '__main__': + conftest.run_this_test(__file__)