feat(behavior_path_planner): subscribe traffic light recognition resu…

…lt (autowarefoundation#5436)

feat(avoidance): use traffic light signal info

Signed-off-by: satoshi-ota <[email protected]>

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

@@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs):
                 "~/input/costmap",
                 "/planning/scenario_planning/parking/costmap_generator/occupancy_grid",
             ),
+            (
+                "~/input/traffic_signals",
+                "/perception/traffic_light_recognition/traffic_signals",
+            ),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/accel", "/localization/acceleration"),
             ("~/input/scenario", "/planning/scenario_planning/scenario"),

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

@@ -8,6 +8,7 @@
     <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
     <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
     <remap from="~/input/occupancy_grid_map" to="/perception/occupancy_grid_map/map"/>
+    <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
     <remap from="~/input/accel" to="/localization/acceleration"/>
     <remap from="~/output/path" to="path_with_lane_id"/>

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -2,6 +2,7 @@
   ros__parameters:
     verbose: false
     max_iteration_num: 100
+    traffic_light_signal_timeout: 1.0
     planning_hz: 10.0
     backward_path_length: 5.0
     forward_path_length: 300.0

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -58,6 +58,7 @@ using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_perception_msgs::msg::TrafficSignalArray;
 using autoware_planning_msgs::msg::LaneletRoute;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using nav_msgs::msg::OccupancyGrid;
@@ -93,6 +94,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr costmap_subscriber_;
+  rclcpp::Subscription<TrafficSignalArray>::SharedPtr traffic_signals_subscriber_;
   rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
@@ -136,6 +138,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onPerception(const PredictedObjects::ConstSharedPtr msg);
   void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
   void onCostMap(const OccupancyGrid::ConstSharedPtr msg);
+  void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg);
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
   void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
   void onOperationMode(const OperationModeState::ConstSharedPtr msg);

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -20,6 +20,7 @@
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 
+#include <lanelet2_extension/regulatory_elements/Forward.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 
@@ -28,6 +29,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -39,6 +41,7 @@
 #include <lanelet2_core/primitives/Lanelet.h>
 
 #include <limits>
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -51,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_perception_msgs::msg::TrafficSignal;
 using autoware_planning_msgs::msg::PoseWithUuidStamped;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::PoseStamped;
@@ -59,8 +63,15 @@ using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
 using tier4_planning_msgs::msg::LateralOffset;
 using PlanResult = PathWithLaneId::SharedPtr;
+using lanelet::TrafficLight;
 using unique_identifier_msgs::msg::UUID;
 
+struct TrafficSignalStamped
+{
+  builtin_interfaces::msg::Time stamp;
+  TrafficSignal signal;
+};
+
 struct BoolStamped
 {
   explicit BoolStamped(bool in_data) : data(in_data) {}
@@ -145,6 +156,7 @@ struct PlannerData
   std::optional<PoseWithUuidStamped> prev_modified_goal{};
   std::optional<UUID> prev_route_id{};
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
+  std::map<int, TrafficSignalStamped> traffic_light_id_map;
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
 
@@ -162,6 +174,21 @@ struct PlannerData
       route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data);
   }
 
+  std::optional<TrafficSignalStamped> getTrafficSignal(const int id) const
+  {
+    if (traffic_light_id_map.count(id) == 0) {
+      return std::nullopt;
+    }
+
+    const auto elapsed_time =
+      (rclcpp::Clock{RCL_ROS_TIME}.now() - traffic_light_id_map.at(id).stamp).seconds();
+    if (elapsed_time > parameters.traffic_light_signal_timeout) {
+      return std::nullopt;
+    }
+
+    return traffic_light_id_map.at(id);
+  }
+
   template <class T>
   size_t findEgoIndex(const std::vector<T> & points) const
   {

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -77,6 +77,7 @@ struct BehaviorPathPlannerParameters
 {
   bool verbose;
   size_t max_iteration_num{100};
+  double traffic_light_signal_timeout{1.0};
 
   ModuleConfigParameters config_avoidance;
   ModuleConfigParameters config_avoidance_by_lc;

planning/behavior_path_planner/package.xml

@@ -46,6 +46,7 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_tf2</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>behaviortree_cpp_v3</depend>
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -106,6 +106,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   costmap_subscriber_ = create_subscription<OccupancyGrid>(
     "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1),
     createSubscriptionOptions(this));
+  traffic_signals_subscriber_ =
+    this->create_subscription<autoware_perception_msgs::msg::TrafficSignalArray>(
+      "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1),
+      createSubscriptionOptions(this));
   lateral_offset_subscriber_ = this->create_subscription<LateralOffset>(
     "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1),
     createSubscriptionOptions(this));
@@ -277,6 +281,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 
   p.verbose = declare_parameter<bool>("verbose");
   p.max_iteration_num = declare_parameter<int>("max_iteration_num");
+  p.traffic_light_signal_timeout = declare_parameter<double>("traffic_light_signal_timeout");
 
   const auto get_scene_module_manager_param = [&](std::string && ns) {
     ModuleConfigParameters config;
@@ -934,6 +939,17 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg)
   const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->costmap = msg;
 }
+void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_pd_);
+
+  for (const auto & signal : msg->signals) {
+    TrafficSignalStamped traffic_signal;
+    traffic_signal.stamp = msg->stamp;
+    traffic_signal.signal = signal;
+    planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal;
+  }
+}
 void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 {
   const std::lock_guard<std::mutex> lock(mutex_map_);