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 3579884064d5d..e6e7a076377f1 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 @@ -221,6 +221,7 @@ + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 472538406c382..6f1f38025b29e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +82,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; + autoware::universe_utils::InterProcessPollingSubscriber< + jpn_signal_v2i_msgs::msg::TrafficLightInfo> + sub_traffic_signals_raw_v2i_{this, "~/input/traffic_signals_raw_v2i"}; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> + sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; @@ -100,6 +109,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void processTrafficSignals( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + void processTrafficSignalsRawV2I( + const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg); bool processData(rclcpp::Clock clock); // publisher diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 399762f0b8607..2f9d82f334c6b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -40,6 +40,7 @@ + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index c40d80c2bf998..8d153e5bff421 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -45,6 +45,7 @@ diagnostic_msgs eigen geometry_msgs + jpn_signal_v2i_msgs libboost-dev pcl_conversions pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 443bfebe2a3eb..084962b3026cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -228,6 +228,17 @@ void BehaviorVelocityPlannerNode::processTrafficSignals( } } +void BehaviorVelocityPlannerNode::processTrafficSignalsRawV2I( + const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg) +{ + 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_light_group_id] = + car_light.min_rest_time_to_red; + } + } +} + bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) { bool is_ready = true; @@ -280,6 +291,9 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) const auto traffic_signals = sub_traffic_signals_.takeData(); if (traffic_signals) processTrafficSignals(traffic_signals); + const auto traffic_signals_raw_v2i = sub_traffic_signals_raw_v2i_.takeData(); + if (traffic_signals_raw_v2i) processTrafficSignalsRawV2I(traffic_signals_raw_v2i); + return is_ready; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index f8b37999cb6bf..ae7338c2735ca 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -62,6 +62,7 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; + std::map traffic_light_time_to_red_id_map_; std::optional external_velocity_limit; bool is_simulation = false; @@ -80,6 +81,8 @@ struct PlannerData std::optional getTrafficSignal( const lanelet::Id id, const bool keep_last_observation = false) const; + + std::optional getRestTimeToRedSignal(const lanelet::Id & id) const; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp index df2a183e3bfb3..ab4ace4b8bae1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp @@ -69,4 +69,14 @@ std::optional PlannerData::getTrafficSignal( } return std::make_optional(traffic_light_id_map.at(id)); } + +std::optional PlannerData::getRestTimeToRedSignal(const lanelet::Id & id) const +{ + try { + return traffic_light_time_to_red_id_map_.at(id); + } catch (std::out_of_range &) { + return std::nullopt; + } +} + } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 23746a61b6043..191af7752202d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_planner/autoware_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_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 9a402f31af0e4..5dc150e88bb6d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -42,6 +42,13 @@ 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_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index b051d5ff7eb76..e485c476775aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -107,6 +107,31 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path) first_ref_stop_path_point_index_ = stop_line.value().first; + // Use V2I if available + 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; + + // Determine whether to stop based on velocity and time constraints + bool should_stop = + (ego_v >= planner_param_.v2i_velocity_threshold && + ego_v * rest_time_allowed_to_go_ahead <= signed_arc_length_to_stop_point) || + (ego_v < planner_param_.v2i_velocity_threshold && + rest_time_allowed_to_go_ahead < planner_param_.v2i_required_time_to_departure); + + // RTC + setSafe(!should_stop); + if (isActivated()) { + return true; + } + *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second); + return true; + } + // Check if stop is coming. const bool is_stop_signal = isStopSignal(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index c30cbbdfc1477..0dfa20379d5f8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -63,6 +63,10 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC 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: