fix error for goal cost angle

2026-01-14 20:07:54 -05:00 · 2019-10-06 14:32:22 +08:00
parent b7f6c1c0bc
commit b942c430e6
1 changed files with 2 additions and 1 deletions
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -167,7 +167,8 @@ def calc_to_goal_cost(traj, goal, config):
    dx = goal[0] - traj[-1, 0]
    dy = goal[1] - traj[-1, 1]
    error_angle = math.atan2(dy, dx)
-    cost = abs(error_angle - traj[-1, 2])
+    cost_angle = error_angle - traj[-1, 2]
+    cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))

    return cost