feat(map based prediction, crosswalk)!: transplantation of pedestrians' behavior prediction against green signal

perception/map_based_prediction/README.md

@@ -191,6 +191,11 @@ This module treats **Pedestrians** and **Bicycles** as objects using the crosswa
 
 If there are a reachable crosswalk entry points within the `prediction_time_horizon` and the objects satisfies above condition, this module outputs additional predicted path to cross the opposite side via the crosswalk entry point.
 
+This module takes into account the corresponding traffic light information.
+When RED signal is indicated, we assume the target object will not walk across.
+In additon, if the target object is stopping (not moving) against GREEN signal, we assume the target object will not walk across either.
+This prediction comes from the assumption that the object should move if the traffic light is green and the object is intended to cross.
+
 <div align="center">
   <img src="images/outside_road.svg" width=90%>
 </div>
@@ -205,10 +210,11 @@ If the target object is inside the road or crosswalk, this module outputs one or
 
 ### Input
 
-| Name                                               | Type                                                 | Description                              |
-| -------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------- |
-| `~/perception/object_recognition/tracking/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path. |
-| `~/vector_map`                                     | `autoware_auto_mapping_msgs::msg::HADMapBin`         | binary data of Lanelet2 Map.             |
+| Name                                                     | Type                                                 | Description                                                |
+| -------------------------------------------------------- | ---------------------------------------------------- | ---------------------------------------------------------- |
+| `~/perception/object_recognition/tracking/objects`       | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects without predicted path.                   |
+| `~/vector_map`                                           | `autoware_auto_mapping_msgs::msg::HADMapBin`         | binary data of Lanelet2 Map.                               |
+| '~/perception/traffic_light_recognition/traffic_signals' | 'autoware_perception_msgs::msg::TrafficSignalArray;' | rearranged information on the corresponding traffic lights |
 
 ### Output
 

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -19,7 +19,12 @@
     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
+    crosswalk_with_signal:
+      use_crosswalk_signal: true
+      threshold_velocity_assumed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped
+      # If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk.
+      distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map
+      timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map
     # 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

@@ -42,6 +42,7 @@
 
 #include <algorithm>
 #include <deque>
+#include <map>
 #include <memory>
 #include <optional>
 #include <string>
@@ -131,6 +132,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
+  std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
 
   // Lanelet Map Pointers
   std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -190,6 +192,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   double acceleration_exponential_half_life_;
 
   bool use_crosswalk_signal_;
+  double threshold_velocity_assumed_as_stopping_;
+  std::vector<double> distance_set_for_no_intention_to_walk_;
+  std::vector<double> timeout_set_for_no_intention_to_walk_;
 
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
@@ -214,7 +219,8 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);
 
-  void removeOldObjectsHistory(const double current_time);
+  void removeOldObjectsHistory(
+    const double current_time, const TrackedObjects::ConstSharedPtr in_objects);
 
   LaneletsData getCurrentLanelets(const TrackedObject & object);
   bool checkCloseLaneletCondition(
@@ -262,6 +268,9 @@ class MapBasedPredictionNode : public rclcpp::Node
     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);
+  bool calcIntentionToCrossWithTrafficSgnal(
+    const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
+    const lanelet::Id & signal_id);
 
   visualization_msgs::msg::Marker getDebugMarker(
     const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -788,7 +788,13 @@
   acceleration_exponential_half_life_ =
     declare_parameter<double>("acceleration_exponential_half_life");
 
-  use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");
+  use_crosswalk_signal_ = declare_parameter<bool>("crosswalk_with_signal.use_crosswalk_signal");
+  threshold_velocity_assumed_as_stopping_ =
+    declare_parameter<double>("crosswalk_with_signal.threshold_velocity_assumed_as_stopping");
+  distance_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
+    "crosswalk_with_signal.distance_set_for_no_intention_to_walk");
+  timeout_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
+    "crosswalk_with_signal.timeout_set_for_no_intention_to_walk");
 
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
@@ -912,7 +918,7 @@
 
   // Remove old objects information in object history
   const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
-  removeOldObjectsHistory(objects_detected_time);
+  removeOldObjectsHistory(objects_detected_time, in_objects);
 
   // result output
   PredictedObjects output;
@@ -1229,16 +1235,13 @@
       }
     }
   }
+
   // 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) {
+      if (!calcIntentionToCrossWithTrafficSgnal(
+            object, crosswalk, crosswalk_signal_id_opt.value())) {
         continue;
       }
     }
@@ -1330,7 +1333,8 @@
   return;
 }
 
-void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time)
+void MapBasedPredictionNode::removeOldObjectsHistory(
+  const double current_time, const TrackedObjects::ConstSharedPtr in_objects)
 {
   std::vector<std::string> invalid_object_id;
   for (auto iter = objects_history_.begin(); iter != objects_history_.end(); ++iter) {
@@ -1371,6 +1375,19 @@
   for (const auto & key : invalid_object_id) {
     objects_history_.erase(key);
   }
+
+  for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) {
+    const bool isDisappeared = std::none_of(
+      in_objects->objects.begin(), in_objects->objects.end(),
+      [&it](autoware_auto_perception_msgs::msg::TrackedObject obj) {
+        return tier4_autoware_utils::toHexString(obj.object_id) == it->first.first;
+      });
+    if (isDisappeared) {
+      it = stopped_times_against_green_.erase(it);
+    } else {
+      ++it;
+    }
+  }
 }
 
 LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object)
@@ -2268,6 +2285,69 @@
   return std::nullopt;
 }
 
+bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSgnal(
+  const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
+  const lanelet::Id & signal_id)
+{
+  const auto signal_color = [&] {
+    const auto elem_opt = getTrafficSignalElement(signal_id);
+    return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN;
+  }();
+
+  const auto key = std::make_pair(tier4_autoware_utils::toHexString(object.object_id), signal_id);
+  if (
+    signal_color == TrafficSignalElement::GREEN &&
+    tier4_autoware_utils::calcNorm(object.kinematics.twist_with_covariance.twist.linear) <
+      threshold_velocity_assumed_as_stopping_) {
+    stopped_times_against_green_.try_emplace(key, this->get_clock()->now());
+
+    const auto timeout_no_intention_to_walk = [&]() {
+      auto InterpolateMap = [](
+                              const std::vector<double> & key_set,
+                              const std::vector<double> & value_set, const double query) {
+        if (query <= key_set.front()) {
+          return value_set.front();
+        } else if (query >= key_set.back()) {
+          return value_set.back();
+        }
+        for (size_t i = 0; i < key_set.size() - 1; ++i) {
+          if (key_set.at(i) <= query && query <= key_set.at(i + 1)) {
+            auto ratio =
+              (query - key_set.at(i)) / std::max(key_set.at(i + 1) - key_set.at(i), 1.0e-5);
+            ratio = std::clamp(ratio, 0.0, 1.0);
+            return value_set.at(i) + ratio * (value_set.at(i + 1) - value_set.at(i));
+          }
+        }
+        return value_set.back();
+      };
+
+      const auto obj_position = object.kinematics.pose_with_covariance.pose.position;
+      const double distance_to_crosswalk = boost::geometry::distance(
+        crosswalk.polygon2d().basicPolygon(),
+        lanelet::BasicPoint2d(obj_position.x, obj_position.y));
+      return InterpolateMap(
+        distance_set_for_no_intention_to_walk_, timeout_set_for_no_intention_to_walk_,
+        distance_to_crosswalk);
+    }();
+
+    if (
+      (this->get_clock()->now() - stopped_times_against_green_.at(key)).seconds() >
+      timeout_no_intention_to_walk) {
+      return false;
+    }
+
+  } else {
+    stopped_times_against_green_.erase(key);
+    // If the pedestrian disappears, another function erases the old data.
+  }
+
+  if (signal_color == TrafficSignalElement::RED) {
+    return false;
+  }
+
+  return true;
+}
+
 }  // namespace map_based_prediction
 
 #include <rclcpp_components/register_node_macro.hpp>