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]>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -177,6 +177,10 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/traffic_signals",
                 "/perception/traffic_light_recognition/traffic_signals",
             ),
+            (
+                "~/input/traffic_signals_raw_v2i",
+                "/v2i/external/v2i_traffic_light_info",
+            ),
             (
                 "~/input/external_velocity_limit_mps",
                 "/planning/scenario_planning/max_velocity_default",

planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -52,6 +52,7 @@
     <remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>
     <remap from="~/input/virtual_traffic_light_states" to="$(var virtual_traffic_light_topic_name)"/>
     <remap from="~/input/occupancy_grid" to="/sensing/lidar/occupancy_grid"/>
+    <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

@@ -40,6 +40,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

@@ -103,6 +103,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));
@@ -301,6 +306,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

@@ -25,6 +25,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>
@@ -64,6 +66,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_;
@@ -79,6 +83,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

@@ -48,6 +48,7 @@
 
 #include <algorithm>
 #include <deque>
+#include <limits>
 #include <map>
 #include <memory>
 #include <vector>
@@ -84,6 +85,7 @@ struct PlannerData
 
   // other internal data
   std::map<int, TrafficSignalStamped> traffic_light_id_map;
+  std::map<int, double> traffic_light_time_to_red_id_map;
   boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
@@ -139,6 +141,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

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

planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml

@@ -6,3 +6,9 @@
       yellow_lamp_period: 2.75
       enable_pass_judge: true
       enable_rtc: true # 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

@@ -41,6 +41,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

@@ -225,6 +225,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.
     setSafe(!isStopSignal());
     if (isActivated()) {

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -61,6 +61,10 @@ class TrafficLightModule : public SceneModuleInterface
     double tl_state_timeout;
     double yellow_lamp_period;
     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: