From b942c430e6df8d854fe00f9a264efaca94cc90c9 Mon Sep 17 00:00:00 2001
From: sszwfy <476020374@qq.com>
Date: Sun, 6 Oct 2019 14:32:22 +0800
Subject: [PATCH] fix error for  goal cost angle

---
 PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index fa1dcc62c4..a7d74332a8 100644
--- 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