From 8548d79ef20d2a15415fe9ad56a10eaf371ab712 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 31 Oct 2023 17:17:10 +0900 Subject: [PATCH] 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_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 ++++ 10 files changed, 74 insertions(+) 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: