feat(map_based_prediction): output rich prediction paths based on lan…

…elet map for crosswalk users (tier4#922)

* feat(map_based_prediction): output rich prediction paths for pedestrian and bicycle

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* perf(map_based_prediction): early return in crosswalk iteration

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(map_based_prediction): update horizon time

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(map_based_prediction): remove redundant param

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(map_based_prediction): restore original prediction horizon time

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(map_based_prediction): separate processes of each object class explicitly

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(map_based_prediction): add prediction algorithm for crosswalk user

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* docs(map_based_prediction): update file name Readme -> README

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

perception/map_based_prediction/Readme.md → perception/map_based_prediction/README.md

@@ -6,6 +6,8 @@
 
 ## Inner-workings / Algorithms
 
+### Path prediction for road users
+
 1. Get lanelet path
    The first step is to get the lanelet of the current position of the car. After that, we obtain several trajectories based on the map.
 
@@ -30,6 +32,29 @@
 4. Drawing predicted trajectories
    From the current position and reference trajectories that we get in the step1, we create predicted trajectories by using Quintic polynomial. Note that, since this algorithm consider lateral and longitudinal motions separately, it sometimes generates dynamically-infeasible trajectories when the vehicle travels at a low speed. To deal with this problem, we only make straight line predictions when the vehicle speed is lower than a certain value (which is given as a parameter).
 
+### Path prediction for crosswalk users
+
+This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions:
+
+- move toward the crosswalk
+- stop near the crosswalk
+
+<div align="center">
+  <img src="images/target_objects.svg" width=90%>
+</div>
+
+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.
+
+<div align="center">
+  <img src="images/outside_road.svg" width=90%>
+</div>
+
+If the target object is inside the road or crosswalk, this module outputs one or two additional prediction path(s) to reach exit point of the crosswalk. The number of prediction paths are depend on whether object is moving or not. If the object is moving, this module outputs one prediction path toward an exit point that existed in the direction of object's movement. One the other hand, if the object has stopped, it is impossible to infer which exit points the object want to go, so this module outputs two prediction paths toward both side exit point.
+
+<div align="center">
+  <img src="images/inside_road.svg" width=90%>
+</div>
+
 ## Inputs / Outputs
 
 ### Input

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -3,7 +3,7 @@
     enable_delay_compensation: true
     prediction_time_horizon: 10.0 #[s]
     prediction_sampling_delta_time: 0.5 #[s]
-    min_velocity_for_map_based_prediction: 1.0 #[m/s]
+    min_velocity_for_map_based_prediction: 1.39 #[m/s]
     dist_threshold_for_searching_lanelet: 3.0 #[m]
     delta_yaw_threshold_for_searching_lanelet: 0.785 #[rad]
     sigma_lateral_offset: 0.5 #[m]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -18,9 +18,11 @@
 #include "map_based_prediction/path_generator.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -86,6 +88,7 @@ using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
+using tier4_autoware_utils::StopWatch;
 
 class MapBasedPredictionNode : public rclcpp::Node
 {
@@ -113,6 +116,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Path Generator
   std::shared_ptr<PathGenerator> path_generator_;
 
+  // Crosswalk Entry Points
+  lanelet::ConstLanelets crosswalks_;
+
   // Parameters
   bool enable_delay_compensation_;
   double prediction_time_horizon_;
@@ -131,6 +137,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   double diff_dist_threshold_to_right_bound_;
   double reference_path_resolution_;
 
+  // Stop watch
+  StopWatch<std::chrono::milliseconds> stop_watch_;
+
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
@@ -140,6 +149,8 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   PredictedObject convertToPredictedObject(const TrackedObject & tracked_object);
 
+  PredictedObject getPredictedObjectAsCrosswalkUser(const TrackedObject & object);
+
   void removeOldObjectsHistory(const double current_time);
 
   LaneletsData getCurrentLanelets(const TrackedObject & object);

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -24,6 +24,8 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+#include <memory>
+#include <utility>
 #include <vector>
 
 namespace map_based_prediction
@@ -47,13 +49,16 @@ struct FrenetPoint
   float d_acc;
 };
 
+using EntryPoint = std::pair<Eigen::Vector2d /*in*/, Eigen::Vector2d /*out*/>;
 using FrenetPath = std::vector<FrenetPoint>;
 using PosePath = std::vector<geometry_msgs::msg::Pose>;
 
 class PathGenerator
 {
 public:
-  PathGenerator(const double time_horizon, const double sampling_time_interval);
+  PathGenerator(
+    const double time_horizon, const double sampling_time_interval,
+    const double min_velocity_for_map_based_prediction);
 
   PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);
 
@@ -64,10 +69,17 @@ class PathGenerator
   PredictedPath generatePathForOnLaneVehicle(
     const TrackedObject & object, const PosePath & ref_paths);
 
+  PredictedPath generatePathForCrosswalkUser(
+    const TrackedObject & object, const EntryPoint & reachable_crosswalk) const;
+
+  PredictedPath generatePathToTargetPoint(
+    const TrackedObject & object, const Eigen::Vector2d & point) const;
+
 private:
   // Parameters
   double time_horizon_;
   double sampling_time_interval_;
+  double min_velocity_for_map_based_prediction_;
 
   // Member functions
   PredictedPath generateStraightPath(const TrackedObject & object) const;