Merge pull request AtsushiSakai#297 from AtsushiSakai/issue_294

Visibility Graph Path Planner implemenation

Author: AtsushiSakai

PathPlanning/Dijkstra/dijkstra.py

@@ -11,32 +11,42 @@
 
 show_animation = True
 
+
 class Dijkstra:
 
-    def __init__(self, ox, oy, reso, rr):
+    def __init__(self, ox, oy, resolution, robot_radius):
         """
         Initialize map for a star planning
 
         ox: x position list of Obstacles [m]
         oy: y position list of Obstacles [m]
-        reso: grid resolution [m]
+        resolution: grid resolution [m]
         rr: robot radius[m]
         """
 
-        self.reso = reso
-        self.rr = rr
+        self.min_x = None
+        self.min_y = None
+        self.max_x = None
+        self.max_y = None
+        self.x_width = None
+        self.y_width = None
+        self.obstacle_map = None
+
+        self.resolution = resolution
+        self.robot_radius = robot_radius
         self.calc_obstacle_map(ox, oy)
         self.motion = self.get_motion_model()
 
     class Node:
-        def __init__(self, x, y, cost, pind):
+        def __init__(self, x, y, cost, parent):
             self.x = x  # index of grid
             self.y = y  # index of grid
             self.cost = cost
-            self.pind = pind # index of previous Node
+            self.parent = parent  # index of previous Node
 
         def __str__(self):
-            return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+            return str(self.x) + "," + str(self.y) + "," + str(
+                self.cost) + "," + str(self.parent)
 
     def planning(self, sx, sy, gx, gy):
         """
@@ -53,39 +63,40 @@ def planning(self, sx, sy, gx, gy):
             ry: y position list of the final path
         """
 
-        nstart = self.Node(self.calc_xyindex(sx, self.minx),
-                           self.calc_xyindex(sy, self.miny), 0.0, -1)
-        ngoal = self.Node(self.calc_xyindex(gx, self.minx),
-                          self.calc_xyindex(gy, self.miny), 0.0, -1)
+        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
+                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
+        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
+                              self.calc_xy_index(gy, self.min_y), 0.0, -1)
 
-        openset, closedset = dict(), dict()
-        openset[self.calc_index(nstart)] = nstart
+        open_set, closed_set = dict(), dict()
+        open_set[self.calc_index(start_node)] = start_node
 
         while 1:
-            c_id = min(openset, key=lambda o: openset[o].cost)
-            current = openset[c_id]
+            c_id = min(open_set, key=lambda o: open_set[o].cost)
+            current = open_set[c_id]
 
             # show graph
             if show_animation:  # pragma: no cover
-                plt.plot(self.calc_position(current.x, self.minx),
-                         self.calc_position(current.y, self.miny), "xc")
+                plt.plot(self.calc_position(current.x, self.min_x),
+                         self.calc_position(current.y, self.min_y), "xc")
                 # for stopping simulation with the esc key.
-                plt.gcf().canvas.mpl_connect('key_release_event',
-                        lambda event: [exit(0) if event.key == 'escape' else None])
-                if len(closedset.keys()) % 10 == 0:
+                plt.gcf().canvas.mpl_connect(
+                    'key_release_event',
+                    lambda event: [exit(0) if event.key == 'escape' else None])
+                if len(closed_set.keys()) % 10 == 0:
                     plt.pause(0.001)
 
-            if current.x == ngoal.x and current.y == ngoal.y:
+            if current.x == goal_node.x and current.y == goal_node.y:
                 print("Find goal")
-                ngoal.pind = current.pind
-                ngoal.cost = current.cost
+                goal_node.parent = current.parent
+                goal_node.cost = current.cost
                 break
 
             # Remove the item from the open set
-            del openset[c_id]
+            del open_set[c_id]
 
             # Add it to the closed set
-            closedset[c_id] = current
+            closed_set[c_id] = current
 
             # expand search grid based on motion model
             for move_x, move_y, move_cost in self.motion:
@@ -94,94 +105,95 @@ def planning(self, sx, sy, gx, gy):
                                  current.cost + move_cost, c_id)
                 n_id = self.calc_index(node)
 
-                if n_id in closedset:
+                if n_id in closed_set:
                     continue
 
                 if not self.verify_node(node):
                     continue
 
-                if n_id not in openset:
-                    openset[n_id] = node  # Discover a new node
+                if n_id not in open_set:
+                    open_set[n_id] = node  # Discover a new node
                 else:
-                    if openset[n_id].cost >= node.cost:
+                    if open_set[n_id].cost >= node.cost:
                         # This path is the best until now. record it!
-                        openset[n_id] = node
+                        open_set[n_id] = node
 
-        rx, ry = self.calc_final_path(ngoal, closedset)
+        rx, ry = self.calc_final_path(goal_node, closed_set)
 
         return rx, ry
 
-    def calc_final_path(self, ngoal, closedset):
+    def calc_final_path(self, goal_node, closed_set):
         # generate final course
-        rx, ry = [self.calc_position(ngoal.x, self.minx)], [
-            self.calc_position(ngoal.y, self.miny)]
-        pind = ngoal.pind
-        while pind != -1:
-            n = closedset[pind]
-            rx.append(self.calc_position(n.x, self.minx))
-            ry.append(self.calc_position(n.y, self.miny))
-            pind = n.pind
+        rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
+            self.calc_position(goal_node.y, self.min_y)]
+        parent = goal_node.parent
+        while parent != -1:
+            n = closed_set[parent]
+            rx.append(self.calc_position(n.x, self.min_x))
+            ry.append(self.calc_position(n.y, self.min_y))
+            parent = n.parent
 
         return rx, ry
 
     def calc_position(self, index, minp):
