Enhance dubins path docs (AtsushiSakai#679)

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

* Enhance dubins path docs

Author: AtsushiSakai

PathPlanning/DubinsPath/dubins_path_planner.py

@@ -39,6 +39,7 @@
 # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
 # ones.
 extensions = [
+    'matplotlib.sphinxext.plot_directive',
     'sphinx.ext.autodoc',
     'sphinx.ext.mathjax',
     'sphinx.ext.viewcode',

docs/conf.py

@@ -9,40 +9,68 @@ Dubins path
 ~~~~~~~~~~~~
 Dubins path is a analytical path planning algorithm for a simple car model.
 
-It can generates a shortest path between 2D poses (x, y, yaw) with maximum curvture comstraint and tangent(yaw angle) constraint.
+It can generates a shortest path between two 2D poses (x, y, yaw) with maximum curvature constraint and tangent(yaw angle) constraint.
 
-The path consist of 3 segments of maximum curvature curves or a straight line segment.
+Generated paths consist of 3 segments of maximum curvature curves or a straight line segment.
 
 Each segment type can is categorized by 3 type: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).' 
 
-Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL. 
+Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL.
 
-For example, one of RSR Dubins paths is:
+Dubins path planner can output each segment type and distance of each course segment.
+
+For example, a RSR Dubins path is:
 
 .. image:: RSR.jpg
    :width: 400px
 
-one of RLR Dubins paths is:
+Each segment distance can be calculated by:
+
+:math:`\alpha = mod(-\theta)`
+
+:math:`\beta = mod(x_{e, yaw} - \theta)`
+
+:math:`p^2 = 2 + d ^ 2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)`
+
+:math:`t = atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta)`
+
+:math:`d_1 = mod(-\alpha + t)`
+
+:math:`d_2 = p`
+
+:math:`d_3 = mod(\beta - t)`
+
+where :math:`\theta` is tangent and d is distance from :math:`x_s` to :math:`x_e`
+
+A RLR Dubins path is:
 
 .. image:: RLR.jpg
    :width: 200px
 
-Dubins path planner can output three types and distances of each course segment.
+Each segment distance can be calculated by:
+
+:math:`t = (6.0 - d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)) / 8.0`
+
+:math:`d_2 = mod(2\pi - acos(t))`
+
+:math:`d_1 = mod(\alpha - atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta) + d_2 / 2.0)`
+
+:math:`d_3 = mod(\alpha - \beta - d_1 + d_2)`
 
 You can generate a path from these information and the maximum curvature information.
 
-In the example code, a path which is minimum course length one among 6 course type is selected and then a path is constructed.
+A path type which has minimum course length among 6 types is selected,
+and then a path is constructed based on the selected type and its distances.
 
-Code
+API
 ~~~~~~~~~~~~~~~~~~~~
 
-.. automodule:: PathPlanning.DubinsPath.dubins_path_planner
-    :members:
+.. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path
 
 
 Reference
 ~~~~~~~~~~~~~~~~~~~~
-
+-  `On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents <https://www.jstor.org/stable/2372560?origin=crossref>`__
 -  `Dubins path - Wikipedia <https://en.wikipedia.org/wiki/Dubins_path>`__
 -  `15.3.1 Dubins Curves <http://planning.cs.uiuc.edu/node821.html>`__
 -  `A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths <https://gieseanw.wordpress.com/2012/10/21/a-comprehensive-step-by-step-tutorial-to-computing-dubins-paths/>`__

docs/modules/path_planning/dubins_path/dubins_path.jpg

@@ -69,5 +69,24 @@ def test_3():
         check_path_length(px, py, lengths)
 
 
+def test_path_plannings_types():
+    dubins_path_planner.show_animation = False
+    start_x = 1.0  # [m]
+    start_y = 1.0  # [m]
+    start_yaw = np.deg2rad(45.0)  # [rad]
+
+    end_x = -3.0  # [m]
+    end_y = -3.0  # [m]
+    end_yaw = np.deg2rad(-45.0)  # [rad]
+
+    curvature = 1.0
+
+    _, _, _, mode, _ = dubins_path_planner.plan_dubins_path(
+        start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature,
+        selected_types=["RSL"])
+
+    assert mode == ["R", "S", "L"]
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)

docs/modules/path_planning/dubins_path/dubins_path_main.rst

@@ -4,6 +4,12 @@
 import numpy as np
 
 
+def test_rot_mat_2d():
+    assert_allclose(angle.rot_mat_2d(0.0),
+                    np.array([[1., 0.],
+                              [0., 1.]]))
+
+
 def test_angle_mod():
     assert_allclose(angle.angle_mod(-4.0), 2.28318531)
     assert(isinstance(angle.angle_mod(-4.0), float))

tests/test_dubins_path_planning.py

@@ -2,16 +2,22 @@
 from scipy.spatial.transform import Rotation as Rot
 
 
-def create_2d_rotation_matrix(angle):
+def rot_mat_2d(angle):
     """
-    Create 2D totation matrix from an angle
+    Create 2D rotation matrix from an angle
 
     Parameters
     ----------
     angle :
 
     Returns
     -------
+    A 2D rotation matrix
+
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+
 
     """
     return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]

tests/test_utils.py

@@ -0,0 +1,49 @@
+"""
+Matplotlib based plotting utilities
+"""
+import math
+import matplotlib.pyplot as plt
+
+
+def plot_arrow(x, y, yaw, arrow_length=1.0,
+               origin_point_plot_style="xr",
+               head_width=0.1, fc="r", ec="k", **kwargs):
+    """
+    Plot an arrow or arrows based on 2D state (x, y, yaw)
+
+    All optional settings of matplotlib.pyplot.arrow can be used.
+    - matplotlib.pyplot.arrow:
+    https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.arrow.html
+
+    Parameters
+    ----------
+    x : a float or array_like
+        a value or a list of arrow origin x position.
+    y : a float or array_like
+        a value or a list of arrow origin y position.
+    yaw : a float or array_like
+        a value or a list of arrow yaw angle (orientation).
+    arrow_length : a float (optional)
+        arrow length. default is 1.0
+    origin_point_plot_style : str (optional)
+        origin point plot style. If None, not plotting.
+    head_width : a float (optional)
+        arrow head width. default is 0.1
+    fc : string (optional)
+        face color
+    ec : string (optional)
+        edge color
+    """
+    if not isinstance(x, float):
+        for (i_x, i_y, i_yaw) in zip(x, y, yaw):
+            plot_arrow(i_x, i_y, i_yaw, head_width=head_width,
+                       fc=fc, ec=ec, **kwargs)
+    else:
+        plt.arrow(x, y,
+                  arrow_length * math.cos(yaw),
+                  arrow_length * math.sin(yaw),
+                  head_width=head_width,
+                  fc=fc, ec=ec,
+                  **kwargs)
+        if origin_point_plot_style is not None:
+            plt.plot(x, y, origin_point_plot_style)