consider the crosswalks signals

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

Author: yuki-takagi-66

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(
@@ -186,6 +194,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   // 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 +258,7 @@ 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);
 
   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

@@ -801,6 +801,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 +875,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 +1229,23 @@ 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()) {
+      // If the crosswalk has traffic light, do something.
+      if (traffic_signal_id_map.count(crosswalk_signal_id_opt.value()) != 0) {
+        const auto & signal_elements =
+          traffic_signal_id_map.at(crosswalk_signal_id_opt.value()).elements;
+        if (signal_elements.size() > 1) {
+          RCLCPP_ERROR(
+            get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are recieved.");
+        } else if (
+          !signal_elements.empty() && signal_elements.front().color == TrafficSignalElement::RED) {
+          continue;
+        }
+      }
+      // TODO(yuki takagi): If a flag is setted, wait for timeout while the pedestrian is stopping.
+    }
+
     const auto edge_points = getCrosswalkEdgePoints(crosswalk);
 
     const auto reachable_first = hasPotentialToReach(
@@ -2211,6 +2239,24 @@ 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 std::nullopt;  // Is it appropriate?
+  }
+  return traffic_light_reg_elems.front()->id();
+}
+
 }  // namespace map_based_prediction
 
 #include <rclcpp_components/register_node_macro.hpp>