From 1dbb40d495c6b2284fe3adb4e1f6fa5f1c155890 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Thu, 15 Feb 2024 20:43:24 +0900 Subject: [PATCH 1/3] feat(behavior velocity planner, traffic light)!: conject with v2i msg (#941) enabling GO/STOP decision by v2i rest time information --------- Signed-off-by: Yuki Takagi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Signed-off-by: Tomohito Ando --- .../behavior_planning.launch.xml | 1 + .../behavior_velocity_planner.launch.xml | 1 + .../behavior_velocity_planner/package.xml | 1 + .../behavior_velocity_planner/src/node.cpp | 18 ++++++++++++++++++ .../behavior_velocity_planner/src/node.hpp | 5 +++++ .../planner_data.hpp | 11 +++++++++++ .../package.xml | 1 + .../config/traffic_light.param.yaml | 6 ++++++ .../src/manager.cpp | 8 ++++++++ .../src/scene.cpp | 19 +++++++++++++++++++ .../src/scene.hpp | 4 ++++ 11 files changed, 75 insertions(+) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index b36b40ed6a980..b4af53a85e0e0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -226,6 +226,7 @@ + diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 22220ccd182a1..2593a289d390d 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -43,6 +43,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 1357b555f0ba4..4f09a8ce2d39c 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -41,6 +41,7 @@ diagnostic_msgs eigen geometry_msgs + jpn_signal_v2i_msgs lanelet2_extension libboost-dev motion_utils diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5da8df9e70c35..4759f78d9bd8a 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -108,6 +108,11 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); + sub_traffic_signals_raw_v2i_ = + this->create_subscription( + "~/input/traffic_signals_raw_v2i", 1, + std::bind(&BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I, this, _1), + createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -330,6 +335,19 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } +void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( + const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + for (const auto & car_light : msg->car_lights) { + for (const auto & state : car_light.states) { + planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = + car_light.min_rest_time_to_red; + } + } +} + void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..29a72a5bc2c8f 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include #include #include @@ -71,6 +73,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_raw_v2i_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; @@ -86,6 +90,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void onTrafficSignalsRawV2I(const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..a692373373da6 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -78,6 +79,7 @@ struct PlannerData // other internal data std::map traffic_light_id_map; std::optional external_velocity_limit; + std::map traffic_light_time_to_red_id_map; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother @@ -132,6 +134,15 @@ struct PlannerData } return std::make_shared(traffic_light_id_map.at(id)); } + + std::optional getRestTimeToRedSignal(const int id) const + { + try { + return traffic_light_time_to_red_id_map.at(id); + } catch (std::out_of_range &) { + return std::nullopt; + } + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 784b7cabfe9ad..4d2563bc2c2ff 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -28,6 +28,7 @@ eigen geometry_msgs interpolation + jpn_signal_v2i_msgs lanelet2_extension motion_utils motion_velocity_smoother diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 23746a61b6043..191af7752202d 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -7,3 +7,9 @@ yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: 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 + + v2i: + use_rest_time: true + last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red + velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not + required_time_to_departure: 3.0 # prevent low speed pass diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 908627540bdcc..521ac677b24a1 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -43,6 +43,14 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.v2i_use_rest_time = getOrDeclareParameter(node, ns + ".v2i.use_rest_time"); + planner_param_.v2i_last_time_allowed_to_pass = + getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); + planner_param_.v2i_velocity_threshold = + getOrDeclareParameter(node, ns + ".v2i.velocity_threshold"); + planner_param_.v2i_required_time_to_departure = + getOrDeclareParameter(node, ns + ".v2i.required_time_to_departure"); + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a74ff8c0cb5e8..93214782571d5 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -228,6 +228,25 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; + const std::optional rest_time_to_red_signal = + planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); + if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal) { + const double rest_time_allowed_to_go_ahead = + rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass; + + const double ego_v = planner_data_->current_velocity->twist.linear.x; + if (ego_v >= planner_param_.v2i_velocity_threshold) { + if (ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } + } else { + if (rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + } + } + return true; + } + // Check if stop is coming. const bool is_stop_signal = isStopSignal(); diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 6e46709267427..7e66db28e5577 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -63,6 +63,10 @@ class TrafficLightModule : public SceneModuleInterface double yellow_lamp_period; double stop_time_hysteresis; bool enable_pass_judge; + bool v2i_use_rest_time; + double v2i_last_time_allowed_to_pass; + double v2i_velocity_threshold; + double v2i_required_time_to_departure; }; public: From 753e93ccf964f88a22081d6523d3730547f820be Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 21 Nov 2023 17:49:06 +0900 Subject: [PATCH 2/3] fix(traffic_light): discard remaining time to red if it is timeout (#1030) Signed-off-by: Tomohito Ando --- .../behavior_velocity_planner/src/node.cpp | 6 ++++-- .../planner_data.hpp | 4 ++-- .../utilization/util.hpp | 6 ++++++ .../src/scene.cpp | 21 ++++++++++++++++--- .../src/scene.hpp | 2 ++ 5 files changed, 32 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 4759f78d9bd8a..88b09903eef39 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -342,8 +342,10 @@ void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( for (const auto & car_light : msg->car_lights) { for (const auto & state : car_light.states) { - planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = - car_light.min_rest_time_to_red; + TrafficSignalTimeToRedStamped time_to_red; + time_to_red.stamp = msg->header.stamp; + time_to_red.time_to_red = car_light.min_rest_time_to_red; + planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = time_to_red; } } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index a692373373da6..0ab939f225031 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -79,7 +79,7 @@ struct PlannerData // other internal data std::map traffic_light_id_map; std::optional external_velocity_limit; - std::map traffic_light_time_to_red_id_map; + std::map traffic_light_time_to_red_id_map; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother @@ -135,7 +135,7 @@ struct PlannerData return std::make_shared(traffic_light_id_map.at(id)); } - std::optional getRestTimeToRedSignal(const int id) const + std::optional getRestTimeToRedSignal(const int id) const { try { return traffic_light_time_to_red_id_map.at(id); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index e042338c53485..87c68506c73ce 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -56,6 +56,12 @@ struct TrafficSignalStamped autoware_perception_msgs::msg::TrafficSignal signal; }; +struct TrafficSignalTimeToRedStamped +{ + builtin_interfaces::msg::Time stamp; + double time_to_red; +}; + using Pose = geometry_msgs::msg::Pose; using Point2d = tier4_autoware_utils::Point2d; using LineString2d = tier4_autoware_utils::LineString2d; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 93214782571d5..b034540c3dbca 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -228,11 +228,13 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - const std::optional rest_time_to_red_signal = + const auto rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); - if (planner_param_.v2i_use_rest_time && rest_time_to_red_signal) { + if ( + planner_param_.v2i_use_rest_time && rest_time_to_red_signal && + !isDataTimeout(rest_time_to_red_signal->stamp)) { const double rest_time_allowed_to_go_ahead = - rest_time_to_red_signal.value() - planner_param_.v2i_last_time_allowed_to_pass; + rest_time_to_red_signal->time_to_red - planner_param_.v2i_last_time_allowed_to_pass; const double ego_v = planner_data_->current_velocity->twist.linear.x; if (ego_v >= planner_param_.v2i_velocity_threshold) { @@ -495,4 +497,17 @@ bool TrafficLightModule::hasTrafficLightShape( return it_lamp != tl_state.elements.end(); } +bool TrafficLightModule::isDataTimeout(const rclcpp::Time & data_time) const +{ + const auto now = clock_->now(); + const bool is_data_timeout = (now - data_time).seconds() > planner_param_.tl_state_timeout; + if (is_data_timeout) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "data is timeout. time diff: " << (now - data_time).seconds()); + } + + return is_data_timeout; +} + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 7e66db28e5577..7e54b441afc04 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -112,6 +112,8 @@ class TrafficLightModule : public SceneModuleInterface void updateTrafficSignal(); + bool isDataTimeout(const rclcpp::Time & data_time) const; + // Lane id const int64_t lane_id_; From c36c197d0cd9fc0bc26b8f787fcda1098311d115 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Mon, 19 Feb 2024 19:28:56 +0900 Subject: [PATCH 3/3] fix(bvp): traffic light state debug (#1083) Signed-off-by: Mehmet Dogru Signed-off-by: Tomohito Ando --- .../src/scene.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index b034540c3dbca..bd80ae6bb5082 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -228,6 +228,10 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; + // Check if stop is coming. + const bool is_stop_signal = isStopSignal(); + + // Decide if stop or proceed using the remaining time to red signal const auto rest_time_to_red_signal = planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); if ( @@ -249,9 +253,6 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } - // Check if stop is coming. - const bool is_stop_signal = isStopSignal(); - // Update stop signal received time if (is_stop_signal) { if (!stop_signal_received_time_ptr_) { @@ -319,6 +320,9 @@ void TrafficLightModule::updateTrafficSignal() TrafficSignalStamped signal; if (!findValidTrafficSignal(signal)) { // Don't stop if it never receives traffic light topic. + // Reset looking_tl_state + looking_tl_state_.elements.clear(); + looking_tl_state_.traffic_signal_id = 0; return; }