add reverse distance and curvature to optimization

Signed-off-by: Takumi Ito <[email protected]>

planning/planning_tuning_tools/scripts/astar_tuning_tools/common/common_classes.py

@@ -7,10 +7,11 @@ def __init__(self, costmap, xs, ys, yaws):
 
 
 class Result:
-    def __init__(self, x, y, yaw, find, waypoints, distance_to_obstacles):
+    def __init__(self, x, y, yaw, find, waypoints, distance_to_obstacles, steerings):
         self.xs = x
         self.ys = y
         self.yaws = yaw
         self.find = find
         self.waypoints = waypoints
         self.distance_to_obstacles = distance_to_obstacles
+        self.steerings = steerings

planning/planning_tuning_tools/scripts/astar_tuning_tools/config/optimization_config.yaml

@@ -14,6 +14,15 @@ evaluation:
   minus_distance_to_obstacle_minimum:
     use_optimization: false
     weight: 0.01
+  steering_average:
+    use_optimization: false
+    weight: 0.01
+  steering_minimum:
+    use_optimization: false
+    weight: 0.01
+  reverse_distance:
+    use_optimization: false
+    weight: 0.01
   calculation_time:
     use_optimization: true
     weight: 0.001
@@ -102,20 +111,20 @@ astar_param:
     min: 0
     max: 1
   distance_heuristic_weight:
-    use_optimization: false
+    use_optimization: true
     initial: 1.5
     min: 0
-    max: 1
+    max: 5
   smoothness_weight:
-    use_optimization: false
+    use_optimization: true
     initial: 0.5
     min: 0
-    max: 1
+    max: 5
   obstacle_distance_weight:
-    use_optimization: false
+    use_optimization: true
     initial: 1.5
     min: 0
-    max: 1
+    max: 5
 
 vehicle_shape:
   length:

planning/planning_tuning_tools/scripts/astar_tuning_tools/evaluator.py

@@ -34,6 +34,9 @@ def __init__(self, results=None):
         self.direction_change = None
         self.minus_distance_to_obstacle_average = None
         self.minus_distance_to_obstacle_minimum = None
+        self.steering_average = None
+        self.steering_minimum = None
+        self.reverse_distance = None
         self.calculation_time = None
         if results is not None:
             self.set_results(results)
@@ -70,6 +73,9 @@ def set_results(self, results):
         total_direction_change = 0
         total_distance_to_obstacle_average = 0
         total_distance_to_obstacle_minimum = 0
+        total_steering_average = 0
+        total_steering_minimum = 0
+        total_reverse_distance = 0
 
         for result in results:
             if result.find:
@@ -86,6 +92,9 @@ def set_results(self, results):
                 total_direction_change += self.count_forward_backward_change(waypoints)
                 total_distance_to_obstacle_average += np.mean(result.distance_to_obstacles)
                 total_distance_to_obstacle_minimum += min(result.distance_to_obstacles)
+                total_steering_average += np.mean(result.steerings)
+                total_steering_minimum += min(result.steerings)
+                total_reverse_distance += self.calculate_reverse_distance(waypoints)
 
         self.N = N
         self.N_success = N_success
@@ -99,6 +108,9 @@ def set_results(self, results):
             self.minus_distance_to_obstacle_minimum = (
                 -total_distance_to_obstacle_minimum / N_success
             )
+            self.steering_average = total_steering_average / N_success
+            self.steering_minimum = total_steering_minimum / N_success
+            self.reverse_distance = total_reverse_distance / N_success
 
     def set_results_from_path(self, dir_path):
         # laod search settings
@@ -133,6 +145,17 @@ def count_forward_backward_change(self, waypoints):
                 pre_is_back = is_back
 
         return count
+
+    def calculate_reverse_distance(self, waypoints):
+        reverse_distance = 0
+        pre_position = waypoints.waypoints[0].pose.position
+        for waypoint in waypoints.waypoints:
+            position = waypoint.pose.position
+            if waypoint.is_back:
+                reverse_distance += math.hypot(position.x-pre_position.x, position.y-pre_position.y)
+            pre_position = position
+
+        return reverse_distance
 
     def print_result(self):
         if self.N is None:

planning/planning_tuning_tools/scripts/astar_tuning_tools/search.py

@@ -32,6 +32,16 @@
 from tqdm import tqdm
 import yaml
 
+def pose2yaw(pose):
+    qw = pose.orientation.w
+    qx = pose.orientation.x
+    qy = pose.orientation.y
+    qz = pose.orientation.z
+    t0 = +2.0 * (qw * qz + qx * qy)
+    t1 = +1.0 - 2.0 * (qy * qy + qz * qz)
+    yaw = math.atan2(t0, t1)
+    return yaw
+
 def search_one(astar, start_pose, goal_pose):
     try:
         find = astar.makePlan(start_pose, goal_pose)
@@ -40,22 +50,21 @@ def search_one(astar, start_pose, goal_pose):
 
     waypoints = fp.PlannerWaypoints()
     distance_to_obstacles = []
+    steerings = []
     if find:
         waypoints = astar.getWaypoints()
+        pre_yaw = pose2yaw(waypoints.waypoints[0].pose)
         for waypoint in waypoints.waypoints:
             distance_to_obstacles.append(astar.getDistanceToObstacle(waypoint.pose))
+            yaw = pose2yaw(waypoint.pose)
+            steerings.append(yaw-pre_yaw)
+            pre_yaw = yaw
 
     x = goal_pose.position.x
     y = goal_pose.position.y
-    qw = goal_pose.orientation.w
-    qx = goal_pose.orientation.x
-    qy = goal_pose.orientation.y
-    qz = goal_pose.orientation.z
-    t0 = +2.0 * (qw * qz + qx * qy)
-    t1 = +1.0 - 2.0 * (qy * qy + qz * qz)
-    yaw = math.atan2(t0, t1)
+    yaw = pose2yaw(goal_pose)
 
-    return Result(x, y, yaw, find, waypoints, distance_to_obstacles)
+    return Result(x, y, yaw, find, waypoints, distance_to_obstacles, steerings)
 
 
 def float_range(start, end, step):

planning/planning_tuning_tools/scripts/astar_tuning_tools/simulated_annealing.py

@@ -239,8 +239,8 @@ def save_param_as_yaml(params, save_name):
 
         val_data_set.append(TestData(costmap, goal_pose))
 
-    initial_temperature = 200.0
-    iterations = 100
+    initial_temperature = 500.0
+    iterations = 1000
 
     simulated_annealing = SimulatedAnnealing(config_path, val_data_set)
     simulated_annealing.simulated_annealing(initial_temperature, iterations)