query stop line from upstream path first

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

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

@@ -143,44 +143,20 @@ def plot_ttc(self):
         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)]
-        v_average = self.ego_ttc_data.data[3 * n_ttc_data : 4 * n_ttc_data]
-        v_monotonic = self.ego_ttc_data.data[4 * n_ttc_data : 5 * n_ttc_data]
         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)
         time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red")
-        average_velocity_plot = self.ttc_vel_ax.plot(
-            ego_ttc_time,
-            v_average,
-            label="time-v(avg)",
-            c="blue",
-            linestyle="dashed",
-            marker="o",
-            markerfacecolor="#ffffff",
-            markersize=3,
-        )
-        monotonic_velocity_plot = self.ttc_vel_ax.plot(
-            ego_ttc_time,
-            v_monotonic,
-            label="time-v(monotonic)",
-            c="red",
-            linestyle="dashed",
-            marker="o",
-            markerfacecolor="#ffffff",
-            markersize=3,
-        )
-        lines = (
-            time_dist_plot + time_velocity_plot + average_velocity_plot + monotonic_velocity_plot
-        )
+        lines = time_dist_plot + time_velocity_plot
         labels = [line.get_label() for line in lines]
         self.ttc_ax.legend(lines, labels, loc="upper left")
 
     def plot_world(self):
         detect_range = self.args.range
         self.world_ax.cla()
         n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size)
-        ego_path_x = self.ego_ttc_data.data[5 * n_ttc_data : 6 * n_ttc_data]
-        ego_path_y = self.ego_ttc_data.data[6 * n_ttc_data : 7 * n_ttc_data]
+        ego_path_x = self.ego_ttc_data.data[3 * n_ttc_data : 4 * n_ttc_data]
+        ego_path_y = self.ego_ttc_data.data[4 * n_ttc_data : 5 * n_ttc_data]
         self.world_ax.set_aspect("equal")
         self.world_ax.scatter(ego_path_x[0], ego_path_y[0], marker="x", c="red", s=15)
         min_x, max_x = min(ego_path_x), max(ego_path_x)
@@ -302,7 +278,7 @@ def on_object_ttc(self, msg):
         default=60,
         help="detect range for drawing",
     )
-    parser.add_argument("--max_time", type=float, default=20, help="max plot limit for time")
+    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

@@ -1320,6 +1320,7 @@ TimeDistanceArray calcIntersectionPassingTime(
   // crop intersection part of the path, and set the reference velocity to intersection_velocity
   // for ego's ttc
   PathWithLaneId reference_path;
+  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
@@ -1328,9 +1329,10 @@ TimeDistanceArray calcIntersectionPassingTime(
     }
     if (
       i > last_intersection_stop_line_candidate_idx &&
-      std::fabs(reference_point.point.longitudinal_velocity_mps) ==
-        std::numeric_limits<double>::epsilon()) {
-      RCLCPP_INFO(rclcpp::get_logger("temp"), "found stop line");
+      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;
@@ -1372,22 +1374,20 @@ TimeDistanceArray calcIntersectionPassingTime(
     return time_distance_array;
   }
   const auto smoothed_path_closest_idx = smoothed_path_closest_idx_opt.value();
-  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_opt = motion_utils::findNearestIndex(
-    smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, 3.0, M_PI_4);
-  if (!last_intersection_stop_line_candidate_nearest_ind_opt) {
-    return time_distance_array;
-  }
-  const auto last_intersection_stop_line_candidate_nearest_ind =
-    last_intersection_stop_line_candidate_nearest_ind_opt.value();
 
-  double monotonic_prev_ego_velocity = -1.0;  // NOTE: negative magic value
-  std::vector<double> average_velocities;
-  average_velocities.reserve(smoothed_reference_path.points.size() - 1 - smoothed_path_closest_idx);
-  std::vector<double> monotonic_ego_velocities;
-  monotonic_ego_velocities.reserve(
-    smoothed_reference_path.points.size() - 1 - smoothed_path_closest_idx);
+  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;
+      const auto nearest = motion_utils::findNearestIndex(
+        smoothed_reference_path.points, upstream_stop_line_point, 3.0, M_PI_4);
+      return nearest ? std::make_optional<size_t>(nearest.get()) : std::nullopt;
+    } 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 = 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);
@@ -1398,33 +1398,23 @@ TimeDistanceArray calcIntersectionPassingTime(
     // 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 monotonic_ego_velocity =
-      std::max<double>(average_velocity, monotonic_prev_ego_velocity);
-    monotonic_prev_ego_velocity = monotonic_ego_velocity;
-    average_velocities.push_back(average_velocity);
-    monotonic_ego_velocities.push_back(monotonic_ego_velocity);
-    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 = [=]() {
-      if (average_velocity < minimum_ego_velocity_division) {
-        return minimum_ego_velocity_division;
+      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>(monotonic_ego_velocity, minimum_ego_velocity);
+        return std::max<double>(average_velocity, minimum_ego_velocity);
       }
     }();
     passing_time += (dist / passing_velocity);
 
     time_distance_array.emplace_back(passing_time, dist_sum);
   }
-  average_velocities.push_back(average_velocities.back());
-  monotonic_ego_velocities.push_back(monotonic_ego_velocities.back());
   debug_ttc_array->layout.dim.resize(3);
-  debug_ttc_array->layout.dim.at(0).label =
-    "lane_id_@[0][0], ttc_time, ttc_dist, average_velocities, monotonic_ego_velocities, path_x, "
-    "path_y";
-  debug_ttc_array->layout.dim.at(0).size = 7;
+  debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y";
+  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(
@@ -1438,12 +1428,6 @@ TimeDistanceArray calcIntersectionPassingTime(
   for (const auto & [t, d] : time_distance_array) {
     debug_ttc_array->data.push_back(d);
   }
-  for (const auto v : average_velocities) {
-    debug_ttc_array->data.push_back(v);
-  }
-  for (const auto v : monotonic_ego_velocities) {
-    debug_ttc_array->data.push_back(v);
-  }
   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);