Fix Reeds-Shepp planner (#358)

* Fix Reeds-Shepp planner

* Fix import

* Fix indentation

---------

Co-authored-by: Peter Corke <peter.i.corke@gmail.com>

Author: hcarlsso

roboticstoolbox/mobile/PlannerBase.py

@@ -23,6 +23,8 @@
 from roboticstoolbox.mobile.Animations import VehiclePolygon
 from colored import fg, attr
 
+from spatialmath.base.graphics import axes_logic
+
 try:
     from progress.bar import FillingCirclesBar
 

roboticstoolbox/mobile/ReedsSheppPlanner.py

@@ -539,21 +539,5 @@ def query(self, start, goal, **kwargs):
     path, status = reedsshepp.query(start, goal)
     print(status)
 
-    # px, py, pyaw, mode, clen = reeds_shepp_path_planning(
-    #     start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
-
-    # if show_animation:  # pragma: no cover
-    #     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.show(block=True)
-
     reedsshepp.plot(path=path, direction=status.direction, configspace=True)
     plt.show(block=True)

tests/test_mobile.py

@@ -16,11 +16,30 @@
 from roboticstoolbox.mobile.drivers import *
 from roboticstoolbox.mobile.sensors import *
 from roboticstoolbox.mobile.Vehicle import *
+from roboticstoolbox.mobile.ReedsSheppPlanner import ReedsSheppPlanner
 
 # from roboticstoolbox.mobile import Planner
 
 # ======================================================================== #
 
+class TestReedsSheppPlanner(unittest.TestCase):
+    def test_turn_around(self):
+        start = (0, 0, 0)
+        goal = (0, 0, pi)
+
+        reedsshepp = ReedsSheppPlanner(curvature=1.0, stepsize=0.1)
+        path, status = reedsshepp.query(start, goal)
+
+        # Turns
+        self.assertEqual(status[0], ['L', 'R', 'L'])
+        # Total length
+        nt.assert_almost_equal(status[1], pi)
+        # Segment lengths
+        nt.assert_array_almost_equal(status[2],[pi/3, -pi/3, pi/3])
+
+
+# ======================================================================== #
+
 
 class TestNavigation(unittest.TestCase):
     def test_edgelist(self):