Merge pull request #1150 from tier4/use-v2i-in-planning

feat(behavior_velocity_planner.traffic_light): use v2i in planning

Author: TomohitoAndo

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

@@ -228,6 +228,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,21 @@ 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) {
+      TrafficSignalTimeToRedStamped time_to_red;
+      time_to_red.stamp = msg->header.stamp;
+      time_to_red.time_to_red = car_light.min_rest_time_to_red;
+      planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = 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, TrafficSignalTimeToRedStamped> 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<TrafficSignalTimeToRedStamped> 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/include/behavior_velocity_planner_common/utilization/util.hpp

@@ -56,6 +56,12 @@ struct TrafficSignalStamped
   autoware_perception_msgs::msg::TrafficSignal signal;
 };
 
+struct TrafficSignalTimeToRedStamped
+{
+  builtin_interfaces::msg::Time stamp;
+  double time_to_red;
+};
+
 using Pose = geometry_msgs::msg::Pose;
 using Point2d = tier4_autoware_utils::Point2d;
 using LineString2d = tier4_autoware_utils::LineString2d;

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

@@ -231,6 +231,28 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     // Check if stop is coming.
     const bool is_stop_signal = isStopSignal();
 
+    // Decide if stop or proceed using the remaining time to red signal
+    const auto 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 &&
+      !isDataTimeout(rest_time_to_red_signal->stamp)) {
+      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;
+      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;
+    }
+
     // Update stop signal received time
     if (is_stop_signal) {
       if (!stop_signal_received_time_ptr_) {
@@ -298,6 +320,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_signal_id = 0;
     return;
   }
 
@@ -476,4 +501,17 @@ bool TrafficLightModule::hasTrafficLightShape(
   return it_lamp != tl_state.elements.end();
 }
 
+bool TrafficLightModule::isDataTimeout(const rclcpp::Time & data_time) const
+{
+  const auto now = clock_->now();
+  const bool is_data_timeout = (now - data_time).seconds() > planner_param_.tl_state_timeout;
+  if (is_data_timeout) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      logger_, *clock_, 5000 /* ms */,
+      "data is timeout. time diff: " << (now - data_time).seconds());
+  }
+
+  return is_data_timeout;
+}
+
 }  // namespace behavior_velocity_planner

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:
@@ -108,6 +112,8 @@ class TrafficLightModule : public SceneModuleInterface
 
   void updateTrafficSignal();
 
+  bool isDataTimeout(const rclcpp::Time & data_time) const;
+
   // Lane id
   const int64_t lane_id_;