github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04: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

8
PathPlanning/RRTStarDubins/rrt_star_dubins.py
View File

@@ -46,7 +46,8 @@ class RRTStarDubins(RRTStar):
def __init__(self, start, goal, obstacle_list, rand_area,
goal_sample_rate=10,
max_iter=200,
connect_circle_dist=50.0
connect_circle_dist=50.0,
robot_radius=0.0,
):
"""
Setting Parameter
@@ -55,6 +56,7 @@ class RRTStarDubins(RRTStar):
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
robot_radius: robot body modeled as circle with given radius
"""
self.start = self.Node(start[0], start[1], start[2])
@@ -69,6 +71,7 @@ class RRTStarDubins(RRTStar):
self.curvature = 1.0 # for dubins path
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
self.robot_radius = robot_radius
def planning(self, animation=True, search_until_max_iter=True):
"""
@@ -84,7 +87,8 @@ class RRTStarDubins(RRTStar):
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
if self.check_collision(new_node, self.obstacle_list):
if self.check_collision(
new_node, self.obstacle_list, self.robot_radius):
near_indexes = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_indexes)
if new_node: