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

soblin · web-flow · commit d5aa5d61c5d3 · 2024-05-10T11:34:15.000+09:00
Signed-off-by: Mamoru Sobue &lt;mamoru.sobue@tier4.jp&gt;
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,7 +1,10 @@
 /**:
   ros__parameters:
     enable_delay_compensation: true
-    prediction_time_horizon: 10.0 #[s]
+    prediction_time_horizon:
+      vehicle: 15.0 #[s]
+      pedestrian: 10.0 #[s]
+      unknown: 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,6 +99,13 @@ 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;
@@ -161,7 +168,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   // Parameters
   bool enable_delay_compensation_;
-  double prediction_time_horizon_;
+  PredictionTimeHorizon prediction_time_horizon_;
   double lateral_control_time_horizon_;
   double prediction_time_horizon_rate_for_validate_lane_length_;
   double prediction_sampling_time_interval_;
@@ -238,7 +245,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 object_detected_time, const double time_horizon);
   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,21 +80,23 @@ using PosePath = std::vector<geometry_msgs::msg::Pose>;
 class PathGenerator
 {
 public:
-  PathGenerator(
-    const double time_horizon, const double lateral_time_horizon,
-    const double sampling_time_interval, const double min_crosswalk_user_velocity);
+  PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity);
 
-  PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);
+  PredictedPath generatePathForNonVehicleObject(
+    const TrackedObject & object, const double duration);
 
-  PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const;
+  PredictedPath generatePathForLowSpeedVehicle(
+    const TrackedObject & object, const double duration) const;
 
-  PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);
+  PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object, const double duration);
 
   PredictedPath generatePathForOnLaneVehicle(
-    const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_paths, const double duration,
+    const double lateral_duration, const double speed_limit = 0.0);
 
   PredictedPath generatePathForCrosswalkUser(
-    const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
+    const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
+    const double duration) const;
 
   PredictedPath generatePathToTargetPoint(
     const TrackedObject & object, const Eigen::Vector2d & point) const;
@@ -111,21 +113,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;
+  PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;
 
   PredictedPath generatePolynomialPath(
-    const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_path, const double duration,
+    const double lateral_duration, const double speed_limit = 0.0);
 
   FrenetPath generateFrenetPath(
-    const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length);
+    const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
+    const double duration, const double lateral_duration);
   Eigen::Vector3d calcLatCoefficients(
     const FrenetPoint & current_point, const FrenetPoint & target_point, const double T);
   Eigen::Vector2d calcLonCoefficients(
@@ -139,7 +141,8 @@ class PathGenerator
     const PosePath & ref_path);
 
   FrenetPoint getFrenetPoint(
-    const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
+    const TrackedObject & object, const PosePath & ref_path, const double duration,
+    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,9 +12,23 @@
           "description": "flag to enable the time delay compensation for the position of the object"
         },
         "prediction_time_horizon": {
-          "type": "number",
-          "default": "10.0",
-          "description": "predict time duration for predicted path"
+          "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"
+            }
+          }
         },
         "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,7 +720,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     google::InstallFailureSignalHandler();
   }
   enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
-  prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
+  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");
   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");
@@ -791,8 +794,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     "crosswalk_with_signal.timeout_set_for_no_intention_to_walk");
 
   path_generator_ = std::make_shared<PathGenerator>(
-    prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
-    min_crosswalk_user_velocity_);
+    prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
 
   path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
   path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
@@ -969,8 +971,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
         // For off lane obstacles
         if (current_lanelets.empty()) {
-          PredictedPath predicted_path =
-            path_generator_->generatePathForOffLaneVehicle(transformed_object);
+          PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle(
+            transformed_object, prediction_time_horizon_.vehicle);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -985,8 +987,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);
+          PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
+            transformed_object, prediction_time_horizon_.vehicle);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -998,13 +1000,14 @@ 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);
+        const auto ref_paths = getPredictedReferencePath(
+          transformed_object, current_lanelets, objects_detected_time,
+          prediction_time_horizon_.vehicle);
 
         // 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);
+          PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
+            transformed_object, prediction_time_horizon_.vehicle);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
 
@@ -1039,7 +1042,8 @@ 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, ref_path.speed_limit);
+            yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle,
+            lateral_control_time_horizon_, ref_path.speed_limit);
           if (predicted_path.path.empty()) continue;
 
           if (!check_lateral_acceleration_constraints_) {
@@ -1102,8 +1106,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
       }
       default: {
         auto predicted_unknown_object = convertToPredictedObject(transformed_object);
-        PredictedPath predicted_path =
-          path_generator_->generatePathForNonVehicleObject(transformed_object);
+        PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(
+          transformed_object, prediction_time_horizon_.unknown);
         predicted_path.confidence = 1.0;
 
         predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1170,7 +1174,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 {
   auto predicted_object = convertToPredictedObject(object);
   {
-    PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object);
+    PredictedPath predicted_path =
+      path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian);
     predicted_path.confidence = 1.0;
 
     predicted_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1221,7 +1226,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_ * 2.0,
+            edge_points.front_left_point, prediction_time_horizon_.pedestrian * 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);
@@ -1231,7 +1236,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_ * 2.0,
+            edge_points.back_left_point, prediction_time_horizon_.pedestrian * 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);
@@ -1255,27 +1260,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_, min_crosswalk_user_velocity_,
-      max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
+      edge_points.front_left_point, prediction_time_horizon_.pedestrian,
+      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_, min_crosswalk_user_velocity_,
-      max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
+      edge_points.back_left_point, prediction_time_horizon_.pedestrian,
+      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_,
+      object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian,
       min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;
     }
 
-    PredictedPath predicted_path =
-      path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get());
+    PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser(
+      object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian);
     predicted_path.confidence = 1.0;
 
     if (predicted_path.path.empty()) {
@@ -1613,7 +1618,7 @@ void MapBasedPredictionNode::updateObjectsHistory(
 
 std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
   const TrackedObject & object, const LaneletsData & current_lanelets_data,
-  const double object_detected_time)
+  const double object_detected_time, const double time_horizon)
 {
   const double obj_vel = std::hypot(
     object.kinematics.twist_with_covariance.twist.linear.x,
@@ -1625,7 +1630,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 = prediction_time_horizon_;
+  const double t_h = 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
