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 4bc7a15cddc48..cb0291a793c52 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 9a402f31af0e4..9edfc4c060950 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)
@@ -57,6 +74,8 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat
velocity_factor_array.header.frame_id = "map";
velocity_factor_array.header.stamp = clock_->now();
+ 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(
@@ -115,6 +134,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(
@@ -175,6 +197,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] =
+ 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..242fb2f9468aa 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