add visibility road map path doc (AtsushiSakai#592)

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

* add visibility road map path doc

Author: AtsushiSakai

PathPlanning/VisibilityRoadMap/geometry.py

@@ -1,4 +1,5 @@
 class Geometry:
+
     class Point:
         def __init__(self, x, y):
             self.x = x
@@ -40,4 +41,4 @@ def orientation(p, q, r):
         if (o4 == 0) and on_segment(p2, q1, q2):
             return True
 
-        return False
+        return False

PathPlanning/VisibilityRoadMap/visibility_road_map.py

@@ -23,14 +23,14 @@
 
 class VisibilityRoadMap:
 
-    def __init__(self, robot_radius, do_plot=False):
-        self.robot_radius = robot_radius
+    def __init__(self, expand_distance, do_plot=False):
+        self.expand_distance = expand_distance
         self.do_plot = do_plot
 
     def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
 
-        nodes = self.generate_graph_node(start_x, start_y, goal_x, goal_y,
-                                         obstacles)
+        nodes = self.generate_visibility_nodes(start_x, start_y,
+                                               goal_x, goal_y, obstacles)
 
         road_map_info = self.generate_road_map_info(nodes, obstacles)
 
@@ -48,7 +48,8 @@ def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
 
         return rx, ry
 
-    def generate_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles):
+    def generate_visibility_nodes(self, start_x, start_y, goal_x, goal_y,
+                                  obstacles):
 
         # add start and goal as nodes
         nodes = [DijkstraSearch.Node(start_x, start_y),
@@ -129,8 +130,8 @@ def calc_offset_xy(self, px, py, x, y, nx, ny):
         offset_vec = math.atan2(math.sin(p_vec) + math.sin(n_vec),
                                 math.cos(p_vec) + math.cos(
                                     n_vec)) + math.pi / 2.0
-        offset_x = x + self.robot_radius * math.cos(offset_vec)
-        offset_y = y + self.robot_radius * math.sin(offset_vec)
+        offset_x = x + self.expand_distance * math.cos(offset_vec)
+        offset_y = y + self.expand_distance * math.sin(offset_vec)
         return offset_x, offset_y
 
     @staticmethod
@@ -184,7 +185,7 @@ def main():
     sx, sy = 10.0, 10.0  # [m]
     gx, gy = 50.0, 50.0  # [m]
 
-    robot_radius = 5.0  # [m]
+    expand_distance = 5.0  # [m]
 
     obstacles = [
         ObstaclePolygon(
@@ -209,8 +210,8 @@ def main():
         plt.axis("equal")
         plt.pause(1.0)
 
-    rx, ry = VisibilityRoadMap(robot_radius, do_plot=show_animation).planning(
-        sx, sy, gx, gy, obstacles)
+    rx, ry = VisibilityRoadMap(expand_distance, do_plot=show_animation)\
+        .planning(sx, sy, gx, gy, obstacles)
 
     if show_animation:  # pragma: no cover
         plt.plot(rx, ry, "-r")

docs/modules/path_planning/grid_base_search/grid_base_search.rst

@@ -19,6 +19,8 @@ This is a 2D grid based path planning with Depth first search algorithm.
 
 In the animation, cyan points are searched nodes.
 
+.. _dijkstra:
+
 Dijkstra algorithm
 ~~~~~~~~~~~~~~~~~~
 

docs/modules/path_planning/path_planning_main.rst

@@ -9,6 +9,7 @@ Path Planning
 .. include:: model_predictive_trajectory_generator/model_predictive_trajectory_generator.rst
 .. include:: state_lattice_planner/state_lattice_planner.rst
 .. include:: prm_planner/prm_planner.rst
+.. include:: visibility_road_map_planner/visibility_road_map_planner.rst
 .. include:: vrm_planner/vrm_planner.rst
 .. include:: rrt/rrt.rst
 .. include:: cubic_spline/cubic_spline.rst

docs/modules/path_planning/visibility_road_map_planner/step0.png

@@ -0,0 +1,71 @@
+Visibility Road-Map planner
+---------------------------
+
+.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/VisibilityRoadMap/animation.gif
+
+`[Code] <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/VisibilityRoadMap/visibility_road_map.py>`_
+
+This visibility road-map planner uses Dijkstra method for graph search.
+
+In the animation, the black lines are polygon obstacles,
+
+red crosses are visibility nodes, and blue lines area collision free visibility graphs.
+
+The red line is the final path searched by dijkstra algorithm frm the visibility graphs.
+
+Algorithms
+~~~~~~~~~~
+
+In this chapter, how does the visibility road map planner search a path.
+
+We assume this planner can be provided these information in the below figure.
+
+- 1. Start point (Red point)
+- 2. Goal point (Blue point)
+- 3. Obstacle polygons (Black lines)
+
+.. image:: visibility_road_map_planner/step0.png
+   :width: 400px
+
+
+Step1: Generate visibility nodes based on polygon obstacles
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The nodes are generated by expanded these polygons vertexes like the below figure:
+
+.. image:: visibility_road_map_planner/step1.png
+   :width: 400px
+
+Each polygon vertex is expanded outward from the vector of adjacent vertices.
+
+The start and goal point are included as nodes as well.
+
+Step2: Generate visibility graphs connecting the nodes.
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+When connecting the nodes, the arc between two nodes is checked to collided or not to each obstacles.
+
+If the arc is collided, the graph is removed.
+
+The blue lines are generated visibility graphs in the figure:
+
+.. image:: visibility_road_map_planner/step2.png
+   :width: 400px
+
+
+Step3: Search the shortest path in the graphs using Dijkstra algorithm
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The red line is searched path in the figure:
+
+.. image:: visibility_road_map_planner/step3.png
+   :width: 400px
+
+You can find the details of Dijkstra algorithm in :ref:`dijkstra`.
+
+References
+^^^^^^^^^^
+
+- `Visibility graph - Wikipedia <https://en.wikipedia.org/wiki/Visibility_graph>`_
+
+