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

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

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/docs/ttc.gif

@@ -126,8 +126,11 @@ def plot_ttc(self):
         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 @@ def plot_ttc(self):
                 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")
@@ -218,6 +219,7 @@ 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 @@ def on_object_ttc(self, msg):
         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/scripts/ttc.py

@@ -36,6 +36,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <list>
 #include <memory>
 #include <set>
@@ -1304,14 +1305,27 @@ TimeDistanceArray calcIntersectionPassingTime(
   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 @@ TimeDistanceArray calcIntersectionPassingTime(
     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 @@ TimeDistanceArray calcIntersectionPassingTime(
   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 @@ TimeDistanceArray calcIntersectionPassingTime(
   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;
 }

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -88,15 +88,14 @@ bool smoothPath(
   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 @@ bool smoothPath(
   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;
 }