From 48fe1d58a4d8081ff77badfe3657c7c3700e0211 Mon Sep 17 00:00:00 2001
From: Hakan Carlsson <hakan.carlsson@gmail.com>
Date: Sat, 10 Aug 2024 08:51:59 +0200
Subject: [PATCH] Fix Reeds-Shepp planner (#358)

* Fix Reeds-Shepp planner

* Fix import

* Fix indentation

---------

Co-authored-by: Peter Corke <peter.i.corke@gmail.com>
---
 roboticstoolbox/mobile/PlannerBase.py       |  2 ++
 roboticstoolbox/mobile/ReedsSheppPlanner.py | 16 ----------------
 tests/test_mobile.py                        | 19 +++++++++++++++++++
 3 files changed, 21 insertions(+), 16 deletions(-)

diff --git a/roboticstoolbox/mobile/PlannerBase.py b/roboticstoolbox/mobile/PlannerBase.py
index d68487799..e62aae77a 100644
--- a/roboticstoolbox/mobile/PlannerBase.py
+++ b/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
 
diff --git a/roboticstoolbox/mobile/ReedsSheppPlanner.py b/roboticstoolbox/mobile/ReedsSheppPlanner.py
index 51c8aae91..bdd7a3c40 100644
--- a/roboticstoolbox/mobile/ReedsSheppPlanner.py
+++ b/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)
diff --git a/tests/test_mobile.py b/tests/test_mobile.py
index ab71b95d0..cbb988698 100644
--- a/tests/test_mobile.py
+++ b/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):