From 8ee537cae4af55e515910294738f01d92d2ed829 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 27 Oct 2023 14:27:10 +0900 Subject: [PATCH 1/4] add ttc live plotter Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 5 + .../config/intersection.param.yaml | 3 + .../scripts/ttc.py | 268 ++++++++++++++++++ .../src/manager.cpp | 1 + .../src/scene_intersection.cpp | 48 +++- .../src/scene_intersection.hpp | 7 + .../src/util.cpp | 21 +- .../src/util.hpp | 5 +- 8 files changed, 354 insertions(+), 4 deletions(-) create mode 100755 planning/behavior_velocity_intersection_module/scripts/ttc.py diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 8ff78dac06716..9e7eb196cd0c1 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -20,3 +20,8 @@ target_link_libraries(${PROJECT_NAME} ) ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/ttc.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 82a5f65622d0b..882cd2c315c43 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -83,6 +83,9 @@ attention_lane_curvature_calculation_ds: 0.5 static_occlusion_with_traffic_light_timeout: 3.5 + debug: + ttc: [] + enable_rtc: intersection: false # 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 intersection_to_occlusion: false diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py new file mode 100755 index 0000000000000..76270c96756c5 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -0,0 +1,268 @@ +#!/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.pred_x = data[13:53:2] + self.pred_y = data[14:53: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.ego_ttc_data = None + self.object_ttc_data = None + self.npcs = [] + self.args = args + 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.colorlist = [ + "#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[0:n_ttc_data] + ego_ttc_dist = self.ego_ttc_data.data[n_ttc_data : 2 * 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, max(ego_ttc_time) + 3.0) + self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0) + for npc, color in zip(self.npcs, cycle(self.colorlist)): + 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[2 * n_ttc_data : 3 * n_ttc_data] + ego_path_y = self.ego_ttc_data.data[3 * n_ttc_data : 4 * 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.npcs, cycle(self.colorlist)): + 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) + 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) + + 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: + self.ego_ttc_data = msg + self.last_sub = time.time() + + def parse_npcs(self): + self.npcs = [] + n_npcs = 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(0, n_npcs): + data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size] + self.npcs.append(NPC(data)) + + def on_object_ttc(self, msg): + with self.lock: + self.object_ttc_data = msg + self.parse_npcs() + self.last_sub = time.time() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--range", + type=float, + default=100, + help="detect range for drawing", + ) + 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) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c0d7fa0b4f1c..5c5f5eac16b81 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -181,6 +181,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 31b63413a6437..05811f49cb15d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -138,6 +138,10 @@ IntersectionModule::IntersectionModule( decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); + ego_ttc_pub_ = node_.create_publisher( + "~/debug/intersection/ego_ttc", 1); + object_ttc_pub_ = node_.create_publisher( + "~/debug/intersection/object_ttc", 1); } void IntersectionModule::initializeRTCStatus() @@ -1465,12 +1469,21 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = util::calcIntersectionPassingTime( 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); + planner_param_.collision_detection.minimum_upstream_velocity, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + const double passing_time = time_distance_array.back().first; util::cutPredictPathWithDuration(target_objects, clock_, passing_time); @@ -1552,6 +1565,15 @@ bool IntersectionModule::checkCollision( return (object2collision > margin) || (object2collision < 0); }; // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 0; // incremented in the loop + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 53; bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; @@ -1650,6 +1672,23 @@ bool IntersectionModule::checkCollision( break; } } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } if (collision_detected) { debug_data_.conflicting_targets.objects.push_back(object); break; @@ -1657,6 +1696,13 @@ bool IntersectionModule::checkCollision( } } + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + return collision_detected; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 93fec171ec0d2..c794ef6c53aad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -150,6 +151,10 @@ class IntersectionModule : public SceneModuleInterface double attention_lane_curvature_calculation_ds; double static_occlusion_with_traffic_light_timeout; } occlusion; + struct Debug + { + std::vector ttc; + } debug; }; enum OcclusionType { @@ -363,6 +368,8 @@ class IntersectionModule : public SceneModuleInterface util::DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr ego_ttc_pub_; + rclcpp::Publisher::SharedPtr object_ttc_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 25eaaf4d8cb9d..47db478d3cd1c 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1308,7 +1308,8 @@ TimeDistanceArray calcIntersectionPassingTime( const std::shared_ptr & planner_data, const std::set & associative_ids, const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity) + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1374,7 +1375,23 @@ TimeDistanceArray calcIntersectionPassingTime( time_distance_array.emplace_back(passing_time, dist_sum); } - + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 4; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + 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 (const auto & p : smoothed_reference_path.points) { + debug_ttc_array->data.push_back(p.point.pose.position.y); + } return time_distance_array; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5faacd4325b06..4e76d9c9d4f4b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,6 +20,8 @@ #include +#include + #include #include #include @@ -149,7 +151,8 @@ TimeDistanceArray calcIntersectionPassingTime( const std::shared_ptr & planner_data, const std::set & associative_ids, const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity); + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, From e99b4f03281c2bdf31e1dd4b6a435ae7c035b732 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 30 Oct 2023 11:50:46 +0900 Subject: [PATCH 2/4] plot first_last collision itr Signed-off-by: Mamoru Sobue --- .../scripts/ttc.py | 16 +++++++++++++--- .../src/scene_intersection.cpp | 8 +++++--- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py index 76270c96756c5..d5db7ca07fe3e 100755 --- a/planning/behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -64,8 +64,12 @@ def __init__(self, data: list[float]): self.collision_start_dist = data[10] self.collision_end_time = data[11] self.collision_end_dist = data[12] - self.pred_x = data[13:53:2] - self.pred_y = data[14:53:2] + 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): @@ -196,6 +200,12 @@ def plot_world(self): 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: @@ -255,7 +265,7 @@ def on_object_ttc(self, msg): parser.add_argument( "--range", type=float, - default=100, + default=60, help="detect range for drawing", ) parser.add_argument("-s", "--save", action="store_true", help="flag to save gif") diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 05811f49cb15d..8b2ce58366a8f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1572,8 +1572,9 @@ bool IntersectionModule::checkCollision( object_ttc_time_array.layout.dim.at(1).label = "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " "start_time, start_dist, " - "end_time, end_dist, prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; - object_ttc_time_array.layout.dim.at(1).size = 53; + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; @@ -1682,7 +1683,8 @@ bool IntersectionModule::checkCollision( object.kinematics.initial_twist_with_covariance.twist.linear.x, 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, start_time_distance_itr->first, start_time_distance_itr->second, - end_time_distance_itr->first, end_time_distance_itr->second}); + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); for (unsigned i = 0; i < 20; i++) { const auto & pos = predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; From 358d992b49cb17023231e4d50975fbe4d9c82a53 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 30 Oct 2023 11:59:27 +0900 Subject: [PATCH 3/4] fix spell Signed-off-by: Mamoru Sobue --- .cspell-partial.json | 2 +- .../scripts/ttc.py | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/.cspell-partial.json b/.cspell-partial.json index 13849ef298019..e231eddd712ea 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -5,5 +5,5 @@ "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen"] + "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"] } diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py index d5db7ca07fe3e..8d1cc82f6776e 100755 --- a/planning/behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -89,7 +89,7 @@ def __init__(self, args): ) self.ego_ttc_data = None self.object_ttc_data = None - self.npcs = [] + self.npc_vehicles = [] self.args = args self.images = [] self.last_sub = time.time() @@ -100,7 +100,7 @@ def __init__(self, args): self.ttc_vel_ax = self.ttc_ax.twinx() self.world_ax = self.fig.add_subplot(1, 2, 2) self.lock = Lock() - self.colorlist = [ + self.color_list = [ "#e41a1c", "#377eb8", "#4daf4a", @@ -127,7 +127,7 @@ def plot_ttc(self): 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) - for npc, color in zip(self.npcs, cycle(self.colorlist)): + 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( @@ -186,7 +186,7 @@ def plot_world(self): head_length=0.1, ) - for npc, color in zip(self.npcs, cycle(self.colorlist)): + 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( [ @@ -245,18 +245,18 @@ def on_ego_ttc(self, msg): self.ego_ttc_data = msg self.last_sub = time.time() - def parse_npcs(self): - self.npcs = [] - n_npcs = int(self.object_ttc_data.layout.dim[0].size) + 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(0, n_npcs): + for i in range(0, n_npc_vehicles): data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size] - self.npcs.append(NPC(data)) + self.npc_vehicles.append(NPC(data)) def on_object_ttc(self, msg): with self.lock: self.object_ttc_data = msg - self.parse_npcs() + self.parse_npc_vehicles() self.last_sub = time.time() From d843a87cebed4688af5652f4201473aa7c8b0a74 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 30 Oct 2023 14:29:51 +0900 Subject: [PATCH 4/4] add default lane_id value Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 +- .../scripts/ttc.py | 31 ++++++++++++------- .../src/scene_intersection.cpp | 10 ++++-- .../src/util.cpp | 14 ++++++--- .../src/util.hpp | 7 +++-- 5 files changed, 41 insertions(+), 23 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 882cd2c315c43..adbc7b3e087d6 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -84,7 +84,7 @@ static_occlusion_with_traffic_light_timeout: 3.5 debug: - ttc: [] + ttc: [0] enable_rtc: intersection: false # 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 diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py index 8d1cc82f6776e..e4633981570d1 100755 --- a/planning/behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -87,10 +87,11 @@ def __init__(self, args): 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.args = args self.images = [] self.last_sub = time.time() @@ -118,8 +119,8 @@ def plot_ttc(self): 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[0:n_ttc_data] - ego_ttc_dist = self.ego_ttc_data.data[n_ttc_data : 2 * n_ttc_data] + 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") @@ -153,8 +154,8 @@ 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[2 * n_ttc_data : 3 * n_ttc_data] - ego_path_y = self.ego_ttc_data.data[3 * n_ttc_data : 4 * 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) @@ -242,26 +243,34 @@ def on_plot_timer(self): def on_ego_ttc(self, msg): with self.lock: - self.ego_ttc_data = msg - self.last_sub = time.time() + 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(0, n_npc_vehicles): + 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: - self.object_ttc_data = msg - self.parse_npc_vehicles() - self.last_sub = time.time() + 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, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 8b2ce58366a8f..c3b429680ab91 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1471,8 +1471,9 @@ bool IntersectionModule::checkCollision( // cut the predicted path at passing_time tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, - time_delay, planner_param_.common.intersection_velocity, + path, planner_data_, lane_id_, 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, &ego_ttc_time_array); @@ -1568,13 +1569,16 @@ bool IntersectionModule::checkCollision( tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; object_ttc_time_array.layout.dim.resize(3); object_ttc_time_array.layout.dim.at(0).label = "objects"; - object_ttc_time_array.layout.dim.at(0).size = 0; // incremented in the loop + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id object_ttc_time_array.layout.dim.at(1).label = "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " "start_time, start_dist, " "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 47db478d3cd1c..c27a2ad218ffb 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1305,9 +1305,10 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, const bool use_upstream_velocity, const double minimum_upstream_velocity, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { @@ -1376,10 +1377,13 @@ TimeDistanceArray calcIntersectionPassingTime( time_distance_array.emplace_back(passing_time, dist_sum); } debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 4; + 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(); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id); + } for (const auto & [t, d] : time_distance_array) { debug_ttc_array->data.push_back(t); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 4e76d9c9d4f4b..11aab06ff1d94 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -148,9 +148,10 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, const bool use_upstream_velocity, const double minimum_upstream_velocity, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);