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

planning/behavior_velocity_intersection_module/README.md

@@ -8,7 +8,7 @@
 2. recognizing the occluded area in the intersection
 3. reacting to arrow signals of associated traffic lights
 
 The module is desinged to be agnositc to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc.
 
 ![topology](./docs/intersection-topology.drawio.svg)
 
@@ -26,7 +26,7 @@
 
 The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`).
 
 `Intersection Area`, which is supposed to be defined on the HDMap, is an area convering the entire intersection.
 
 ![attention_area](./docs/intersection-attention.drawio.svg)
 
@@ -38,7 +38,7 @@
 | ------------------------------ | --------------------------------------------------------------- | ------------------------------------------------ |
 | straight                       | Highest priority of all                                         | Priority over left/right lanes of the same group |
 | left(Left hand traffic)        | Priority over the other group and right lanes of the same group | Priority over right lanes of the same group      |
 | right(Left hand traffic)       | Priority only over the other group                              | priority only over the other gruop               |
 | left(Right hand traffic)       | Priority only over the other group                              | Priority only over the other group               |
 | right(Right hand traffic)      | Priority over the other group and left lanes of the same group  | priority over left lanes of the same group       |
 
@@ -86,6 +86,14 @@
 
 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

@@ -21,19 +21,21 @@
         timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area
 
       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
 
       occlusion:
         enable: false
@@ -57,7 +59,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
 
       enable_rtc:
         intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval

planning/behavior_velocity_intersection_module/package.xml

@@ -9,6 +9,7 @@
   <maintainer email="[email protected]">Takayuki Murooka</maintainer>
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
+  <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/behavior_velocity_intersection_module/scripts/ttc.py

@@ -0,0 +1,290 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 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.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+from dataclasses import dataclass
+from itertools import cycle
+import math
+from threading import Lock
+import time
+
+import imageio
+import matplotlib
+import matplotlib.pyplot as plt
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float64MultiArrayStamped
+
+matplotlib.use("TKAgg")
+
+
+@dataclass
+class NPC:
+    x: float
+    y: float
+    th: float
+    width: float
+    height: float
+    speed: float
+    dangerous: bool
+    ref_object_enter_time: float
+    ref_object_exit_time: float
+    collision_start_time: float
+    collision_start_dist: float
+    collision_end_time: float
+    collision_end_dist: float
+    pred_x: list[float]
+    pred_y: list[float]
+
+    def __init__(self, data: list[float]):
+        self.x = data[0]
+        self.y = data[1]
+        self.th = data[2]
+        self.width = data[3]
+        self.height = data[4]
+        self.speed = data[5]
+        self.dangerous = bool(int(data[6]))
+        self.ref_object_enter_time = data[7]
+        self.ref_object_exit_time = data[8]
+        self.collision_start_time = data[9]
+        self.collision_start_dist = data[10]
+        self.collision_end_time = data[11]
+        self.collision_end_dist = data[12]
+        self.first_collision_x = data[13]
+        self.first_collision_y = data[14]
+        self.last_collision_x = data[15]
+        self.last_collision_y = data[16]
+        self.pred_x = data[17:58:2]
+        self.pred_y = data[18:58:2]
+
+
+class TTCVisualizer(Node):
+    def __init__(self, args):
+        super().__init__("ttc_visualizer")
+        self.ttc_dist_pub = self.create_subscription(
+            Float64MultiArrayStamped,
+            "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/ego_ttc",
+            self.on_ego_ttc,
+            1,
+        )
+        self.ttc_time_pub = self.create_subscription(
+            Float64MultiArrayStamped,
+            "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/object_ttc",
+            self.on_object_ttc,
+            1,
+        )
+        self.args = args
+        self.lane_id = args.lane_id
+        self.ego_ttc_data = None
+        self.object_ttc_data = None
+        self.npc_vehicles = []
+        self.images = []
+        self.last_sub = time.time()
+
+        self.plot_timer = self.create_timer(0.2, self.on_plot_timer)
+        self.fig = plt.figure(figsize=(13, 6))
+        self.ttc_ax = self.fig.add_subplot(1, 2, 1)
+        self.ttc_vel_ax = self.ttc_ax.twinx()
+        self.world_ax = self.fig.add_subplot(1, 2, 2)
+        self.lock = Lock()
+        self.color_list = [
+            "#e41a1c",
+            "#377eb8",
+            "#4daf4a",
+            "#984ea3",
+            "#ff7f00",
+            "#ffff33",
+            "#a65628",
+            "#f781bf",
+        ]
+        plt.ion()
+        plt.show(block=False)
+
+    def plot_ttc(self):
+        self.ttc_ax.cla()
+        self.ttc_vel_ax.cla()
+
+        n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size)
+        ego_ttc_time = self.ego_ttc_data.data[n_ttc_data : 2 * n_ttc_data]
+        ego_ttc_dist = self.ego_ttc_data.data[2 * n_ttc_data : 3 * n_ttc_data]
+
+        self.ttc_ax.grid()
+        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,
+            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
+            self.ttc_ax.fill(
+                [t0, t0, t1, t1, 0, 0],
+                [d0, 0, 0, d1, d1, d0],
+                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)
+        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")
+
+    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[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)
+        min_y, max_y = min(ego_path_y), max(ego_path_y)
+        x_dir = 1 if ego_path_x[-1] > ego_path_x[0] else -1
+        y_dir = 1 if ego_path_y[-1] > ego_path_y[0] else -1
+        min_x = min_x - detect_range if x_dir == 1 else min_x - 20
+        max_x = max_x + detect_range if x_dir == -1 else max_x + 20
+        min_y = min_y - detect_range if y_dir == 1 else min_y - 20
+        max_y = max_y + detect_range if y_dir == -1 else max_y + 20
+        self.world_ax.set_xlim(min_x, max_x)
+        self.world_ax.set_ylim(min_y, max_y)
+        arrows = [
+            (x0, y0, math.atan2(x1 - x0, y1 - y0))
+            for (x0, y0, x1, y1) in zip(
+                ego_path_x[0:-1:5],
+                ego_path_y[0:-1:5],
+                ego_path_x[4:-1:5],
+                ego_path_y[4:-1:5],
+            )
+        ]
+        for x, y, th in arrows:
+            self.world_ax.arrow(
+                x,
+                y,
+                math.sin(th) * 0.5,
+                math.cos(th) * 0.5,
+                head_width=0.1,
+                head_length=0.1,
+            )
+
+        for npc, color in zip(self.npc_vehicles, cycle(self.color_list)):
+            x, y, th, w, h = npc.x, npc.y, npc.th, npc.width, npc.height
+            bbox = np.array(
+                [
+                    [-w / 2, -h / 2],
+                    [+w / 2, -h / 2],
+                    [+w / 2, +h / 2],
+                    [-w / 2, +h / 2],
+                    [-w / 2, -h / 2],
+                ]
+            ).transpose()
+            Rth = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]])
+            bbox_rot = Rth @ bbox
+            self.world_ax.fill(bbox_rot[0, :] + x, bbox_rot[1, :] + y, color, alpha=0.5)
+            self.world_ax.plot(
+                [npc.first_collision_x, npc.last_collision_x],
+                [npc.first_collision_y, npc.last_collision_y],
+                c=color,
+                linewidth=3.0,
+            )
+            if npc.dangerous:
+                self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linewidth=1.5)
+            else:
+                self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linestyle="--")
+
+        self.world_ax.plot(ego_path_x, ego_path_y, c="k", linestyle="--")
+
+    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:
+            if (not self.ego_ttc_data) or (not self.object_ttc_data):
+                return
+
+            if not self.last_sub:
+                return
+
+            now = time.time()
+            if (now - self.last_sub) > 1.0:
+                print("elapsed more than 1sec from last sub, exit/save fig")
+                self.cleanup()
+
+            self.plot_ttc()
+            self.plot_world()
+            self.fig.canvas.flush_events()
+
+            if self.args.save:
+                image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8")
+                image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,))
+                self.images.append(image)
+
+    def on_ego_ttc(self, msg):
+        with self.lock:
+            if int(msg.data[0]) == self.lane_id:
+                self.ego_ttc_data = msg
+                self.last_sub = time.time()
+
+    def parse_npc_vehicles(self):
+        self.npc_vehicles = []
+        n_npc_vehicles = int(self.object_ttc_data.layout.dim[0].size)
+        npc_data_size = int(self.object_ttc_data.layout.dim[1].size)
+        for i in range(1, n_npc_vehicles):
+            data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size]
+            self.npc_vehicles.append(NPC(data))
+
+    def on_object_ttc(self, msg):
+        with self.lock:
+            if int(msg.data[0]) == self.lane_id:
+                self.object_ttc_data = msg
+                self.parse_npc_vehicles()
+                self.last_sub = time.time()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--lane_id",
+        type=int,
+        required=True,
+        help="lane_id to analyze",
+    )
+    parser.add_argument(
+        "--range",
+        type=float,
+        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")
+    args = parser.parse_args()
+
+    rclpy.init()
+    visualizer = TTCVisualizer(args)
+    rclpy.spin(visualizer)

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -100,6 +100,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");
+  ip.collision_detection.use_upstream_velocity =
+    node.declare_parameter<bool>(ns + ".collision_detection.use_upstream_velocity");
+  ip.collision_detection.minimum_upstream_velocity =
+    node.declare_parameter<double>(ns + ".collision_detection.minimum_upstream_velocity");
 
   ip.occlusion.enable = node.declare_parameter<bool>(ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1081,7 +1081,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
                                       collision_state_machine_.getDuration());
 
   const bool has_collision = checkCollision(
-    *path, &target_objects, path_lanelets, closest_idx, time_to_restart, traffic_prioritized_level);
+    *path, &target_objects, path_lanelets, closest_idx,
+    std::min<size_t>(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart,
+    traffic_prioritized_level);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
@@ -1281,18 +1283,20 @@ util::TargetObjects IntersectionModule::generateTargetObjects(
 bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
-  const int closest_idx, const double time_delay,
-  const util::TrafficPrioritizedLevel & traffic_prioritized_level)
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
 
   // check collision between target_objects predicted path and ego lane
   // cut the predicted path at passing_time
   const auto time_distance_array = util::calcIntersectionPassingTime(
-    path, planner_data_, associative_ids_, closest_idx, time_delay,
-    planner_param_.common.intersection_velocity,
-    planner_param_.collision_detection.minimum_ego_predicted_velocity);
+    path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx,
+    time_delay, planner_param_.common.intersection_velocity,
+    planner_param_.collision_detection.minimum_ego_predicted_velocity,
+    planner_param_.collision_detection.use_upstream_velocity,
+    planner_param_.collision_detection.minimum_upstream_velocity);
   const double passing_time = time_distance_array.back().first;
   util::cutPredictPathWithDuration(target_objects, clock_, passing_time);