fix(map_based_prediction): revert use different time horizon (#6877)

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -1,10 +1,7 @@
 /**:
   ros__parameters:
     enable_delay_compensation: true
-    prediction_time_horizon:
-      vehicle: 15.0 #[s]
-      pedestrian: 10.0 #[s]
-      unknown: 10.0 #[s]
+    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]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -99,13 +99,6 @@ struct PredictedRefPath
   Maneuver maneuver;
 };
 
-struct PredictionTimeHorizon
-{
-  double vehicle;
-  double pedestrian;
-  double unknown;
-};
-
 using LaneletsData = std::vector<LaneletData>;
 using ManeuverProbability = std::unordered_map<Maneuver, float>;
 using autoware_auto_mapping_msgs::msg::HADMapBin;
@@ -168,7 +161,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   // Parameters
   bool enable_delay_compensation_;
-  PredictionTimeHorizon prediction_time_horizon_;
+  double prediction_time_horizon_;
   double lateral_control_time_horizon_;
   double prediction_time_horizon_rate_for_validate_lane_length_;
   double prediction_sampling_time_interval_;
@@ -245,7 +238,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   std::vector<PredictedRefPath> getPredictedReferencePath(
     const TrackedObject & object, const LaneletsData & current_lanelets_data,
-    const double object_detected_time, const double time_horizon);
+    const double object_detected_time);
   Maneuver predictObjectManeuver(
     const TrackedObject & object, const LaneletData & current_lanelet_data,
     const double object_detected_time);

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -80,23 +80,21 @@ using PosePath = std::vector<geometry_msgs::msg::Pose>;
 class PathGenerator
 {
 public:
-  PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity);
+  PathGenerator(
+    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, const double duration);
+  PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);
 
-  PredictedPath generatePathForLowSpeedVehicle(
-    const TrackedObject & object, const double duration) const;
+  PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const;
 
-  PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object, const double duration);
+  PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);
 
   PredictedPath generatePathForOnLaneVehicle(
-    const TrackedObject & object, const PosePath & ref_paths, const double duration,
-    const double lateral_duration, const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0);
 
   PredictedPath generatePathForCrosswalkUser(
-    const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
-    const double duration) const;
+    const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
 
   PredictedPath generatePathToTargetPoint(
     const TrackedObject & object, const Eigen::Vector2d & point) const;
@@ -113,21 +111,21 @@ class PathGenerator
 
 private:
   // Parameters
+  double time_horizon_;
+  double lateral_time_horizon_;
   double sampling_time_interval_;
   double min_crosswalk_user_velocity_;
   bool use_vehicle_acceleration_;
   double acceleration_exponential_half_life_;
 
   // Member functions
-  PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;
+  PredictedPath generateStraightPath(const TrackedObject & object) const;
 
   PredictedPath generatePolynomialPath(
-    const TrackedObject & object, const PosePath & ref_path, const double duration,
-    const double lateral_duration, const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
 
   FrenetPath generateFrenetPath(
-    const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
-    const double duration, const double lateral_duration);
+    const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length);
   Eigen::Vector3d calcLatCoefficients(
     const FrenetPoint & current_point, const FrenetPoint & target_point, const double T);
   Eigen::Vector2d calcLonCoefficients(
@@ -141,8 +139,7 @@ class PathGenerator
     const PosePath & ref_path);
 
   FrenetPoint getFrenetPoint(
-    const TrackedObject & object, const PosePath & ref_path, const double duration,
-    const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
 };
 }  // namespace map_based_prediction
 

perception/map_based_prediction/schema/map_based_prediction.schema.json

@@ -12,23 +12,9 @@
           "description": "flag to enable the time delay compensation for the position of the object"
         },
         "prediction_time_horizon": {
-          "properties": {
-            "vehicle": {
-              "type": "number",
-              "default": "15.0",
-              "description": "predict time duration for predicted path of vehicle"
-            },
-            "pedestrian": {
-              "type": "number",
-              "default": "10.0",
-              "description": "predict time duration for predicted path of pedestrian"
-            },
-            "unknown": {
-              "type": "number",
-              "default": "10.0",
-              "description": "predict time duration for predicted path of unknown"
-            }
-          }
+          "type": "number",
+          "default": "10.0",
+          "description": "predict time duration for predicted path"
         },
         "lateral_control_time_horizon": {
           "type": "number",

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.
@@ -720,81 +720,79 @@
     google::InstallFailureSignalHandler();
   }
   enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
-  prediction_time_horizon_.vehicle = declare_parameter<double>("prediction_time_horizon.vehicle");
-  prediction_time_horizon_.pedestrian =
-    declare_parameter<double>("prediction_time_horizon.pedestrian");
-  prediction_time_horizon_.unknown = declare_parameter<double>("prediction_time_horizon.unknown");
+  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");
   min_crosswalk_user_velocity_ = declare_parameter<double>("min_crosswalk_user_velocity");
   max_crosswalk_user_delta_yaw_threshold_for_lanelet_ =
     declare_parameter<double>("max_crosswalk_user_delta_yaw_threshold_for_lanelet");
   dist_threshold_for_searching_lanelet_ =
     declare_parameter<double>("dist_threshold_for_searching_lanelet");
   delta_yaw_threshold_for_searching_lanelet_ =
     declare_parameter<double>("delta_yaw_threshold_for_searching_lanelet");
   sigma_lateral_offset_ = declare_parameter<double>("sigma_lateral_offset");
   sigma_yaw_angle_deg_ = declare_parameter<double>("sigma_yaw_angle_deg");
   object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
   history_time_length_ = declare_parameter<double>("history_time_length");
 
   check_lateral_acceleration_constraints_ =
     declare_parameter<bool>("check_lateral_acceleration_constraints");
   max_lateral_accel_ = declare_parameter<double>("max_lateral_accel");
   min_acceleration_before_curve_ = declare_parameter<double>("min_acceleration_before_curve");
 
   {  // lane change detection
     lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");
 
     // lane change detection by time_to_change_lane
     dist_threshold_to_bound_ = declare_parameter<double>(
       "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection");  // 1m
     time_threshold_to_bound_ = declare_parameter<double>(
       "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection");
     cutoff_freq_of_velocity_lpf_ = declare_parameter<double>(
       "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_"
       "detection");
 
     // lane change detection by lat_diff_distance
     dist_ratio_threshold_to_left_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_left_bound");
     dist_ratio_threshold_to_right_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.dist_ratio_threshold_to_right_bound");
     diff_dist_threshold_to_left_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_left_bound");
     diff_dist_threshold_to_right_bound_ = declare_parameter<double>(
       "lane_change_detection.lat_diff_distance.diff_dist_threshold_to_right_bound");
 
     num_continuous_state_transition_ =
       declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
 
     consider_only_routable_neighbours_ =
       declare_parameter<bool>("lane_change_detection.consider_only_routable_neighbours");
   }
   reference_path_resolution_ = declare_parameter<double>("reference_path_resolution");
   /* prediction path will disabled when the estimated path length exceeds lanelet length. This
    * parameter control the estimated path length = vx * th * (rate)  */
   prediction_time_horizon_rate_for_validate_lane_length_ =
     declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
 
   use_vehicle_acceleration_ = declare_parameter<bool>("use_vehicle_acceleration");
   speed_limit_multiplier_ = declare_parameter<double>("speed_limit_multiplier");
   acceleration_exponential_half_life_ =
     declare_parameter<double>("acceleration_exponential_half_life");
 
   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_sampling_time_interval_, min_crosswalk_user_velocity_);
+    prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
+    min_crosswalk_user_velocity_);
 
   path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
   path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
@@ -971,8 +969,8 @@
 
         // For off lane obstacles
         if (current_lanelets.empty()) {
-          PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle(
-            transformed_object, prediction_time_horizon_.vehicle);
+          PredictedPath predicted_path =
+            path_generator_->generatePathForOffLaneVehicle(transformed_object);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -987,8 +985,8 @@
           transformed_object.kinematics.twist_with_covariance.twist.linear.x,
           transformed_object.kinematics.twist_with_covariance.twist.linear.y);
         if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) {
-          PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
-            transformed_object, prediction_time_horizon_.vehicle);
+          PredictedPath predicted_path =
+            path_generator_->generatePathForLowSpeedVehicle(transformed_object);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -1000,14 +998,13 @@
 
         // Get Predicted Reference Path for Each Maneuver and current lanelets
         // return: <probability, paths>
-        const auto ref_paths = getPredictedReferencePath(
-          transformed_object, current_lanelets, objects_detected_time,
-          prediction_time_horizon_.vehicle);
+        const auto ref_paths =
+          getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time);
 
         // If predicted reference path is empty, assume this object is out of the lane
         if (ref_paths.empty()) {
-          PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
-            transformed_object, prediction_time_horizon_.vehicle);
+          PredictedPath predicted_path =
+            path_generator_->generatePathForLowSpeedVehicle(transformed_object);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -1042,8 +1039,7 @@
 
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
-            yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle,
-            lateral_control_time_horizon_, ref_path.speed_limit);
+            yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit);
           if (predicted_path.path.empty()) continue;
 
           if (!check_lateral_acceleration_constraints_) {
@@ -1106,8 +1102,8 @@
       }
       default: {
         auto predicted_unknown_object = convertToPredictedObject(transformed_object);
-        PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(
-          transformed_object, prediction_time_horizon_.unknown);
+        PredictedPath predicted_path =
+          path_generator_->generatePathForNonVehicleObject(transformed_object);
         predicted_path.confidence = 1.0;
 
         predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1174,8 +1170,7 @@
 {
   auto predicted_object = convertToPredictedObject(object);
   {
-    PredictedPath predicted_path =
-      path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian);
+    PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object);
     predicted_path.confidence = 1.0;
 
     predicted_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1226,7 +1221,7 @@
 
       if (hasPotentialToReach(
             object, edge_points.front_center_point, edge_points.front_right_point,
-            edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0,
+            edge_points.front_left_point, prediction_time_horizon_ * 2.0,
             min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
         PredictedPath predicted_path =
           path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point);
@@ -1236,7 +1231,7 @@
 
       if (hasPotentialToReach(
             object, edge_points.back_center_point, edge_points.back_right_point,
-            edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0,
+            edge_points.back_left_point, prediction_time_horizon_ * 2.0,
             min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
         PredictedPath predicted_path =
           path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point);
@@ -1260,27 +1255,27 @@
 
     const auto reachable_first = hasPotentialToReach(
       object, edge_points.front_center_point, edge_points.front_right_point,
-      edge_points.front_left_point, prediction_time_horizon_.pedestrian,
-      min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
+      edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
+      max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
     const auto reachable_second = hasPotentialToReach(
       object, edge_points.back_center_point, edge_points.back_right_point,
-      edge_points.back_left_point, prediction_time_horizon_.pedestrian,
-      min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
+      edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_,
+      max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
 
     if (!reachable_first && !reachable_second) {
       continue;
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian,
+      object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
       min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;
     }
 
-    PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser(
-      object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian);
+    PredictedPath predicted_path =
+      path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());
     predicted_path.confidence = 1.0;
 
     if (predicted_path.path.empty()) {
@@ -1618,7 +1613,7 @@
 
 std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
   const TrackedObject & object, const LaneletsData & current_lanelets_data,
-  const double object_detected_time, const double time_horizon)
+  const double object_detected_time)
 {
   const double obj_vel = std::hypot(
     object.kinematics.twist_with_covariance.twist.linear.x,
@@ -1630,7 +1625,7 @@
                                object.kinematics.acceleration_with_covariance.accel.linear.x,
                                object.kinematics.acceleration_with_covariance.accel.linear.y)
                            : 0.0;
-  const double t_h = time_horizon;
+  const double t_h = prediction_time_horizon_;
   const double λ = std::log(2) / acceleration_exponential_half_life_;
 
   auto get_search_distance_with_decaying_acc = [&]() -> double {