diff --git a/PathPlanning/DubinsPath/animation.gif b/PathPlanning/DubinsPath/animation.gif new file mode 100644 index 00000000..5865a4ba Binary files /dev/null and b/PathPlanning/DubinsPath/animation.gif differ diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index ba91360f..52eb1c1a 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -1,15 +1,15 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- """ Dubins path planner sample code author Atsushi Sakai(@Atsushi_twi) -License MIT - """ import math +import numpy as np +import matplotlib.pyplot as plt + +show_animation = True def mod2pi(theta): @@ -147,7 +147,7 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): dx = ex dy = ey D = math.sqrt(dx ** 2.0 + dy ** 2.0) - d = D / c + d = D * c # print(dx, dy, D, d) theta = mod2pi(math.atan2(dy, dx)) @@ -163,7 +163,6 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): for planner in planners: t, p, q, mode = planner(alpha, beta, d) if t is None: - # print("".join(mode) + " cannot generate path") continue cost = (abs(t) + abs(p) + abs(q)) @@ -213,14 +212,6 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): py = [- math.sin(-syaw) * x + math.cos(-syaw) * y + sy for x, y in zip(lpx, lpy)] pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] - # print(syaw) - # pyaw = lpyaw - - # plt.plot(pyaw, "-r") - # plt.plot(lpyaw, "-b") - # plt.plot(eyaw, "*r") - # plt.plot(syaw, "*b") - # plt.show() return px, py, pyaw, mode, clen @@ -234,14 +225,14 @@ def generate_course(length, mode, c): for m, l in zip(mode, length): pd = 0.0 if m is "S": - d = 1.0 / c + d = 1.0 * c else: # turning couse d = math.radians(3.0) while pd < abs(l - d): # print(pd, l) - px.append(px[-1] + d * c * math.cos(pyaw[-1])) - py.append(py[-1] + d * c * math.sin(pyaw[-1])) + px.append(px[-1] + d / c * math.cos(pyaw[-1])) + py.append(py[-1] + d / c * math.sin(pyaw[-1])) if m is "L": # left turn pyaw.append(pyaw[-1] + d) @@ -252,8 +243,8 @@ def generate_course(length, mode, c): pd += d else: d = l - pd - px.append(px[-1] + d * c * math.cos(pyaw[-1])) - py.append(py[-1] + d * c * math.sin(pyaw[-1])) + px.append(px[-1] + d / c * math.cos(pyaw[-1])) + py.append(py[-1] + d / c * math.sin(pyaw[-1])) if m is "L": # left turn pyaw.append(pyaw[-1] + d) @@ -267,10 +258,9 @@ def generate_course(length, mode, c): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): - u""" + """ Plot arrow """ - import matplotlib.pyplot as plt if not isinstance(x, float): for (ix, iy, iyaw) in zip(x, y, yaw): @@ -281,9 +271,8 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): plt.plot(x, y) -if __name__ == '__main__': +def main(): print("Dubins path planner sample start!!") - import matplotlib.pyplot as plt start_x = 1.0 # [m] start_y = 1.0 # [m] @@ -298,16 +287,58 @@ if __name__ == '__main__': px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) - plt.plot(px, py, label="final course " + "".join(mode)) + if show_animation: + plt.plot(px, py, label="final course " + "".join(mode)) - # plotting - plot_arrow(start_x, start_y, start_yaw) - plot_arrow(end_x, end_y, end_yaw) + # plotting + plot_arrow(start_x, start_y, start_yaw) + plot_arrow(end_x, end_y, end_yaw) - # for (ix, iy, iyaw) in zip(px, py, pyaw): - # plot_arrow(ix, iy, iyaw, fc="b") + # for (ix, iy, iyaw) in zip(px, py, pyaw): + # plot_arrow(ix, iy, iyaw, fc="b") - plt.legend() - plt.grid(True) - plt.axis("equal") - plt.show() + plt.legend() + plt.grid(True) + plt.axis("equal") + plt.show() + + +def test(): + + NTEST = 5 + + for i in range(NTEST): + start_x = (np.random.rand() - 0.5) * 10.0 # [m] + start_y = (np.random.rand() - 0.5) * 10.0 # [m] + start_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + + end_x = (np.random.rand() - 0.5) * 10.0 # [m] + end_y = (np.random.rand() - 0.5) * 10.0 # [m] + end_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + + curvature = 1.0 / (np.random.rand() * 5.0) + + px, py, pyaw, mode, clen = dubins_path_planning( + start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) + + if show_animation: + plt.cla() + plt.plot(px, py, label="final course " + str(mode)) + + # plotting + plot_arrow(start_x, start_y, start_yaw) + plot_arrow(end_x, end_y, end_yaw) + + plt.legend() + plt.grid(True) + plt.axis("equal") + plt.xlim(-10, 10) + plt.ylim(-10, 10) + plt.pause(1.0) + + print("Test done") + + +if __name__ == '__main__': + test() + main() diff --git a/PathPlanning/DubinsPath/figures/figure_1.png b/PathPlanning/DubinsPath/figures/figure_1.png deleted file mode 100644 index d9dc5db8..00000000 Binary files a/PathPlanning/DubinsPath/figures/figure_1.png and /dev/null differ diff --git a/PathPlanning/DubinsPath/figures/figure_12.png b/PathPlanning/DubinsPath/figures/figure_12.png deleted file mode 100644 index 060c3c89..00000000 Binary files a/PathPlanning/DubinsPath/figures/figure_12.png and /dev/null differ diff --git a/PathPlanning/DubinsPath/figures/figure_13.png b/PathPlanning/DubinsPath/figures/figure_13.png deleted file mode 100644 index 5c4b0b15..00000000 Binary files a/PathPlanning/DubinsPath/figures/figure_13.png and /dev/null differ diff --git a/PathPlanning/DubinsPath/figures/figure_15.png b/PathPlanning/DubinsPath/figures/figure_15.png deleted file mode 100644 index 2112d559..00000000 Binary files a/PathPlanning/DubinsPath/figures/figure_15.png and /dev/null differ diff --git a/README.md b/README.md index 630c284e..6e98ca8d 100644 --- a/README.md +++ b/README.md @@ -366,10 +366,7 @@ Ref: A sample code for Dubins path planning. - -![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_1.png?raw=True) -![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_13.png?raw=True) -![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/figures/figure_15.png?raw=True) +![dubins](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DubinsPath/animation.gif?raw=True) Ref: