fix dubins path length bug and clean up codes. (AtsushiSakai#527)

AtsushiSakai · web-flow · commit 6f06b535b9d6 · 2021-07-10T07:15:10.000+09:00
* fix dubins path length bug and clean up codes.

* fix line length CI error

* fix line length CI error

* fix line length CI error

* fix line length CI error

* fix line length CI error

* fix line length CI error

* fix line length CI error

* fix line length CI error
diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py
@@ -14,6 +14,46 @@
 show_animation = True
 
 
+def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature,
+                         step_size=0.1):
+    """
+    Dubins path planner
+
+    :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
+
+    lp_x, lp_y, lp_yaw, modes, lengths = dubins_path_planning_from_origin(
+        le_xy[0], le_xy[1], le_yaw, curvature, step_size)
+
+    rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
+    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]
+
+    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)
 
@@ -148,14 +188,14 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
     alpha = mod2pi(- theta)
     beta = mod2pi(end_yaw - theta)
 
-    planners = [left_straight_left, right_straight_right, left_straight_right,
-                right_straight_left, right_left_right,
-                left_right_left]
+    planning_funcs = [left_straight_left, right_straight_right,
+                      left_straight_right, right_straight_left,
+                      right_left_right, left_right_left]
 
     best_cost = float("inf")
     bt, bp, bq, best_mode = None, None, None, None
 
-    for planner in planners:
+    for planner in planning_funcs:
         t, p, q, mode = planner(alpha, beta, d)
         if t is None:
             continue
@@ -166,10 +206,15 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
             best_cost = cost
     lengths = [bt, bp, bq]
 
-    x_list, y_list, yaw_list, directions = generate_local_course(
-        sum(lengths), lengths, best_mode, curvature, step_size)
+    x_list, y_list, yaw_list, directions = generate_local_course(sum(lengths),
+                                                                 lengths,
+                                                                 best_mode,
+                                                                 curvature,
+                                                                 step_size)
+
+    lengths = [length / curvature for length in lengths]
 
-    return x_list, y_list, yaw_list, best_mode, best_cost
+    return x_list, y_list, yaw_list, best_mode, lengths
 
 
 def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
@@ -203,49 +248,15 @@ def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
     return path_x, path_y, path_yaw, directions
 
 
-def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1):
-    """
-    Dubins path planner
-
-    input:
-        s_x x position of start point [m]
-        s_y y position of start point [m]
-        s_yaw yaw angle of start point [rad]
-        g_x x position of end point [m]
-        g_y y position of end point [m]
-        g_yaw yaw angle of end point [rad]
-        c curvature [1/m]
-
-    """
-
-    g_x = g_x - s_x
-    g_y = 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
-
-    lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin(
-        le_xy[0], le_xy[1], le_yaw, c, step_size)
-
-    rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
-    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]
-
-    return x_list, y_list, yaw_list, mode, lengths
-
-
-def generate_local_course(total_length, lengths, mode, max_curvature,
+def generate_local_course(total_length, lengths, modes, max_curvature,
                           step_size):
     n_point = math.trunc(total_length / step_size) + len(lengths) + 4
 
-    path_x = [0.0 for _ in range(n_point)]
-    path_y = [0.0 for _ in range(n_point)]
-    path_yaw = [0.0 for _ in range(n_point)]
+    p_x = [0.0 for _ in range(n_point)]
+    p_y = [0.0 for _ in range(n_point)]
+    p_yaw = [0.0 for _ in range(n_point)]
     directions = [0.0 for _ in range(n_point)]
-    index = 1
+    ind = 1
 
     if lengths[0] > 0.0:
         directions[0] = 1
@@ -254,7 +265,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
 
     ll = 0.0
 
-    for (m, length, i) in zip(mode, lengths, range(len(mode))):
+    for (m, length, i) in zip(modes, lengths, range(len(modes))):
         if length == 0.0:
             continue
         elif length > 0.0:
@@ -263,54 +274,57 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
             dist = -step_size
 
         # set origin state
-        origin_x, origin_y, origin_yaw = \
-            path_x[index], path_y[index], path_yaw[index]
+        origin_x, origin_y, origin_yaw = p_x[ind], p_y[ind], p_yaw[ind]
 
-        index -= 1
+        ind -= 1
         if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
             pd = - dist - ll
         else:
             pd = dist - ll
 
         while abs(pd) <= abs(length):
-            index += 1
-            path_x, path_y, path_yaw, directions = interpolate(
-                index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
-                path_x, path_y, path_yaw, directions)
+            ind += 1
+            p_x, p_y, p_yaw, directions = interpolate(ind, pd, m,
+                                                      max_curvature,
+                                                      origin_x,
+                                                      origin_y,
+                                                      origin_yaw,
+                                                      p_x, p_y,
+                                                      p_yaw,
+                                                      directions)
             pd += dist
 
         ll = length - pd - dist  # calc remain length
 
-        index += 1
-        path_x, path_y, path_yaw, directions = interpolate(
-            index, length, m, max_curvature, origin_x, origin_y, origin_yaw,
-            path_x, path_y, path_yaw, directions)
+        ind += 1
+        p_x, p_y, p_yaw, directions = interpolate(ind, length, m,
+                                                  max_curvature,
+                                                  origin_x, origin_y,
+                                                  origin_yaw,
+                                                  p_x, p_y, p_yaw,
+                                                  directions)
 
-    if len(path_x) <= 1:
+    if len(p_x) <= 1:
         return [], [], [], []
 
     # remove unused data
-    while len(path_x) >= 1 and path_x[-1] == 0.0:
-        path_x.pop()
-        path_y.pop()
-        path_yaw.pop()
+    while len(p_x) >= 1 and p_x[-1] == 0.0:
+        p_x.pop()
+        p_y.pop()
+        p_yaw.pop()
         directions.pop()
 
-    return path_x, path_y, path_yaw, directions
+    return p_x, p_y, p_yaw, directions
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r",
                ec="k"):  # pragma: no cover
-    """
-    Plot arrow
-    """
-
     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)
     else:
-        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
-                  fc=fc, ec=ec, head_width=width, head_length=width)
+        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc,
+                  ec=ec, head_width=width, head_length=width)
         plt.plot(x, y)
 
 
@@ -327,9 +341,13 @@ def main():
 
     curvature = 1.0
 
-    path_x, path_y, path_yaw, mode, path_length = dubins_path_planning(
-        start_x, start_y, start_yaw,
-        end_x, end_y, end_yaw, curvature)
+    path_x, path_y, path_yaw, mode, lengths = dubins_path_planning(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))
diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py
@@ -133,9 +133,10 @@ def plot_start_goal_arrow(self):  # pragma: no cover
 
     def steer(self, from_node, to_node):
 
-        px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
-            from_node.x, from_node.y, from_node.yaw,
-            to_node.x, to_node.y, to_node.yaw, self.curvature)
+        px, py, pyaw, mode, course_lengths = \
+            dubins_path_planning.dubins_path_planning(
+                from_node.x, from_node.y, from_node.yaw,
+                to_node.x, to_node.y, to_node.yaw, self.curvature)
 
         if len(px) <= 1:  # cannot find a dubins path
             return None
@@ -148,7 +149,7 @@ def steer(self, from_node, to_node):
         new_node.path_x = px
         new_node.path_y = py
         new_node.path_yaw = pyaw
-        new_node.cost += course_length
+        new_node.cost += sum([abs(c) for c in course_lengths])
         new_node.parent = from_node
 
         return new_node
diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py
@@ -139,9 +139,10 @@ def plot_start_goal_arrow(self):
 
     def steer(self, from_node, to_node):
 
-        px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
-            from_node.x, from_node.y, from_node.yaw,
-            to_node.x, to_node.y, to_node.yaw, self.curvature)
+        px, py, pyaw, mode, course_lengths = \
+            dubins_path_planning.dubins_path_planning(
+                from_node.x, from_node.y, from_node.yaw,
+                to_node.x, to_node.y, to_node.yaw, self.curvature)
 
         if len(px) <= 1:  # cannot find a dubins path
             return None
@@ -154,18 +155,20 @@ def steer(self, from_node, to_node):
         new_node.path_x = px
         new_node.path_y = py
         new_node.path_yaw = pyaw
-        new_node.cost += course_length
+        new_node.cost += sum([abs(c) for c in course_lengths])
         new_node.parent = from_node
 
         return new_node
 
     def calc_new_cost(self, from_node, to_node):
 
-        _, _, _, _, course_length = dubins_path_planning.dubins_path_planning(
+        _, _, _, _, course_lengths = dubins_path_planning.dubins_path_planning(
             from_node.x, from_node.y, from_node.yaw,
             to_node.x, to_node.y, to_node.yaw, self.curvature)
 
-        return from_node.cost + course_length
+        cost = sum([abs(c) for c in course_lengths])
+
+        return from_node.cost + cost
 
     def get_random_node(self):
 
diff --git a/tests/test_dubins_path_planning.py b/tests/test_dubins_path_planning.py
@@ -1,12 +1,13 @@
-import conftest
 import numpy as np
+
+import conftest
 from PathPlanning.DubinsPath import dubins_path_planning
 
 np.random.seed(12345)
 
 
-def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
-                         end_x, end_y, end_yaw):
+def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
+                         end_y, end_yaw):
     assert (abs(px[0] - start_x) <= 0.01)
     assert (abs(py[0] - start_y) <= 0.01)
     assert (abs(pyaw[0] - start_yaw) <= 0.01)
@@ -15,6 +16,12 @@ def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
     assert (abs(pyaw[-1] - end_yaw) <= 0.01)
 
 
+def check_path_length(px, py, lengths):
+    path_len = sum(
+        [np.hypot(dx, dy) for (dx, dy) in zip(np.diff(px), np.diff(py))])
+    assert (abs(path_len - sum(lengths)) <= 0.1)
+
+
 def test_1():
     start_x = 1.0  # [m]
     start_y = 1.0  # [m]
@@ -26,12 +33,12 @@ def test_1():
 
     curvature = 1.0
 
-    px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
+    px, py, pyaw, mode, lengths = dubins_path_planning.dubins_path_planning(
         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)
+    check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
+                         end_y, end_yaw)
+    check_path_length(px, py, lengths)
 
 
 def test_2():
@@ -53,12 +60,13 @@ def test_3():
 
         curvature = 1.0 / (np.random.rand() * 5.0)
 
-        px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
+        px, py, pyaw, mode, lengths = \
+            dubins_path_planning.dubins_path_planning(
             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)
+        check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
+                             end_y, end_yaw)
+        check_path_length(px, py, lengths)
 
 
 if __name__ == '__main__':
