feat(map_based_prediction): consider crosswalks signals (autowarefoun…

…dation#6189)

* consider the crosswalks signals
* update with the reviewers comments

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -19,6 +19,7 @@
     use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
     speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
     acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
+    use_crosswalk_signal: true
     # parameter for shoulder lane prediction
     prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -29,6 +29,7 @@
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -42,6 +43,7 @@
 #include <algorithm>
 #include <deque>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -107,6 +109,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::StringStamped;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
@@ -122,6 +127,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
 
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
@@ -131,6 +137,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
   std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;
 
+  std::unordered_map<lanelet::Id, TrafficSignal> traffic_signal_id_map_;
+
   // parameter update
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
   rcl_interfaces::msg::SetParametersResult onParam(
@@ -181,11 +189,14 @@ class MapBasedPredictionNode : public rclcpp::Node
   double speed_limit_multiplier_;
   double acceleration_exponential_half_life_;
 
+  bool use_crosswalk_signal_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
+  void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);
   void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
 
   bool doesPathCrossAnyFence(const PredictedPath & predicted_path);
@@ -249,6 +260,8 @@ class MapBasedPredictionNode : public rclcpp::Node
     const LaneletsData & lanelets_data);
   bool isDuplicated(
     const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
+  std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
+  std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);
 
   visualization_msgs::msg::Marker getDebugMarker(
     const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);

perception/map_based_prediction/launch/map_based_prediction.launch.xml

@@ -3,12 +3,14 @@
   <arg name="param_path" default="$(find-pkg-share map_based_prediction)/config/map_based_prediction.param.yaml"/>
 
   <arg name="vector_map_topic" default="/map/vector_map"/>
+  <arg name="traffic_signals_topic" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="output_topic" default="objects"/>
   <arg name="input_topic" default="/perception/object_recognition/tracking/objects"/>
 
   <node pkg="map_based_prediction" exec="map_based_prediction" name="map_based_prediction" output="screen">
     <param from="$(var param_path)"/>
     <remap from="/vector_map" to="$(var vector_map_topic)"/>
+    <remap from="/traffic_signals" to="$(var traffic_signals_topic)"/>
     <remap from="~/output/objects" to="$(var output_topic)"/>
     <remap from="~/input/objects" to="$(var input_topic)"/>
   </node>

perception/map_based_prediction/package.xml

@@ -16,6 +16,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libgoogle-glog-dev</depend>

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -788,6 +788,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   acceleration_exponential_half_life_ =
     declare_parameter<double>("acceleration_exponential_half_life");
 
+  use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");
+
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
     min_crosswalk_user_velocity_);
@@ -801,6 +803,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   sub_map_ = this->create_subscription<HADMapBin>(
     "/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1));
+  sub_traffic_signals_ = this->create_subscription<TrafficSignalArray>(
+    "/traffic_signals", 1,
+    std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1));
 
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
   pub_debug_markers_ =
@@ -872,6 +877,14 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg)
   crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end());
 }
 
+void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg)
+{
+  traffic_signal_id_map_.clear();
+  for (const auto & signal : msg->signals) {
+    traffic_signal_id_map_[signal.traffic_signal_id] = signal;
+  }
+}
+
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
   stop_watch_.tic();
@@ -1218,6 +1231,18 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   }
   // try to find the edge points for all crosswalks and generate path to the crosswalk edge
   for (const auto & crosswalk : crosswalks_) {
+    const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
+    if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
+      const auto signal_color = [&] {
+        const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value());
+        return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN;
+      }();
+
+      if (signal_color == TrafficSignalElement::RED) {
+        continue;
+      }
+    }
+
     const auto edge_points = getCrosswalkEdgePoints(crosswalk);
 
     const auto reachable_first = hasPotentialToReach(
@@ -2211,6 +2236,38 @@ bool MapBasedPredictionNode::isDuplicated(
 
   return false;
 }
+
+std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
+  const lanelet::ConstLanelet & way_lanelet)
+{
+  const auto traffic_light_reg_elems =
+    way_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
+  if (traffic_light_reg_elems.empty()) {
+    return std::nullopt;
+  } else if (traffic_light_reg_elems.size() > 1) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "[Map Based Prediction]: "
+      "Multiple regulatory elements as TrafficLight are defined to one lanelet object.");
+  }
+  return traffic_light_reg_elems.front()->id();
+}
+
+std::optional<TrafficSignalElement> MapBasedPredictionNode::getTrafficSignalElement(
+  const lanelet::Id & id)
+{
+  if (traffic_signal_id_map_.count(id) != 0) {
+    const auto & signal_elements = traffic_signal_id_map_.at(id).elements;
+    if (signal_elements.size() > 1) {
+      RCLCPP_ERROR(
+        get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received.");
+    } else if (!signal_elements.empty()) {
+      return signal_elements.front();
+    }
+  }
+  return std::nullopt;
+}
+
 }  // namespace map_based_prediction
 
 #include <rclcpp_components/register_node_macro.hpp>