start implementation of quinitc polynomials planner

PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py

@@ -0,0 +1,57 @@
+"""
+
+Quinitc Polynomials Planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+import math
+
+
+def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga):
+
+    return None
+
+
+def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
+    u"""
+    Plot arrow
+    """
+
+    if not isinstance(x, float):
+        for (ix, iy, iyaw) in zip(x, y, yaw):
+            plot_arrow(ix, iy, iyaw)
+    else:
+        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)
+
+
+def main():
+    print(__file__ + " start!!")
+
+    sx = 10.0
+    sy = 10.0
+    syaw = math.radians(10.0)
+    sv = 0.0  # [m/s]
+    sa = 0.0
+    gx = 30.0
+    gy = 20.0
+    gyaw = math.radians(90.0)
+    gv = 0.0  # [m/s]
+    ga = 0.0
+
+    quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga)
+
+    plot_arrow(sx, sy, syaw)
+    plot_arrow(gx, gy, gyaw)
+    plt.grid(True)
+    plt.axis("equal")
+
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()