-        pos = index*self.reso+minp
+        pos = index * self.resolution + minp
         return pos
 
-    def calc_xyindex(self, position, minp):
-        return round((position - minp)/self.reso)
+    def calc_xy_index(self, position, minp):
+        return round((position - minp) / self.resolution)
 
     def calc_index(self, node):
-        return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
+        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
 
     def verify_node(self, node):
-        px = self.calc_position(node.x, self.minx)
-        py = self.calc_position(node.y, self.miny)
+        px = self.calc_position(node.x, self.min_x)
+        py = self.calc_position(node.y, self.min_y)
 
-        if px < self.minx:
+        if px < self.min_x:
             return False
-        elif py < self.miny:
+        if py < self.min_y:
             return False
-        elif px >= self.maxx:
+        if px >= self.max_x:
             return False
-        elif py >= self.maxy:
+        if py >= self.max_y:
             return False
 
-        if self.obmap[node.x][node.y]:
+        if self.obstacle_map[node.x][node.y]:
             return False
 
         return True
 
     def calc_obstacle_map(self, ox, oy):
 
-        self.minx = round(min(ox))
-        self.miny = round(min(oy))
-        self.maxx = round(max(ox))
-        self.maxy = round(max(oy))
-        print("minx:", self.minx)
-        print("miny:", self.miny)
-        print("maxx:", self.maxx)
-        print("maxy:", self.maxy)
+        self.min_x = round(min(ox))
+        self.min_y = round(min(oy))
+        self.max_x = round(max(ox))
+        self.max_y = round(max(oy))
+        print("min_x:", self.min_x)
+        print("min_y:", self.min_y)
+        print("max_x:", self.max_x)
+        print("max_y:", self.max_y)
 
-        self.xwidth = round((self.maxx - self.minx)/self.reso)
-        self.ywidth = round((self.maxy - self.miny)/self.reso)
-        print("xwidth:", self.xwidth)
-        print("ywidth:", self.ywidth)
+        self.x_width = round((self.max_x - self.min_x) / self.resolution)
+        self.y_width = round((self.max_y - self.min_y) / self.resolution)
+        print("x_width:", self.x_width)
+        print("y_width:", self.y_width)
 
         # obstacle map generation
-        self.obmap = [[False for i in range(self.ywidth)]
-                      for i in range(self.xwidth)]
-        for ix in range(self.xwidth):
-            x = self.calc_position(ix, self.minx)
-            for iy in range(self.ywidth):
-                y = self.calc_position(iy, self.miny)
+        self.obstacle_map = [[False for _ in range(self.y_width)]
+                             for _ in range(self.x_width)]
+        for ix in range(self.x_width):
+            x = self.calc_position(ix, self.min_x)
+            for iy in range(self.y_width):
+                y = self.calc_position(iy, self.min_y)
                 for iox, ioy in zip(ox, oy):
                     d = math.hypot(iox - x, ioy - y)
-                    if d <= self.rr:
-                        self.obmap[ix][iy] = True
+                    if d <= self.robot_radius:
+                        self.obstacle_map[ix][iy] = True
                         break
 
-    def get_motion_model(self):
+    @staticmethod
+    def get_motion_model():
         # dx, dy, cost
         motion = [[1, 0, 1],
                   [0, 1, 1],
@@ -239,6 +251,7 @@ def main():
 
     if show_animation:  # pragma: no cover
         plt.plot(rx, ry, "-r")
+        plt.pause(0.01)
         plt.show()
 
 

PathPlanning/VisibilityRoadMap/__init__.py

@@ -0,0 +1,43 @@
+class Geometry:
+    class Point:
+        def __init__(self, x, y):
+            self.x = x
+            self.y = y
+
+    @staticmethod
+    def is_seg_intersect(p1, q1, p2, q2):
+
+        def on_segment(p, q, r):
+            if ((q.x <= max(p.x, r.x)) and (q.x >= min(p.x, r.x)) and
+                    (q.y <= max(p.y, r.y)) and (q.y >= min(p.y, r.y))):
+                return True
+            return False
+
+        def orientation(p, q, r):
+            val = (float(q.y - p.y) * (r.x - q.x)) - (
+                    float(q.x - p.x) * (r.y - q.y))
+            if val > 0:
+                return 1
+            if val < 0:
+                return 2
+            return 0
+
+        # Find the 4 orientations required for
+        # the general and special cases
+        o1 = orientation(p1, q1, p2)
+        o2 = orientation(p1, q1, q2)
+        o3 = orientation(p2, q2, p1)
+        o4 = orientation(p2, q2, q1)
+
+        if (o1 != o2) and (o3 != o4):
+            return True
+        if (o1 == 0) and on_segment(p1, p2, q1):
+            return True
+        if (o2 == 0) and on_segment(p1, q2, q1):
+            return True
+        if (o3 == 0) and on_segment(p2, p1, q2):
+            return True
+        if (o4 == 0) and on_segment(p2, q1, q2):
+            return True
+
+        return False