Limit play area in RRT (AtsushiSakai#540)

* 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

Author: szaguldo-kamaz

PathPlanning/RRT/rrt.py

@@ -32,6 +32,15 @@ def __init__(self, x, y):
             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,20 +49,27 @@ def __init__(self,
                  expand_dis=3.0,
                  path_resolution=0.5,
                  goal_sample_rate=5,
-                 max_iter=500):
+                 max_iter=500,
+                 play_area=None
+                 ):
         """
         Setting Parameter
 
         start:Start Position [x,y]
         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 @@ def planning(self, animation=True):
 
             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 @@ def draw_graph(self, rnd=None):
         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 @@ def get_nearest_node_index(node_list, rnd_node):
 
         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: