dwa pr (AtsushiSakai#390)

* dwa pr

* dwa_pr

* dwa_pr

* dwa_pr

* dwa_pr

* make changes rerun CI

* rerun CI...again..

* rerun CI.....

* rerun CI.....

* rerun CI final time!

* modified const to class variable

* put back missing comment

* add test for dwa stuck case

* add test dwa stuck case

* add test dwa stuck case

* add test dwa stuck case

* add stuck test in original test file

Author: weicent

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -19,7 +19,6 @@ def dwa_control(x, config, goal, ob):
     """
     Dynamic Window Approach control
     """
-
     dw = calc_dynamic_window(x, config)
 
     u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
@@ -51,6 +50,7 @@ def __init__(self):
         self.to_goal_cost_gain = 0.15
         self.speed_cost_gain = 1.0
         self.obstacle_cost_gain = 1.0
+        self.robot_stuck_flag_cons = 0.001  # constant to prevent robot stucked
         self.robot_type = RobotType.circle
 
         # if robot_type == RobotType.circle
@@ -60,6 +60,23 @@ def __init__(self):
         # if robot_type == RobotType.rectangle
         self.robot_width = 0.5  # [m] for collision check
         self.robot_length = 1.2  # [m] for collision check
+        # obstacles [x(m) y(m), ....]
+        self.ob = np.array([[-1, -1],
+                            [0, 2],
+                            [4.0, 2.0],
+                            [5.0, 4.0],
+                            [5.0, 5.0],
+                            [5.0, 6.0],
+                            [5.0, 9.0],
+                            [8.0, 9.0],
+                            [7.0, 9.0],
+                            [8.0, 10.0],
+                            [9.0, 11.0],
+                            [12.0, 13.0],
+                            [12.0, 12.0],
+                            [15.0, 15.0],
+                            [13.0, 13.0]
+                            ])
 
     @property
     def robot_type(self):
@@ -72,6 +89,9 @@ def robot_type(self, value):
         self._robot_type = value
 
 
+config = Config()
+
+
 def motion(x, u, dt):
     """
     motion model
@@ -139,7 +159,6 @@ def calc_control_and_trajectory(x, dw, config, goal, ob):
         for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
 
             trajectory = predict_trajectory(x_init, v, y, config)
-
             # calc cost
             to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
             speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
@@ -152,13 +171,19 @@ def calc_control_and_trajectory(x, dw, config, goal, ob):
                 min_cost = final_cost
                 best_u = [v, y]
                 best_trajectory = trajectory
-
+                if abs(best_u[0]) < config.robot_stuck_flag_cons \
+                        and abs(x[3]) < config.robot_stuck_flag_cons:
+                    # to ensure the robot do not get stuck in
+                    # best v=0 m/s (in front of an obstacle) and
+                    # best omega=0 rad/s (heading to the goal with
+                    # angle difference of 0)
+                    best_u[1] = -config.max_delta_yaw_rate
     return best_u, best_trajectory
 
 
 def calc_obstacle_cost(trajectory, ob, config):
     """
-        calc obstacle cost inf: collision
+    calc obstacle cost inf: collision
     """
     ox = ob[:, 0]
     oy = ob[:, 1]
@@ -238,29 +263,12 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
     x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
     # goal position [x(m), y(m)]
     goal = np.array([gx, gy])
-    # obstacles [x(m) y(m), ....]
-    ob = np.array([[-1, -1],
-                   [0, 2],
-                   [4.0, 2.0],
-                   [5.0, 4.0],
-                   [5.0, 5.0],
-                   [5.0, 6.0],
-                   [5.0, 9.0],
-                   [8.0, 9.0],
-                   [7.0, 9.0],
-                   [8.0, 10.0],
-                   [9.0, 11.0],
-                   [12.0, 13.0],
-                   [12.0, 12.0],
-                   [15.0, 15.0],
-                   [13.0, 13.0]
-                   ])
 
     # input [forward speed, yaw_rate]
-    config = Config()
+
     config.robot_type = robot_type
     trajectory = np.array(x)
-
+    ob = config.ob
     while True:
         u, predicted_trajectory = dwa_control(x, config, goal, ob)
         x = motion(x, u, config.dt)  # simulate robot

tests/test_dynamic_window_approach.py

@@ -1,5 +1,6 @@
 import os
 import sys
+import numpy as np
 from unittest import TestCase
 
 sys.path.append(os.path.dirname(__file__) + "/../")
@@ -20,8 +21,38 @@ def test_main2(self):
         m.show_animation = False
         m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle)
 
+    def test_stuck_main(self):
+        m.show_animation = False
+        # adjust cost
+        m.config.to_goal_cost_gain = 0.2
+        m.config.obstacle_cost_gain = 2.0
+        # obstacles and goals for stuck condition
+        m.config.ob = -1 * np.array([[-1.0, -1.0],
+                                     [0.0, 2.0],
+                                     [2.0, 6.0],
+                                     [2.0, 8.0],
+                                     [3.0, 9.27],
+                                     [3.79, 9.39],
+                                     [7.25, 8.97],
+                                     [7.0, 2.0],
+                                     [3.0, 4.0],
+                                     [6.0, 5.0],
+                                     [3.5, 5.8],
+                                     [6.0, 9.0],
+                                     [8.8, 9.0],
+                                     [5.0, 9.0],
+                                     [7.5, 3.0],
+                                     [9.0, 8.0],
+                                     [5.8, 4.4],
+                                     [12.0, 12.0],
+                                     [3.0, 2.0],
+                                     [13.0, 13.0]
+                                     ])
+        m.main(gx=-5.0, gy=-7.0)
+
 
 if __name__ == '__main__':  # pragma: no cover
     test = TestDynamicWindowApproach()
     test.test_main1()
     test.test_main2()
+    test.test_stuck_main()