Enhance dubins path docs (#664)

* Engance dubins path docs

* Update dubins_path.rst

* fix doc artifact link in CI

* wip

* wip

* wip

* Update dubins_path.rst

* wip

* wip

* wip

* wip

* wip

.circleci/config.yml

@@ -14,6 +14,7 @@ jobs:
           command: |
             python -m venv venv
             . venv/bin/activate
+            pip install -r requirements/requirements.txt
             pip install -r docs/doc_requirements.txt
             cd docs;make html
       - store_artifacts:

PathPlanning/DubinsPath/dubins_path_planner.py

@@ -5,64 +5,82 @@ Dubins path planner sample code
 author Atsushi Sakai(@Atsushi_twi)
 
 """
-import math
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
-import matplotlib.pyplot as plt
+import math
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+from utils.angle import angle_mod, create_2d_rotation_matrix
 
 show_animation = True
 
 
-def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature,
-                         step_size=0.1):
+def plan_dubins_path(s_x, s_y, s_yaw,
+                     g_x, g_y, g_yaw,
+                     curvature,
+                     step_size=0.1):
     """
-    Dubins path planner
+    Path dubins path
+
+    Parameters
+    ----------
+    s_x : float
+        x position of the start point [m]
+    s_y : float
+        y position of the start point [m]
+    s_yaw : float
+        yaw angle of the start point [rad]
+    g_x : float
+        x position of the goal point [m]
+    g_y : float
+        y position of the end point [m]
+    g_yaw : float
+        yaw angle of the end point [rad]
+    curvature : float
+        curvature for curve [1/m]
+    step_size : float (optional)
+        step size between two path points [m]. Default is 0.1
+
+    Returns
+    -------
+    x_list: array
+        x positions of the path
+    y_list: array
+        y positions of the path
+    yaw_list: array
+        yaw angles of the path
+    modes: array
+        mode list of the path
+    lengths: array
+        length list of the path segments.
 
-    :param s_x: x position of start point [m]
-    :param s_y: y position of start point [m]
-    :param s_yaw: yaw angle of start point [rad]
-    :param g_x: x position of end point [m]
-    :param g_y: y position of end point [m]
-    :param g_yaw: yaw angle of end point [rad]
-    :param curvature: curvature for curve [1/m]
-    :param step_size: (optional) step size between two path points [m]
-    :return:
-        x_list: x positions of a path
-        y_list: y positions of a path
-        yaw_list: yaw angles of a path
-        modes: mode list of a path
-        lengths: length of path segments.
     """
-
-    g_x -= s_x
-    g_y -= s_y
-
-    l_rot = Rot.from_euler('z', s_yaw).as_matrix()[0:2, 0:2]
-    le_xy = np.stack([g_x, g_y]).T @ l_rot
-    le_yaw = g_yaw - s_yaw
+    # calculate local goal x, y, yaw
+    l_rot = create_2d_rotation_matrix(s_yaw)
+    le_xy = np.stack([g_x - s_x, g_y - s_y]).T @ l_rot
+    local_goal_x = le_xy[0]
+    local_goal_y = le_xy[1]
+    local_goal_yaw = g_yaw - s_yaw
 
     lp_x, lp_y, lp_yaw, modes, lengths = dubins_path_planning_from_origin(
-        le_xy[0], le_xy[1], le_yaw, curvature, step_size)
+        local_goal_x, local_goal_y, local_goal_yaw, curvature, step_size)
 
-    rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
+    # Convert a local coordinate path to the global coordinate
+    rot = create_2d_rotation_matrix(-s_yaw)
     converted_xy = np.stack([lp_x, lp_y]).T @ rot
     x_list = converted_xy[:, 0] + s_x
     y_list = converted_xy[:, 1] + s_y
-    yaw_list = [pi_2_pi(i_yaw + s_yaw) for i_yaw in lp_yaw]
+    yaw_list = angle_mod(np.array(lp_yaw) + s_yaw)
 
     return x_list, y_list, yaw_list, modes, lengths
 
 
-def mod2pi(theta):
-    return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
+def _mod2pi(theta):
+    return angle_mod(theta, zero_2_2pi=True)
 
 
-def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
-
-
-def left_straight_left(alpha, beta, d):
+def _LSL(alpha, beta, d):
     sa = math.sin(alpha)
     sb = math.sin(beta)
     ca = math.cos(alpha)
@@ -76,9 +94,9 @@ def left_straight_left(alpha, beta, d):
     if p_squared < 0:
         return None, None, None, mode
     tmp1 = math.atan2((cb - ca), tmp0)
-    t = mod2pi(-alpha + tmp1)
+    t = _mod2pi(-alpha + tmp1)
     p = math.sqrt(p_squared)
-    q = mod2pi(beta - tmp1)
+    q = _mod2pi(beta - tmp1)
 
     return t, p, q, mode
 
@@ -96,9 +114,9 @@ def right_straight_right(alpha, beta, d):
     if p_squared < 0:
         return None, None, None, mode
     tmp1 = math.atan2((ca - cb), tmp0)
-    t = mod2pi(alpha - tmp1)
+    t = angle_mod(alpha - tmp1, zero_2_2pi=True)
     p = math.sqrt(p_squared)
-    q = mod2pi(-beta + tmp1)
+    q = _mod2pi(-beta + tmp1)
 
     return t, p, q, mode
 
@@ -116,8 +134,8 @@ def left_straight_right(alpha, beta, d):
         return None, None, None, mode
     p = math.sqrt(p_squared)
     tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
-    t = mod2pi(-alpha + tmp2)
-    q = mod2pi(-mod2pi(beta) + tmp2)
+    t = _mod2pi(-alpha + tmp2)
+    q = _mod2pi(-_mod2pi(beta) + tmp2)
 
     return t, p, q, mode
 
@@ -135,8 +153,8 @@ def right_straight_left(alpha, beta, d):
         return None, None, None, mode
     p = math.sqrt(p_squared)
     tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
-    t = mod2pi(alpha - tmp2)
-    q = mod2pi(beta - tmp2)
+    t = _mod2pi(alpha - tmp2)
+    q = _mod2pi(beta - tmp2)
 
     return t, p, q, mode
 
@@ -153,13 +171,13 @@ def right_left_right(alpha, beta, d):
     if abs(tmp_rlr) > 1.0:
         return None, None, None, mode
 
-    p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
-    t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
-    q = mod2pi(alpha - beta - t + mod2pi(p))
+    p = _mod2pi(2 * math.pi - math.acos(tmp_rlr))
+    t = _mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + _mod2pi(p / 2.0))
+    q = _mod2pi(alpha - beta - t + _mod2pi(p))
     return t, p, q, mode
 
 
-def left_right_left(alpha, beta, d):
+def _LRL(alpha, beta, d):
     sa = math.sin(alpha)
     sb = math.sin(beta)
     ca = math.cos(alpha)
@@ -170,9 +188,9 @@ def left_right_left(alpha, beta, d):
     tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
     if abs(tmp_lrl) > 1:
         return None, None, None, mode
-    p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
-    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
-    q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
+    p = _mod2pi(2 * math.pi - math.acos(tmp_lrl))
+    t = _mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
+    q = _mod2pi(_mod2pi(beta) - alpha - t + _mod2pi(p))
 
     return t, p, q, mode
 
@@ -184,13 +202,13 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
     D = math.hypot(dx, dy)
     d = D * curvature
 
-    theta = mod2pi(math.atan2(dy, dx))
-    alpha = mod2pi(- theta)
-    beta = mod2pi(end_yaw - theta)
+    theta = _mod2pi(math.atan2(dy, dx))
+    alpha = _mod2pi(- theta)
+    beta = _mod2pi(end_yaw - theta)
 
-    planning_funcs = [left_straight_left, right_straight_right,
+    planning_funcs = [_LSL, right_straight_right,
                       left_straight_right, right_straight_left,
-                      right_left_right, left_right_left]
+                      right_left_right, _LRL]
 
     best_cost = float("inf")
     bt, bp, bq, best_mode = None, None, None, None
@@ -319,6 +337,7 @@ def generate_local_course(total_length, lengths, modes, max_curvature,
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r",
                ec="k"):  # pragma: no cover
+    import matplotlib.pyplot as plt
     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)
@@ -330,6 +349,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r",
 
 def main():
     print("Dubins path planner sample start!!")
+    import matplotlib.pyplot as plt
 
     start_x = 1.0  # [m]
     start_y = 1.0  # [m]
@@ -341,13 +361,13 @@ def main():
 
     curvature = 1.0
 
-    path_x, path_y, path_yaw, mode, lengths = dubins_path_planning(start_x,
-                                                                   start_y,
-                                                                   start_yaw,
-                                                                   end_x,
-                                                                   end_y,
-                                                                   end_yaw,
-                                                                   curvature)
+    path_x, path_y, path_yaw, mode, lengths = plan_dubins_path(start_x,
+                                                               start_y,
+                                                               start_yaw,
+                                                               end_x,
+                                                               end_y,
+                                                               end_yaw,
+                                                               curvature)
 
     if show_animation:
         plt.plot(path_x, path_y, label="final course " + "".join(mode))

PathPlanning/HybridAStar/car.py

@@ -6,7 +6,7 @@ author: Zheng Zh (@Zhengzh)
 
 """
 
-from math import sqrt, cos, sin, tan, pi
+from math import cos, sin, tan, pi
 
 import matplotlib.pyplot as plt
 import numpy as np

PathPlanning/RRTDubins/rrt_dubins.py

@@ -20,7 +20,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
 
 try:
     from rrt import RRT
-    import dubins_path_planning
+    import dubins_path_planner
 except ImportError:
     raise
 
@@ -130,15 +130,15 @@ class RRTDubins(RRT):
         plt.pause(0.01)
 
     def plot_start_goal_arrow(self):  # pragma: no cover
-        dubins_path_planning.plot_arrow(
+        dubins_path_planner.plot_arrow(
             self.start.x, self.start.y, self.start.yaw)
-        dubins_path_planning.plot_arrow(
+        dubins_path_planner.plot_arrow(
             self.end.x, self.end.y, self.end.yaw)
 
     def steer(self, from_node, to_node):
 
         px, py, pyaw, mode, course_lengths = \
-            dubins_path_planning.dubins_path_planning(
+            dubins_path_planner.plan_dubins_path(
                 from_node.x, from_node.y, from_node.yaw,
                 to_node.x, to_node.y, to_node.yaw, self.curvature)
 
@@ -160,7 +160,7 @@ class RRTDubins(RRT):
 
     def calc_new_cost(self, from_node, to_node):
 
-        _, _, _, _, course_length = dubins_path_planning.dubins_path_planning(
+        _, _, _, _, course_length = dubins_path_planner.plan_dubins_path(
             from_node.x, from_node.y, from_node.yaw,
             to_node.x, to_node.y, to_node.yaw, self.curvature)
 

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -20,7 +20,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../RRTStar/")
 
 try:
-    import dubins_path_planning
+    import dubins_path_planner
     from rrt_star import RRTStar
 except ImportError:
     raise
@@ -136,15 +136,15 @@ class RRTStarDubins(RRTStar):
         plt.pause(0.01)
 
     def plot_start_goal_arrow(self):
-        dubins_path_planning.plot_arrow(
+        dubins_path_planner.plot_arrow(
             self.start.x, self.start.y, self.start.yaw)
-        dubins_path_planning.plot_arrow(
+        dubins_path_planner.plot_arrow(
             self.end.x, self.end.y, self.end.yaw)
 
     def steer(self, from_node, to_node):
 
         px, py, pyaw, mode, course_lengths = \
-            dubins_path_planning.dubins_path_planning(
+            dubins_path_planner.plan_dubins_path(
                 from_node.x, from_node.y, from_node.yaw,
                 to_node.x, to_node.y, to_node.yaw, self.curvature)
 
@@ -166,7 +166,7 @@ class RRTStarDubins(RRTStar):
 
     def calc_new_cost(self, from_node, to_node):
 
-        _, _, _, _, course_lengths = dubins_path_planning.dubins_path_planning(
+        _, _, _, _, course_lengths = dubins_path_planner.plan_dubins_path(
             from_node.x, from_node.y, from_node.yaw,
             to_node.x, to_node.y, to_node.yaw, self.curvature)
 

docs/conf.py

@@ -13,8 +13,8 @@
 # documentation root, use os.path.abspath to make it absolute, like shown here.
 #
 import os
-# import sys
-# sys.path.insert(0, os.path.abspath('.'))
+import sys
+sys.path.insert(0, os.path.abspath('../'))
 
 
 # -- Project information -----------------------------------------------------
@@ -42,6 +42,7 @@ extensions = [
     'sphinx.ext.autodoc',
     'sphinx.ext.mathjax',
     'sphinx.ext.viewcode',
+    'sphinx.ext.napoleon',
     'IPython.sphinxext.ipython_console_highlighting',
 ]
 

docs/doc_requirements.txt

@@ -1,3 +1,4 @@
 sphinx == 4.3.2 # For sphinx documentation
 sphinx_rtd_theme == 1.0.0
 IPython == 7.31.1 # For sphinx documentation
+sphinxcontrib-napoleon == 0.7 # For auto doc

docs/modules/path_planning/dubins_path/dubins_path.rst

@@ -7,12 +7,37 @@ A sample code for Dubins path planning.
 
 Dubins path
 ~~~~~~~~~~~~
-Dubims path is a analyrical path planning algorithm for a simple car model.
+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.
 
+The path 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. 
+
+For example, one of RSR Dubins paths is:
+
+.. image:: dubins_path/RSR.jpg
+   :width: 400px
+
+one of RLR Dubins paths is:
+
+.. image:: dubins_path/RLR.jpg
+   :width: 200px
+
+Dubins path planner can output three types and distances of each course segment.
+
+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.
+
+Code
+~~~~~~~~~~~~~~~~~~~~
+
+.. automodule:: PathPlanning.DubinsPath.dubins_path_planner
+    :members:
 
 
 Reference
@@ -20,3 +45,4 @@ Reference
 
 -  `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/>`__

tests/test_diff_codestyle.py

@@ -92,7 +92,9 @@ def find_diff(sha):
 def run_flake8(diff):
     """Run flake8 on the given diff."""
     res = subprocess.run(
-        ['flake8', '--diff'],
+        ['flake8', '--diff', '--ignore',
+         'E402'  # top level import for sys.path.append
+         ],
         input=diff,
         stdout=subprocess.PIPE,
         encoding='utf-8',

tests/test_dubins_path_planning.py

@@ -1,7 +1,7 @@
 import numpy as np
 
 import conftest
-from PathPlanning.DubinsPath import dubins_path_planning
+from PathPlanning.DubinsPath import dubins_path_planner
 
 np.random.seed(12345)
 
@@ -33,7 +33,7 @@ def test_1():
 
     curvature = 1.0
 
-    px, py, pyaw, mode, lengths = dubins_path_planning.dubins_path_planning(
+    px, py, pyaw, mode, lengths = dubins_path_planner.plan_dubins_path(
         start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
 
     check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
@@ -42,8 +42,8 @@ def test_1():
 
 
 def test_2():
-    dubins_path_planning.show_animation = False
-    dubins_path_planning.main()
+    dubins_path_planner.show_animation = False
+    dubins_path_planner.main()
 
 
 def test_3():
@@ -61,8 +61,8 @@ def test_3():
         curvature = 1.0 / (np.random.rand() * 5.0)
 
         px, py, pyaw, mode, lengths = \
-            dubins_path_planning.dubins_path_planning(
-            start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
+            dubins_path_planner.plan_dubins_path(
+                start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
 
         check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
                              end_y, end_yaw)

tests/test_utils.py

@@ -0,0 +1,21 @@
+import conftest  # Add root path to sys.path
+from utils import angle
+from numpy.testing import assert_allclose
+import numpy as np
+
+
+def test_angle_mod():
+    assert_allclose(angle.angle_mod(-4.0), 2.28318531)
+    assert(isinstance(angle.angle_mod(-4.0), float))
+    assert_allclose(angle.angle_mod([-4.0]), [2.28318531])
+    assert(isinstance(angle.angle_mod([-4.0]), np.ndarray))
+
+    assert_allclose(angle.angle_mod([-150.0, 190.0, 350], degree=True),
+                    [-150., -170., -10.])
+
+    assert_allclose(angle.angle_mod(-60.0, zero_2_2pi=True, degree=True),
+                    [300.])
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)

utils/angle.py

@@ -0,0 +1,77 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as Rot
+
+
+def create_2d_rotation_matrix(angle):
+    """
+    Create 2D totation matrix from an angle
+
+    Parameters
+    ----------
+    angle :
+
+    Returns
+    -------
+
+    """
+    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
+
+
+def angle_mod(x, zero_2_2pi=False, degree=False):
+    """
+    Angle modulo operation
+    Default angle modulo range is [-pi, pi)
+
+    Parameters
+    ----------
+    x : float or array_like
+        A angle or an array of angles. This array is flattened for
+        the calculation. When an angle is provided, a float angle is returned.
+    zero_2_2pi : bool, optional
+        Change angle modulo range to [0, 2pi)
+        Default is False.
+    degree : bool, optional
+        If True, then the given angles are assumed to be in degrees.
+        Default is False.
+
+    Returns
+    -------
+    ret : float or ndarray
+        an angle or an array of modulated angle.
+
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+    2.28318531
+
+    >>> angle_mod([-4.0])
+    np.array(2.28318531)
+
+    >>> angle_mod([-150.0, 190.0, 350], degree=True)
+    array([-150., -170.,  -10.])
+
+    >>> angle_mod(-60.0, zero_2_2pi=True, degree=True)
+    array([300.])
+
+    """
+    if isinstance(x, float):
+        is_float = True
+    else:
+        is_float = False
+
+    x = np.asarray(x).flatten()
+    if degree:
+        x = np.deg2rad(x)
+
+    if zero_2_2pi:
+        mod_angle = x % (2 * np.pi)
+    else:
+        mod_angle = (x + np.pi) % (2 * np.pi) - np.pi
+
+    if degree:
+        mod_angle = np.rad2deg(mod_angle)
+
+    if is_float:
+        return mod_angle.item()
+    else:
+        return mod_angle