From b1374191118741ecfd8e15a77a472f627627f0b4 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 --- .../behavior_planning.launch.py | 4 ++++ .../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, 78 insertions(+) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 62d4c5b7188ee..5dcaa586506e5 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,6 +177,10 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), + ( + "~/input/traffic_signals_raw_v2i", + "/v2i/external/v2i_traffic_light_info", + ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", 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 b517b77b328c7..379b60cff7f94 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -52,6 +52,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 5a307398e9afd..229bf1f7d05ba 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -40,6 +40,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 438fdd7f37e3a..fe5200904c91b 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -103,6 +103,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)); @@ -301,6 +306,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 b3084db19d1e8..598595b33a7b6 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include #include @@ -64,6 +66,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_; @@ -79,6 +83,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 2668d83b0f510..ad374294d0686 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 @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -84,6 +85,7 @@ struct PlannerData // other internal data std::map traffic_light_id_map; + std::map traffic_light_time_to_red_id_map; boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -139,6 +141,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 aedbab65068fb..8441be5b18dbb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -26,6 +26,7 @@ eigen geometry_msgs interpolation + jpn_signal_v2i_msgs lanelet2_extension libboost-dev motion_utils 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 e8e0357daa4a1..a80c97779edaf 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 @@ -6,3 +6,9 @@ yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: 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 + + 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 f035544b81592..8ef247847c503 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,6 +41,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 e0633926e35df..e8b3c0598514a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -225,6 +225,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. setSafe(!isStopSignal()); if (isActivated()) { diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index a24db2c440e91..b53f9c55e7bb6 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -61,6 +61,10 @@ class TrafficLightModule : public SceneModuleInterface double tl_state_timeout; double yellow_lamp_period; 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: