enable to control lateral path convergence speed by setting control t…

…ime horizon Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Oct 12, 2023 · 6938abf · 6938abf
1 parent 84995ec
commit 6938abf
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 8 deletions.
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/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]

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/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_;

diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/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_;
 

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -697,6 +697,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
 {
   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");
@@ -743,7 +745,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     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,

diff --git a/perception/map_based_prediction/src/path_generator.cpp b/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,16 +214,20 @@ FrenetPath PathGenerator::generateFrenetPath(
 {
   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 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);
+    // 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 * std::pow(t, 2) +
                           lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4);
     if (s_next > max_length) {