add v2i to behavior_velocity_traffic_light

Signed-off-by: Y.Hisaki <[email protected]>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -221,6 +221,7 @@
       <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity_default"/>
       <remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
       <remap from="~/input/virtual_traffic_light_states" to="$(var input_virtual_traffic_light_topic_name)"/>
+      <remap from="~/input/traffic_signals_raw_v2i" to="/v2i/external/v2i_traffic_light_info"/>
       <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
       <remap from="~/output/path" to="path"/>
       <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp

@@ -27,6 +27,7 @@
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
+#include <jpn_signal_v2i_msgs/msg/traffic_light_info.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -81,6 +82,15 @@ 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<nav_msgs::msg::OccupancyGrid>
     sub_occupancy_grid_{this, "~/input/occupancy_grid"};
 
@@ -100,6 +110,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

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -40,6 +40,7 @@
     <remap from="~/input/vector_map_inside_area_filtered_pointcloud" to="vector_map_inside_area_filtered/pointcloud"/>
     <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity_default"/>
     <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
+    <remap from="~/input/traffic_signals_raw_v2i" to="/v2i/external/v2i_traffic_light_info"/>
     <remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
     <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
     <remap from="~/output/path" to="path"/>

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml

@@ -45,6 +45,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>jpn_signal_v2i_msgs</depend>
   <depend>libboost-dev</depend>
   <depend>pcl_conversions</depend>
   <depend>pluginlib</depend>

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;
 }
 

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<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
+  std::map<lanelet::Id, double> traffic_light_time_to_red_id_map_;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
 
   bool is_simulation = false;
@@ -80,6 +81,8 @@ struct PlannerData
 
   std::optional<TrafficSignalStamped> getTrafficSignal(
     const lanelet::Id id, const bool keep_last_observation = false) const;
+
+  std::optional<double> getRestTimeToRedSignal(const lanelet::Id & id) const;
 };
 }  // namespace autoware::behavior_velocity_planner
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/planner_data.cpp

@@ -69,4 +69,14 @@ std::optional<TrafficSignalStamped> PlannerData::getTrafficSignal(
   }
   return std::make_optional<TrafficSignalStamped>(traffic_light_id_map.at(id));
 }
+
+std::optional<double> 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

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

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<bool>(node, ns + ".enable_pass_judge");
   planner_param_.yellow_lamp_period =
     getOrDeclareParameter<double>(node, ns + ".yellow_lamp_period");
+  planner_param_.v2i_use_rest_time = getOrDeclareParameter<bool>(node, ns + ".v2i.use_rest_time");
+  planner_param_.v2i_last_time_allowed_to_pass =
+    getOrDeclareParameter<double>(node, ns + ".v2i.last_time_allowed_to_pass");
+  planner_param_.v2i_velocity_threshold =
+    getOrDeclareParameter<double>(node, ns + ".v2i.velocity_threshold");
+  planner_param_.v2i_required_time_to_departure =
+    getOrDeclareParameter<double>(node, ns + ".v2i.required_time_to_departure");
   pub_tl_state_ = node.create_publisher<autoware_perception_msgs::msg::TrafficLightGroup>(
     "~/output/traffic_signal", 1);
 }

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<double> 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();
 

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: