Merge pull request AtsushiSakai#199 from KoenHan/dwa_only

DWA cost function toward goal

Author: AtsushiSakai

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -31,7 +31,7 @@ def __init__(self):
         self.yawrate_reso = 0.1 * math.pi / 180.0  # [rad/s]
         self.dt = 0.1  # [s]
         self.predict_time = 3.0  # [s]
-        self.to_goal_cost_gain = 1.0
+        self.to_goal_cost_gain = 0.15
         self.speed_cost_gain = 1.0
         self.robot_radius = 1.0  # [m]
 
@@ -137,14 +137,12 @@ def calc_obstacle_cost(traj, ob, config):
 
 
 def calc_to_goal_cost(traj, goal, config):
-    # calc to goal cost. It is 2D norm.
-
-    goal_magnitude = math.sqrt(goal[0]**2 + goal[1]**2)
-    traj_magnitude = math.sqrt(traj[-1, 0]**2 + traj[-1, 1]**2)
-    dot_product = (goal[0] * traj[-1, 0]) + (goal[1] * traj[-1, 1])
-    error = dot_product / (goal_magnitude * traj_magnitude)
-    error_angle = math.acos(error)
-    cost = config.to_goal_cost_gain * error_angle
+    # calc to goal cost.
+
+    dx = goal[0] - traj[-1, 0]
+    dy = goal[1] - traj[-1, 1]
+    error_angle = math.atan2(dy, dx)
+    cost = config.to_goal_cost_gain * abs(error_angle - traj[-1, 2])
 
     return cost