github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 02:08:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

Limit play area in RRT (#540)

Browse Source 
* add support for limiting the play area

* plot the play area

* Update rrt.py

* fix param list

* fix code style

* fix more code style

* fix code style even more
This commit is contained in:
szaguldo-kamaz
2021-10-03 16:40:44 +09:00
committed by GitHub
parent a2fc9f914a
commit acc3604f73
1 changed files with 43 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
PathPlanning/RRT/rrt.py
View File

@@ -32,6 +32,15 @@ class RRT:
self.path_y = []
self.parent = None
class AreaBounds:
def __init__(self, area):
self.xmin = float(area[0])
self.xmax = float(area[1])
self.ymin = float(area[2])
self.ymax = float(area[3])
def __init__(self,
start,
goal,
@@ -40,7 +49,9 @@ class RRT:
expand_dis=3.0,
path_resolution=0.5,
goal_sample_rate=5,
max_iter=500):
max_iter=500,
play_area=None
):
"""
Setting Parameter
@@ -48,12 +59,17 @@ class RRT:
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
play_area:stay inside this area [xmin,xmax,ymin,ymax]
"""
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
if play_area is not None:
self.play_area = self.AreaBounds(play_area)
else:
self.play_area = None
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
@@ -76,7 +92,8 @@ class RRT:
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if self.check_collision(new_node, self.obstacle_list):
if self.check_if_outside_play_area(new_node, self.play_area) and \
self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
if animation and i % 5 == 0:
@@ -163,6 +180,15 @@ class RRT:
for (ox, oy, size) in self.obstacle_list:
self.plot_circle(ox, oy, size)
if self.play_area is not None:
plt.plot([self.play_area.xmin, self.play_area.xmax,
self.play_area.xmax, self.play_area.xmin,
self.play_area.xmin],
[self.play_area.ymin, self.play_area.ymin,
self.play_area.ymax, self.play_area.ymax,
self.play_area.ymin],
"-k")
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
@@ -186,6 +212,18 @@ class RRT:
return minind
@staticmethod
def check_if_outside_play_area(node, play_area):
if play_area is None:
return True # no play_area was defined, every pos should be ok
if node.x < play_area.xmin or node.x > play_area.xmax or \
node.y < play_area.ymin or node.y > play_area.ymax:
return False # outside - bad
else:
return True # inside - ok
@staticmethod
def check_collision(node, obstacleList):
@@ -222,7 +260,9 @@ def main(gx=6.0, gy=10.0):
start=[0, 0],
goal=[gx, gy],
rand_area=[-2, 15],
obstacle_list=obstacleList)
obstacle_list=obstacleList,
# play_area=[0, 10, 0, 14]
)
path = rrt.planning(animation=show_animation)
if path is None: