diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 2ed6a366d1..ac68efe23f 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/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 - - 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 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] + + 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: 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__)