feat(intersection): rectify initial accel/velocity profile in ego velocity profile

planning/behavior_velocity_intersection_module/README.md

@@ -86,6 +86,14 @@ The parameters `collision_detection.collision_start_margin_time` and `collision_
 
 If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions.
 
+Currently, the intersection module uses `motion_velocity_smoother` feature to precisely calculate ego vehicle velocity profile along the intersection lane under longitudinal/lateral constraints. If the flag `collision_detection.use_upstream_velocity` is true, the target velocity profile of the original path is used. Otherwise the target velocity is set to `common.intersection_velocity`. In the trajectory smoothing process the target velocity at/before ego trajectory points are set to ego current velocity. The smoothed trajectory is then converted to an array of (time, distance) which indicates the arrival time to each trajectory point on the path from current ego position. You can visualize this array by adding the lane id to `debug.ttc` and running
+
+```bash
+ros2 run behavior_velocity_intersection_module ttc.py --lane_id <lane_id>
+```
+
+![ego ttc profile](./docs/ttc.gif)
+
 #### Stop Line Automatic Generation
 
 If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front.

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -35,24 +35,24 @@
           distance_thr: 1.0 # [m]
 
       collision_detection:
-        state_transit_margin_time: 1.0
+        state_transit_margin_time: 0.5
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
         fully_prioritized:
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
         partially_prioritized:
-          collision_start_margin_time: 2.0
+          collision_start_margin_time: 3.0
           collision_end_margin_time: 2.0
         not_prioritized:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
+          collision_start_margin_time: 3.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 2.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
         use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
         minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
         yield_on_green_traffic_light:
-          distance_to_assigned_lanelet_start: 5.0
-          duration: 2.0
+          distance_to_assigned_lanelet_start: 10.0
+          duration: 3.0
           object_dist_to_stopline: 10.0 # [m]
         ignore_on_amber_traffic_light:
           object_expected_deceleration: 2.0 # [m/ss]
@@ -81,7 +81,7 @@
           maximum_peeking_distance: 6.0 # [m]
         attention_lane_crop_curvature_threshold: 0.25
         attention_lane_curvature_calculation_ds: 0.5
-        static_occlusion_with_traffic_light_timeout: 3.5
+        static_occlusion_with_traffic_light_timeout: 0.5
 
       debug:
         ttc: [0]

planning/behavior_velocity_intersection_module/scripts/ttc.py

@@ -21,7 +21,7 @@
 from threading import Lock
 import time
 
 import imageio
 import matplotlib
 import matplotlib.pyplot as plt
 import numpy as np
@@ -126,8 +126,11 @@
         self.ttc_ax.set_xlabel("ego time")
         self.ttc_ax.set_ylabel("ego dist")
         time_dist_plot = self.ttc_ax.plot(ego_ttc_time, ego_ttc_dist, label="time-dist", c="orange")
-        self.ttc_ax.set_xlim(min(ego_ttc_time) - 2.0, max(ego_ttc_time) + 3.0)
-        self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0)
+        self.ttc_ax.set_xlim(
+            min(ego_ttc_time) - 2.0,
+            min(max(ego_ttc_time) + 3.0, self.args.max_time),
+        )
+        # self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0)
         for npc, color in zip(self.npc_vehicles, cycle(self.color_list)):
             t0, t1 = npc.collision_start_time, npc.collision_end_time
             d0, d1 = npc.collision_start_dist, npc.collision_end_dist
@@ -137,15 +140,13 @@
                 c=color,
                 alpha=0.2,
             )
-
         dd = [d1 - d0 for d0, d1 in zip(ego_ttc_dist, ego_ttc_dist[1:])]
         dt = [t1 - t0 for t0, t1 in zip(ego_ttc_time, ego_ttc_time[1:])]
         v = [d / t for d, t in zip(dd, dt)]
         self.ttc_vel_ax.yaxis.set_label_position("right")
         self.ttc_vel_ax.set_ylabel("ego velocity")
-        self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0)
+        # self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0)
         time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red")
-
         lines = time_dist_plot + time_velocity_plot
         labels = [line.get_label() for line in lines]
         self.ttc_ax.legend(lines, labels, loc="upper left")
@@ -216,8 +217,9 @@
 
     def cleanup(self):
         if self.args.save:
             kwargs_write = {"fps": self.args.fps, "quantizer": "nq"}
             imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write)
+        rclpy.shutdown()
 
     def on_plot_timer(self):
         with self.lock:
@@ -277,6 +279,7 @@
         default=60,
         help="detect range for drawing",
     )
+    parser.add_argument("--max_time", type=float, default=100, help="max plot limit for time")
     parser.add_argument("-s", "--save", action="store_true", help="flag to save gif")
     parser.add_argument("--gif", type=str, default="ttc", help="filename of gif file")
     parser.add_argument("--fps", type=float, default=5, help="fps of gif")

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -36,6 +36,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <list>
 #include <memory>
 #include <set>
@@ -92,8 +93,8 @@
     return duplicate_idx_opt.value();
   }
 
   const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
     inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
   // vector.insert(i) inserts element on the left side of v[i]
   // the velocity need to be zero order hold(from prior point)
   int insert_idx = closest_idx;
@@ -207,8 +208,8 @@
   stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y());
   stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z());
 
   return motion_utils::findFirstNearestIndexWithSoftConstraints(
     path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
 }
 
@@ -310,8 +311,8 @@
 
   // (2) ego front stop line position on interpolated path
   const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose;
   const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints(
     path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
 
   // (3) occlusion peeking stop line position on interpolated path
@@ -393,7 +394,7 @@
   for (const auto & [stop_idx_ip, stop_idx] : stop_lines) {
     const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose;
     const auto insert_idx = insertPointIndex(
       insert_point, original_path, planner_data->ego_nearest_dist_threshold,
       planner_data->ego_nearest_yaw_threshold);
     if (!insert_idx) {
       return std::nullopt;
@@ -545,9 +546,9 @@
     static_cast<int>(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist,
     0));
   const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose;
   return insertPointIndex(
     insert_point, original_path, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
 }
 
 static std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLanelets(
@@ -1304,14 +1305,27 @@
   const bool use_upstream_velocity, const double minimum_upstream_velocity,
   tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array)
 {
-  double dist_sum = 0.0;
+  const double current_velocity = planner_data->current_velocity->twist.linear.x;
+
   int assigned_lane_found = false;
 
   // crop intersection part of the path, and set the reference velocity to intersection_velocity
   // for ego's ttc
   PathWithLaneId reference_path;
-  for (size_t i = closest_idx; i < path.points.size(); ++i) {
+  std::optional<size_t> upstream_stop_line{std::nullopt};
+  for (size_t i = 0; i < path.points.size() - 1; ++i) {
     auto reference_point = path.points.at(i);
+    // assume backward velocity is current ego velocity
+    if (i < closest_idx) {
+      reference_point.point.longitudinal_velocity_mps = current_velocity;
+    }
+    if (
+      i > last_intersection_stop_line_candidate_idx &&
+      std::fabs(reference_point.point.longitudinal_velocity_mps) <
+        std::numeric_limits<double>::epsilon() &&
+      !upstream_stop_line) {
+      upstream_stop_line = i;
+    }
     if (!use_upstream_velocity) {
       reference_point.point.longitudinal_velocity_mps = intersection_velocity;
     }
@@ -1326,41 +1340,63 @@
     return {{0.0, 0.0}};  // has already passed the intersection.
   }
 
+  std::vector<std::pair<double, double>> original_path_xy;
+  for (size_t i = 0; i < reference_path.points.size(); ++i) {
+    const auto & p = reference_path.points.at(i).point.pose.position;
+    original_path_xy.emplace_back(p.x, p.y);
+  }
+
   // apply smoother to reference velocity
   PathWithLaneId smoothed_reference_path = reference_path;
-  smoothPath(reference_path, smoothed_reference_path, planner_data);
+  if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) {
+    smoothed_reference_path = reference_path;
+  }
 
   // calculate when ego is going to reach each (interpolated) points on the path
   TimeDistanceArray time_distance_array{};
-  dist_sum = 0.0;
+  double dist_sum = 0.0;
   double passing_time = time_delay;
   time_distance_array.emplace_back(passing_time, dist_sum);
 
   // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so
   // `last_intersection_stop_line_candidate_idx` makes no sense
-  const auto last_intersection_stop_line_candidate_point_orig =
-    path.points.at(last_intersection_stop_line_candidate_idx).point.pose;
-  const auto last_intersection_stop_line_candidate_nearest_ind =
-    motion_utils::findFirstNearestIndexWithSoftConstraints(
-      smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig,
-      planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold);
+  const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    smoothed_reference_path.points, path.points.at(closest_idx).point.pose,
+    planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold);
+
+  const std::optional<size_t> upstream_stop_line_idx_opt = [&]() -> std::optional<size_t> {
+    if (upstream_stop_line) {
+      const auto upstream_stop_line_point = path.points.at(upstream_stop_line.value()).point.pose;
+      return motion_utils::findFirstNearestIndexWithSoftConstraints(
+        smoothed_reference_path.points, upstream_stop_line_point,
+        planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold);
+    } else {
+      return std::nullopt;
+    }
+  }();
+  const bool has_upstream_stopline = upstream_stop_line_idx_opt.has_value();
+  const size_t upstream_stopline_ind = upstream_stop_line_idx_opt.value_or(0);
 
-  for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) {
-    const auto & p1 = smoothed_reference_path.points.at(i - 1);
-    const auto & p2 = smoothed_reference_path.points.at(i);
+  for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) {
+    const auto & p1 = smoothed_reference_path.points.at(i);
+    const auto & p2 = smoothed_reference_path.points.at(i + 1);
 
     const double dist = tier4_autoware_utils::calcDistance2d(p1, p2);
     dist_sum += dist;
 
     // use average velocity between p1 and p2
     const double average_velocity =
       (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0;
-    const double minimum_ego_velocity_division =
-      (use_upstream_velocity && i > last_intersection_stop_line_candidate_nearest_ind)
-        ? minimum_upstream_velocity /* to avoid null division */
-        : minimum_ego_velocity;
-    const double passing_velocity =
-      std::max<double>(minimum_ego_velocity_division, average_velocity);
+    const double passing_velocity = [=]() {
+      if (use_upstream_velocity) {
+        if (has_upstream_stopline && i > upstream_stopline_ind) {
+          return minimum_upstream_velocity;
+        }
+        return std::max<double>(average_velocity, minimum_ego_velocity);
+      } else {
+        return std::max<double>(average_velocity, minimum_ego_velocity);
+      }
+    }();
     passing_time += (dist / passing_velocity);
 
     time_distance_array.emplace_back(passing_time, dist_sum);
@@ -1370,6 +1406,8 @@
   debug_ttc_array->layout.dim.at(0).size = 5;
   debug_ttc_array->layout.dim.at(1).label = "values";
   debug_ttc_array->layout.dim.at(1).size = time_distance_array.size();
+  debug_ttc_array->data.reserve(
+    time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size);
   for (unsigned i = 0; i < time_distance_array.size(); ++i) {
     debug_ttc_array->data.push_back(lane_id);
   }
@@ -1379,11 +1417,13 @@
   for (const auto & [t, d] : time_distance_array) {
     debug_ttc_array->data.push_back(d);
   }
-  for (const auto & p : smoothed_reference_path.points) {
-    debug_ttc_array->data.push_back(p.point.pose.position.x);
+  for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) {
+    const auto & p = smoothed_reference_path.points.at(i).point.pose.position;
+    debug_ttc_array->data.push_back(p.x);
   }
-  for (const auto & p : smoothed_reference_path.points) {
-    debug_ttc_array->data.push_back(p.point.pose.position.y);
+  for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) {
+    const auto & p = smoothed_reference_path.points.at(i).point.pose.position;
+    debug_ttc_array->data.push_back(p.y);
   }
   return time_distance_array;
 }
@@ -1464,7 +1504,7 @@
     const auto & p = path.points.at(i).point.pose;
     const auto & p_prev = path.points.at(prev_idx).point.pose;
     if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) {
       continue;
     }
     prev_idx = i;
     const double yaw = tf2::getYaw(p.orientation);

planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp

@@ -88,15 +88,14 @@
   const auto & smoother = planner_data->velocity_smoother_;
 
   auto trajectory = convertPathToTrajectoryPoints(in_path);
-  if (external_v_limit) {
-    motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
-      0, trajectory.size(), external_v_limit->max_velocity, trajectory);
-  }
   const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
 
+  const auto traj_steering_rate_limited =
+    smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false);
+
   // Resample trajectory with ego-velocity based interval distances
   auto traj_resampled = smoother->resampleTrajectory(
-    traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold,
+    traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
   const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints(
     traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
@@ -114,6 +113,10 @@
   traj_smoothed.insert(
     traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
 
+  if (external_v_limit) {
+    motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
+      traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
+  }
   out_path = convertTrajectoryPointsToPath(traj_smoothed);
   return true;
 }