Revert "feat(map_based_prediction): use different time horizon (#6877)"

satoshi-ota · satoshi-ota · commit afcc1e0513ab · 2024-05-10T15:28:25.000+09:00
This reverts commit d5aa5d6.
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/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]
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
@@ -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);
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
@@ -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
 
diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/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",
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -720,10 +720,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     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");
@@ -794,7 +791,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     "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 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
         // 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 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           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 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
         // 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 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
         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 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
       }
       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 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 {
   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 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
       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 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
       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 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
     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 @@ void MapBasedPredictionNode::updateObjectsHistory(
 
 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 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
                                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 {
diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp
