feat(map_based_prediction): enable to control lateral path convergence speed by setting control time horizon

perception/map_based_prediction/README.md

@@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity.
 
 Currently, we use the upper method with a low-pass filter to calculate lateral velocity.
 
+### Path generation
+
+Path generation is generated on the frenet frame. The path is generated by the following steps:
+
+1. Get the frenet frame of the reference path.
+2. Generate the frenet frame of the current position of the object and the finite position of the object.
+3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.)
+4. Convert the path to the global coordinate.
+
+See paper [2] for more details.
+
+#### Tuning lateral path shape
+
+`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.)
+
 ### 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:
@@ -155,6 +170,7 @@ If the target object is inside the road or crosswalk, this module outputs one or
 | ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- |
 | `enable_delay_compensation`                                      | [-]   | bool   | flag to enable the time delay compensation for the position of the object                                                             |
 | `prediction_time_horizon`                                        | [s]   | double | predict time duration for predicted path                                                                                              |
+| `lateral_control_time_horizon`                                   | [s]   | double | time duration for predicted path will reach the reference path (mostly center of the lane)                                            |
 | `prediction_sampling_delta_time`                                 | [s]   | double | sampling time for points in predicted path                                                                                            |
 | `min_velocity_for_map_based_prediction`                          | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value                                                        |
 | `min_crosswalk_user_velocity`                                    | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated                                                                    |

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -2,6 +2,7 @@
   ros__parameters:
     enable_delay_compensation: true
     prediction_time_horizon: 10.0 #[s]
+    lateral_control_time_horizon: 5.0 #[s]
     prediction_sampling_delta_time: 0.5 #[s]
     min_velocity_for_map_based_prediction: 1.39 #[m/s]
     min_crosswalk_user_velocity: 1.39 #[m/s]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -135,6 +135,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Parameters
   bool enable_delay_compensation_;
   double prediction_time_horizon_;
+  double lateral_control_time_horizon_;
   double prediction_time_horizon_rate_for_validate_lane_length_;
   double prediction_sampling_time_interval_;
   double min_velocity_for_map_based_prediction_;

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -81,8 +81,8 @@ class PathGenerator
 {
 public:
   PathGenerator(
-    const double time_horizon, const double sampling_time_interval,
-    const double min_crosswalk_user_velocity);
+    const double time_horizon, const double lateral_time_horizon,
+    const double sampling_time_interval, const double min_crosswalk_user_velocity);
 
   PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);
 
@@ -102,6 +102,7 @@ class PathGenerator
 private:
   // Parameters
   double time_horizon_;
+  double lateral_time_horizon_;
   double sampling_time_interval_;
   double min_crosswalk_user_velocity_;
 

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.
@@ -694,6 +694,8 @@
 {
   enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
   prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
+  lateral_control_time_horizon_ =
+    declare_parameter<double>("lateral_control_time_horizon");  // [s] for lateral control point
   prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
   min_velocity_for_map_based_prediction_ =
     declare_parameter<double>("min_velocity_for_map_based_prediction");
@@ -740,7 +742,8 @@
     declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
 
   path_generator_ = std::make_shared<PathGenerator>(
-    prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
+    prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
+    min_crosswalk_user_velocity_);
 
   sub_objects_ = this->create_subscription<TrackedObjects>(
     "~/input/objects", 1,

perception/map_based_prediction/src/path_generator.cpp

@@ -23,9 +23,10 @@
 namespace map_based_prediction
 {
 PathGenerator::PathGenerator(
-  const double time_horizon, const double sampling_time_interval,
+  const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval,
   const double min_crosswalk_user_velocity)
 : time_horizon_(time_horizon),
+  lateral_time_horizon_(lateral_time_horizon),
   sampling_time_interval_(sampling_time_interval),
   min_crosswalk_user_velocity_(min_crosswalk_user_velocity)
 {
@@ -213,18 +214,25 @@
 {
   FrenetPath path;
   const double duration = time_horizon_;
+  const double lateral_duration = lateral_time_horizon_;
 
   // Compute Lateral and Longitudinal Coefficients to generate the trajectory
-  const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration);
+  const Eigen::Vector3d lat_coeff =
+    calcLatCoefficients(current_point, target_point, lateral_duration);
   const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration);
 
   path.reserve(static_cast<size_t>(duration / sampling_time_interval_));
   for (double t = 0.0; t <= duration; t += sampling_time_interval_) {
-    const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) +
-                          lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) +
-                          lat_coeff(2) * std::pow(t, 5);
-    const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) +
-                          lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4);
+    const double current_acc =
+      0.0;  // Currently we assume the object is traveling at a constant speed
+    const double d_next_ = current_point.d + current_point.d_vel * t +
+                           current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) +
+                           lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5);
+    // t > lateral_duration: 0.0, else d_next_
+    const double d_next = t > lateral_duration ? 0.0 : d_next_;
+    const double s_next = current_point.s + current_point.s_vel * t +
+                          2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) +
+                          lon_coeff(1) * std::pow(t, 4);
     if (s_next > max_length) {
       break;
     }