Wrap yaw error when checking Reeds-Shepp goal pose

PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py

@@ -200,7 +200,9 @@ def search_best_goal_node(self):
         # angle check
         final_goal_indexes = []
         for i in goal_indexes:
-            if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th:
+            yaw_diff = self.node_list[i].yaw - self.end.yaw
+            yaw_error = math.atan2(math.sin(yaw_diff), math.cos(yaw_diff))
+            if abs(yaw_error) <= self.goal_yaw_th:
                 final_goal_indexes.append(i)
 
         print("final_goal_indexes:", len(final_goal_indexes))

tests/test_rrt_star_reeds_shepp.py

@@ -42,5 +42,21 @@ def test_too_big_step_size():
     assert path is None
 
 
+def test_goal_yaw_wraparound():
+    yaw_offset = 0.005
+    planner = m.RRTStarReedsShepp(
+        start=[0.0, 0.0, m.math.pi - yaw_offset],
+        goal=[0.0, 0.0, -m.math.pi + yaw_offset],
+        obstacle_list=[],
+        rand_area=[-1.0, 1.0],
+        max_iter=0,
+    )
+    planner.node_list = [planner.start]
+
+    goal_index = planner.search_best_goal_node()
+
+    assert goal_index == 0
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)