Fix/path smoothing robot radius #1231
Conversation
There was a problem hiding this comment.
Pull Request Overview
This PR ensures the path smoothing routine respects the robot’s radius by updating collision checks and adding a safety-focused unit test.
- Expanded line_collision_check to sample finite-length segments and inflate obstacles by robot_radius.
- Updated path_smoothing to pass robot_radius into collision checks.
- Added a new test verifying no smoothed path point encroaches on obstacles.
Reviewed Changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 2 comments.
| File | Description |
|---|---|
| tests/test_rrt_with_pathsmoothing_radius.py | New test ensuring smoothed path maintains clearance. |
| PathPlanning/RRT/rrt_with_pathsmoothing.py | Modified collision logic and updated function calls. |
Comments suppressed due to low confidence (2)
tests/test_rrt_with_pathsmoothing_radius.py:43
- [nitpick] Using a strict '>' may fail when a point lies exactly at the clearance boundary. Consider using '>=' if touching the safety radius is acceptable or adjust the test to account for floating-point tolerances.
assert d > min_safe_dist, \
PathPlanning/RRT/rrt_with_pathsmoothing.py:76
- [nitpick] The variable 'size' represents an obstacle radius. Rename it to 'obs_radius' or 'radius' for clearer semantics.
for (ox, oy, size) in obstacle_list:
| length = math.hypot(dx, dy) | ||
|
|
||
| if length == 0: | ||
| return True # Degenerate case |
There was a problem hiding this comment.
The degenerate case returns True even if the single point lies inside an inflated obstacle. Instead, explicitly check collision at (x1, y1) against all obstacles with robot_radius.
| return True # Degenerate case | |
| for (ox, oy, size) in obstacle_list: | |
| d = math.hypot(ox - x1, oy - y1) | |
| if d <= size + robot_radius: | |
| return False # Collision | |
| return True # Safe |
Copilot uses AI. Check for mistakes.
| if length == 0: | ||
| return True # Degenerate case | ||
|
|
||
| steps = int(length / 0.2) + 1 # Sampling every 0.2m along the segment |
There was a problem hiding this comment.
[nitpick] The magic value 0.2 is hard-coded. Consider extracting it to a named constant (e.g., SAMPLE_STEP) or making it a configurable parameter for easier tuning and clarity.
| steps = int(length / 0.2) + 1 # Sampling every 0.2m along the segment | |
| steps = int(length / SAMPLE_STEP) + 1 # Sampling every SAMPLE_STEP meters along the segment |
Copilot uses AI. Check for mistakes.
| if length == 0: | ||
| return True # Degenerate case | ||
|
|
||
| steps = int(length / 0.2) + 1 # Sampling every 0.2m along the segment |
There was a problem hiding this comment.
This 0.2 should be an optional argument also for this function.
| @@ -121,12 +151,14 @@ def main(): | |||
| (9, 5, 2) | |||
| ] # [x,y,size] | |||
There was a problem hiding this comment.
| ] # [x,y,size] | |
| ] # [x,y,radius] |
| x = x1 + t * dx | ||
| y = y1 + t * dy | ||
|
|
||
| for (ox, oy, size) in obstacle_list: |
There was a problem hiding this comment.
This size should be obstacle_radius.
Reference issue
Fixes path smoothing safety issue discussed in Bug: path_smoothing() Ignores robot_radius, Causing Smoothed Paths to Violate Obstacle Clearance #1230
What does this implement/fix?
This PR fixes an issue in the
path_smoothing()function, where the collision checking vialine_collision_check()did not takerobot_radiusinto account correctly. This caused the smoothed path to pass dangerously close to obstacles.line_collision_check()to check collision along finite-length line segments and incorporaterobot_radius.path_smoothing()logic to use this corrected check.Additional information
CheckList