fix: Hybrid A* behavior when path is not found

[parmaski]

Reference issue

None

What does this implement/fix?

issue1: car.py

The vehicle goes out of the search grid when its angle is approximately pi, because the yaw angle is not clipped correctly.

Example: set the start position reverse.

@@ -404,7 +404,7 @@ def main():
     # Set Initial parameters
-    start = [10.0, 10.0, np.deg2rad(90.0)]
+    start = [10.0, 10.0, np.deg2rad(-90.0)]
     goal = [50.0, 50.0, np.deg2rad(-90.0)]

Result:

Error(calc_index): -2959
Error(calc_index): -2030
Error(calc_index): -2959
Error(calc_index): -3019
...

issue2: hybrid_a_star.py

hybrid_a_star_planning returns three empty tuples when no route found, despite returns Path class when ok.
This mismatch causes the crash of subsequent plot function.

Example: Surround the vehicle with walls

@@ -382,16 +382,16 @@ def main():
 
     ox, oy = [], []
 
-    for i in range(60):
+    for i in range(15):
         ox.append(i)
         oy.append(0.0)
-    for i in range(60):
-        ox.append(60.0)
+    for i in range(15):
+        ox.append(15.0)
         oy.append(i)
-    for i in range(61):
+    for i in range(16):
         ox.append(i)
-        oy.append(60.0)
-    for i in range(61):
+        oy.append(15.0)
+    for i in range(16):
         ox.append(0.0)
         oy.append(i)

Result:

Error: Cannot find path, No open set
Traceback (most recent call last):
  File "/workspaces/PythonRobotics/PathPlanning/HybridAStar/hybrid_a_star.py", line 440, in <module>
    main()
  File "/workspaces/PythonRobotics/PathPlanning/HybridAStar/hybrid_a_star.py", line 422, in main
    x = path.x_list
        ^^^^^^^^^^^
AttributeError: 'tuple' object has no attribute 'x_list'

after fix

The program finishes as expected even after applying the two patches above.

Start Hybrid A* planning
start :  [10.0, 10.0, np.float64(-1.5707963267948966)]
goal :  [50.0, 50.0, np.float64(-1.5707963267948966)]
Error: Cannot find path, No open set
/workspaces/PythonRobotics/PathPlanning/HybridAStar/hybrid_a_star.py done!!

Additional information

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example?
• All CIs are green? (You can check it after submitting)

[AtsushiSakai]

LGTM. Thanks!!