github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-17 05:11:32 -05:00
Code Issues Packages Projects Releases Wiki Activity

Add optional robot radius to RRT/RRTStar path planners (#655)

Browse Source 
* Add optional robot radius to RRT/RRTStar path planners.
* update __init__ and check_collision to include radius
* during animation, if a robot radius is given then it is drawn

* Add test for robot radius

* Correct import error

* Correct missing robot_radius errors

* Address "expected 2 blank lines, found 1" error

* Address "line too long" errors

* Add missing argument description.

* Remove collision_check_with_xy and replace with check_collision

* Fix "missing whitespace after ','" error

* Update PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>

Co-authored-by: Atsushi Sakai <asakai.amsl+github@gmail.com>
This commit is contained in:
Chris Mower
2022-04-19 04:53:24 +01:00
committed by GitHub
parent af2061a1e7
commit b53fdf75f6
10 changed files with 88 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -37,11 +37,13 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
def __init__(self, start, goal, obstacle_list, rand_area,
max_iter=200,
connect_circle_dist=50.0
connect_circle_dist=50.0,
robot_radius=0.0
):
super().__init__(start, goal, obstacle_list, rand_area,
max_iter=max_iter,
connect_circle_dist=connect_circle_dist,
robot_radius=robot_radius
)
self.target_speed = 10.0 / 3.6
@@ -127,7 +129,11 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
print("path is too long")
find_goal = False
if not self.collision_check_with_xy(x, y, self.obstacle_list):
tmp_node = self.Node(x, y, 0)
tmp_node.path_x = x
tmp_node.path_y = y
if not self.check_collision(
tmp_node, self.obstacle_list, self.robot_radius):
print("This path is collision")
find_goal = False
@@ -151,19 +157,6 @@ class ClosedLoopRRTStar(RRTStarReedsShepp):
return fgoalinds
@staticmethod
def collision_check_with_xy(x, y, obstacle_list):
for (ox, oy, size) in obstacle_list:
for (ix, iy) in zip(x, y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
return True # safe
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
print("Start" + __file__)