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..6aac965227efb 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: false + 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/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 518d3275a28d3..16f6e85c422d6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -31,6 +31,7 @@ autoware_universe_utils eigen geometry_msgs + jpn_signal_v2i_msgs pluginlib rclcpp tf2 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 1d1ee7fc996b2..25f34b7e809d2 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 @@ -17,14 +17,17 @@ #include #include #include +#include #include #include #include +#include #include #include #include +#include namespace autoware::behavior_velocity_planner { using autoware::universe_utils::getOrDeclareParameter; @@ -42,8 +45,22 @@ 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); + + if (planner_param_.v2i_use_rest_time) { + RCLCPP_INFO(logger_, "V2I is enabled"); + v2i_subscriber_ = autoware::universe_utils::InterProcessPollingSubscriber< + jpn_signal_v2i_msgs::msg::TrafficLightInfo>:: + create_subscription(&node, "/v2i/external/v2i_traffic_light_info", 1); + } } void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) @@ -53,6 +70,8 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; + if (planner_param_.v2i_use_rest_time) updateV2IRestTimeInfo(); + nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( @@ -103,6 +122,9 @@ void TrafficLightModuleManager::launchNewModules( registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_, time_keeper_, + std::bind( + &TrafficLightModuleManager::getV2IRestTimeToRedSignal, this, + traffic_light_reg_elem.first->id()), planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( @@ -163,6 +185,37 @@ bool TrafficLightModuleManager::hasSameTrafficLight( return false; } +void TrafficLightModuleManager::updateV2IRestTimeInfo() +{ + if (!v2i_subscriber_) { + return; + } + auto msg = v2i_subscriber_->takeData(); + if (!msg) { + return; + } + + std::vector traffic_light_ids; + traffic_light_id_to_rest_time_map_.clear(); + for (const auto & car_light : msg->car_lights) { + for (const auto & state : car_light.states) { + traffic_light_id_to_rest_time_map_[state.traffic_light_group_id] = { + msg->header.stamp, car_light.min_rest_time_to_red}; + traffic_light_ids.push_back(state.traffic_light_group_id); + } + } +} + +std::optional TrafficLightModuleManager::getV2IRestTimeToRedSignal( + const lanelet::Id & id) const +{ + const auto rest_time_to_red_signal = traffic_light_id_to_rest_time_map_.find(id); + if (rest_time_to_red_signal == traffic_light_id_to_rest_time_map_.end()) { + return std::nullopt; + } + return rest_time_to_red_signal->second; +} + } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index 5ac32d1107880..e2ecedfdf31de 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -20,11 +20,14 @@ #include #include #include +#include #include +#include #include #include +#include #include #include @@ -55,6 +58,16 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr element, const lanelet::TrafficLightConstPtr registered_element) const; + void updateV2IRestTimeInfo(); + + std::optional getV2IRestTimeToRedSignal( + const lanelet::Id & id) const; + + // V2I + autoware::universe_utils::InterProcessPollingSubscriber< + jpn_signal_v2i_msgs::msg::TrafficLightInfo>::SharedPtr v2i_subscriber_; + std::map traffic_light_id_to_rest_time_map_; + // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; 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 3a8692e9c53dd..78eb824603340 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 @@ -19,13 +19,17 @@ #include #include #include +#include +#include #include #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -45,6 +49,8 @@ TrafficLightModule::TrafficLightModule( lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, + const std::function(void)> & + get_rest_time_to_red_signal, const std::shared_ptr planning_factor_interface) : SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), @@ -53,7 +59,8 @@ TrafficLightModule::TrafficLightModule( lane_(lane), state_(State::APPROACH), debug_data_(), - is_prev_state_stop_(false) + is_prev_state_stop_(false), + get_rest_time_to_red_signal_(get_rest_time_to_red_signal) { planner_param_ = planner_param; } @@ -110,6 +117,16 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path) // Check if stop is coming. const bool is_stop_signal = isStopSignal(); + // Use V2I if available + if (planner_param_.v2i_use_rest_time && !is_stop_signal) { + bool is_v2i_handled = handleV2I(signed_arc_length_to_stop_point, [&]() { + *path = insertStopPose(input_path, stop_line.value().first, stop_line.value().second); + }); + if (is_v2i_handled) { + return true; + } + } + // Update stop signal received time if (is_stop_signal) { if (!stop_signal_received_time_ptr_) { @@ -186,6 +203,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_light_group_id = 0; return; } @@ -213,7 +233,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const delay_response_time); const bool distance_stoppable = pass_judge_line_distance < signed_arc_length; - const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; + const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 1.0; const bool stoppable = distance_stoppable || slow_velocity; const bool reachable = signed_arc_length < reachable_distance; @@ -302,4 +322,44 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( return modified_path; } +bool TrafficLightModule::handleV2I( + const double & signed_arc_length_to_stop_point, const std::function & insert_stop_pose) +{ + std::optional rest_time_to_red_signal = + get_rest_time_to_red_signal_(); + + if (!rest_time_to_red_signal) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, + "Failed to get V2I rest time to red signal. traffic_light_lane_id: %ld", lane_id_); + return false; + } + + auto time_diff = (clock_->now() - rest_time_to_red_signal->stamp).seconds(); + if (time_diff > planner_param_.tl_state_timeout) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "V2I data is timeout. traffic_light_lane_id: %ld, time diff: %f", + lane_id_, time_diff); + return false; + } + + const double rest_time_allowed_to_go_ahead = + 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; + + const 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); + + setSafe(!should_stop); + if (isActivated()) { + is_prev_state_stop_ = false; + return true; + } + insert_stop_pose(); + is_prev_state_stop_ = true; + return true; +} } // namespace autoware::behavior_velocity_planner 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 71f0855a4af6f..c1577a95f6536 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 @@ -15,6 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ +#include #include #include #include @@ -33,6 +34,13 @@ namespace autoware::behavior_velocity_planner { + +struct TrafficSignalTimeToRedStamped +{ + builtin_interfaces::msg::Time stamp; + double time_to_red{}; +}; + class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: @@ -63,6 +71,11 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC double yellow_lamp_period; double stop_time_hysteresis; bool enable_pass_judge; + // V2I Parameter + bool v2i_use_rest_time; + double v2i_last_time_allowed_to_pass; + double v2i_velocity_threshold; + double v2i_required_time_to_departure; }; public: @@ -71,6 +84,8 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, const std::shared_ptr time_keeper, + const std::function(void)> & + get_rest_time_to_red_signal, const std::shared_ptr planning_factor_interface); @@ -103,6 +118,15 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC void updateTrafficSignal(); + /** + * @brief Handle V2I Rest Time to Red Signal + * @param signed_arc_length_to_stop_point signed arc length to stop point + * @param output_path output path + * @return true if V2I is handled + */ + bool handleV2I( + const double & signed_arc_length_to_stop_point, const std::function & insert_stop_pose); + // Lane id const int64_t lane_id_; @@ -131,6 +155,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC // Traffic Light State TrafficSignal looking_tl_state_; + + // V2I + std::function(void)> get_rest_time_to_red_signal_; }; } // namespace autoware::behavior_velocity_planner