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 <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <[email protected]>
Signed-off-by: Tomohito Ando <[email protected]>

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

@@ -226,6 +226,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="$(var input_traffic_light_topic_name)"/>
+      <remap from="~/input/traffic_signals_raw_v2i" to="/v2i/external/v2i_traffic_light_info"/>
       <remap from="~/input/virtual_traffic_light_states" to="$(var input_virtual_traffic_light_topic_name)"/>
       <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
       <remap from="~/output/path" to="path"/>

planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -43,6 +43,7 @@
     <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
     <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="~/input/traffic_signals_raw_v2i" to="/v2i/external/v2i_traffic_light_info"/>
     <remap from="~/output/path" to="path"/>
     <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
     <remap from="~/output/infrastructure_commands" to="/planning/scenario_planning/status/infrastructure_commands"/>

planning/behavior_velocity_planner/package.xml

@@ -41,6 +41,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>jpn_signal_v2i_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libboost-dev</depend>
   <depend>motion_utils</depend>

planning/behavior_velocity_planner/src/node.cpp

@@ -108,6 +108,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<jpn_signal_v2i_msgs::msg::TrafficLightInfo>(
+      "~/input/traffic_signals_raw_v2i", 1,
+      std::bind(&BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I, this, _1),
+      createSubscriptionOptions(this));
   sub_external_velocity_limit_ = this->create_subscription<VelocityLimit>(
     "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(),
     std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1));
@@ -330,6 +335,19 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
   }
 }
 
+void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I(
+  const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg)
+{
+  std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);

planning/behavior_velocity_planner/src/node.hpp

@@ -27,6 +27,8 @@
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <jpn_signal_v2i_msgs/msg/extra_traffic_signal.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>
@@ -71,6 +73,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_map_;
   rclcpp::Subscription<autoware_perception_msgs::msg::TrafficSignalArray>::SharedPtr
     sub_traffic_signals_;
+  rclcpp::Subscription<jpn_signal_v2i_msgs::msg::TrafficLightInfo>::SharedPtr
+    sub_traffic_signals_raw_v2i_;
   rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
     sub_virtual_traffic_light_states_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_occupancy_grid_;
@@ -86,6 +90,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);

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -40,6 +40,7 @@
 
 #include <algorithm>
 #include <deque>
+#include <limits>
 #include <map>
 #include <memory>
 #include <optional>
@@ -78,6 +79,7 @@ struct PlannerData
   // other internal data
   std::map<int, TrafficSignalStamped> traffic_light_id_map;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
+  std::map<int, double> traffic_light_time_to_red_id_map;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
   // velocity smoother
@@ -132,6 +134,15 @@ struct PlannerData
     }
     return std::make_shared<TrafficSignalStamped>(traffic_light_id_map.at(id));
   }
+
+  std::optional<double> 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
 

planning/behavior_velocity_planner_common/package.xml

@@ -28,6 +28,7 @@
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
+  <depend>jpn_signal_v2i_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>motion_utils</depend>
   <depend>motion_velocity_smoother</depend>

planning/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_traffic_light_module/src/manager.cpp

@@ -43,6 +43,14 @@ 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::TrafficSignal>(
     "~/output/traffic_signal", 1);
 }

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -228,6 +228,25 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
     first_ref_stop_path_point_index_ = stop_line_point_idx;
 
+    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;
+      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.
     const bool is_stop_signal = isStopSignal();
 

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -63,6 +63,10 @@ class TrafficLightModule : public SceneModuleInterface
     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: