feat(map_based_prediction): use obstacle acceleration for map prediction

perception/map_based_prediction/README.md

@@ -124,7 +124,7 @@ See paper [2] for more details.
 
 `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.)
 
-#### Pruning predicted paths with lateral acceleration constraint
+#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles)
 
 It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated.
 
@@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain
 
 You can change these parameters in rosparam in the table below.
 
-| param name                                | default value  |
-| ----------------------------------------- | -------------- |
-| `check_lateral_acceleration_constraints_` | `false` [bool] |
-| `max_lateral_accel`                       | `2.0` [m/s^2]  |
-| `min_acceleration_before_curve`           | `-2.0` [m/s^2] |
+| param name                               | default value  |
+| ---------------------------------------- | -------------- |
+| `check_lateral_acceleration_constraints` | `false` [bool] |
+| `max_lateral_accel`                      | `2.0` [m/s^2]  |
+| `min_acceleration_before_curve`          | `-2.0` [m/s^2] |
+
+## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles)
+
+By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length.
+
+### Decaying Acceleration Model
+
+Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as:
+
+$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $
+
+where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life.
+
+Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as:
+
+$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $
+
+and
+
+$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $
+
+With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor).
+
+Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction:
+
+- `use_vehicle_acceleration`: to enable the feature.
+- `acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds.
+- `speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0.
+
+You can change these parameters in `rosparam` in the table below.
+
+| Param Name                           | Default Value  |
+| ------------------------------------ | -------------- |
+| `use_vehicle_acceleration`           | `false` [bool] |
+| `acceleration_exponential_half_life` | `2.5` [s]      |
+| `speed_limit_multiplier`             | `1.5` []       |
 
 ### Path prediction for crosswalk users
 

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -16,6 +16,9 @@
     check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints
     max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths
     min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve
+    use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
+    speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
+    acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
     # parameter for shoulder lane prediction
     prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 
@@ -32,6 +35,6 @@
         diff_dist_threshold_to_left_bound: 0.29 #[m]
         diff_dist_threshold_to_right_bound: -0.29 #[m]
       num_continuous_state_transition: 3
       consider_only_routable_neighbours: false
 
     reference_path_resolution: 0.5 #[m]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -22,6 +22,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
@@ -89,6 +90,7 @@ struct LaneletData
 struct PredictedRefPath
 {
   float probability;
+  double speed_limit;
   PosePath path;
   Maneuver maneuver;
 };
@@ -175,6 +177,10 @@ class MapBasedPredictionNode : public rclcpp::Node
   double max_lateral_accel_;
   double min_acceleration_before_curve_;
 
+  bool use_vehicle_acceleration_;
+  double speed_limit_multiplier_;
+  double acceleration_exponential_half_life_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
@@ -231,7 +237,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   void addReferencePaths(
     const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths,
     const float path_probability, const ManeuverProbability & maneuver_probability,
-    const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths);
+    const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
+    const double speed_limit = 0.0);
   std::vector<PosePath> convertPathType(const lanelet::routing::LaneletPaths & paths);
 
   void updateFuturePossibleLanelets(

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -91,25 +91,38 @@
   PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);
 
   PredictedPath generatePathForOnLaneVehicle(
-    const TrackedObject & object, const PosePath & ref_paths);
+    const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0);
 
   PredictedPath generatePathForCrosswalkUser(
     const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
 
   PredictedPath generatePathToTargetPoint(
     const TrackedObject & object, const Eigen::Vector2d & point) const;
 
+  void setUseVehicleAcceleration(const bool use_vehicle_acceleration)
+  {
+    use_vehicle_acceleration_ = use_vehicle_acceleration;
+  }
+
+  void setAccelerationHalfLife(const double acceleration_exponential_half_life)
+  {
+    acceleration_exponential_half_life_ = acceleration_exponential_half_life;
+  }
+
 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 generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path);
+  PredictedPath generatePolynomialPath(
+    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);
@@ -125,7 +138,8 @@
     const TrackedObject & object, const FrenetPath & frenet_predicted_path,
     const PosePath & ref_path);
 
-  FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path);
+  FrenetPoint getFrenetPoint(
+    const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
 };
 }  // namespace map_based_prediction
 

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.
@@ -24,7 +24,6 @@
 #include <tier4_autoware_utils/math/constants.hpp>
 #include <tier4_autoware_utils/math/normalization.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
-#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
@@ -385,11 +384,7 @@
   const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const bool use_yaw_information = false)
 {
-  using Point = boost::geometry::model::d2::point_xy<double>;
-
   const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  const Point p_object{obj_pos.x, obj_pos.y};
-
   lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
   // nearest lanelet
   constexpr double search_radius = 10.0;  // [m]
@@ -788,10 +783,18 @@
   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");
+
   path_generator_ = std::make_shared<PathGenerator>(
     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_);
+
   sub_objects_ = this->create_subscription<TrackedObjects>(
     "~/input/objects", 1,
     std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1));
@@ -817,6 +820,13 @@
   updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_);
   updateParam(
     parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_);
+  updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_);
+  updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_);
+  updateParam(
+    parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_);
+
+  path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
+  path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -1010,7 +1020,7 @@
 
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
-            yaw_fixed_transformed_object, ref_path.path);
+            yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit);
           if (predicted_path.path.empty()) continue;
 
           if (!check_lateral_acceleration_constraints_) {
@@ -1555,14 +1565,63 @@
     object.kinematics.twist_with_covariance.twist.linear.x,
     object.kinematics.twist_with_covariance.twist.linear.y);
 
+  // Using a decaying acceleration model. Consult the README for more information about the model.
+  const double obj_acc = (use_vehicle_acceleration_)
+                           ? std::hypot(
+                               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 λ = std::log(2) / acceleration_exponential_half_life_;
+
+  auto get_search_distance_with_decaying_acc = [&]() -> double {
+    const double acceleration_distance =
+      obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
+    double search_dist = acceleration_distance + obj_vel * t_h;
+    return search_dist;
+  };
+
+  auto get_search_distance_with_partial_acc = [&](const double final_speed) -> double {
+    constexpr double epsilon = 1E-5;
+    if (std::abs(obj_acc) < epsilon) {
+      // Assume constant speed
+      return obj_vel * t_h;
+    }
+    // Time to reach final speed
+    const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
+    // It is assumed the vehicle accelerates until final_speed is reached and
+    // then continues at constant speed for the rest of the time horizon
+    const double search_dist =
+      // Distance covered while accelerating
+      obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
+      obj_vel * t_f +
+      // Distance covered at constant speed
+      final_speed * (t_h - t_f);
+    return search_dist;
+  };
+
   std::vector<PredictedRefPath> all_ref_paths;
+
   for (const auto & current_lanelet_data : current_lanelets_data) {
-    // parameter for lanelet::routing::PossiblePathsParams
-    const double search_dist = prediction_time_horizon_ * obj_vel +
-                               lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
+    const lanelet::traffic_rules::SpeedLimitInformation limit =
+      traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet);
+    const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
+
+    double final_speed_after_acceleration =
+      obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
+
+    const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
+    const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;
+    const bool object_has_surpassed_limit_already = obj_vel > final_speed_limit;
+
+    double search_dist = (final_speed_surpasses_limit && !object_has_surpassed_limit_already)
+                           ? get_search_distance_with_partial_acc(final_speed_limit)
+                           : get_search_distance_with_decaying_acc();
+    search_dist += lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
+
     lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true};
     const double validate_time_horizon =
-      prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_;
+      t_h * prediction_time_horizon_rate_for_validate_lane_length_;
 
     // lambda function to get possible paths for isolated lanelet
     // isolated is often caused by lanelet with no connection e.g. shoulder-lane
@@ -1644,7 +1703,8 @@
     // Step4. add candidate reference paths to the all_ref_paths
     const float path_prob = current_lanelet_data.probability;
     const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) {
-      addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths);
+      addReferencePaths(
+        object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths, final_speed_limit);
     };
     addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE);
     addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE);
@@ -1966,16 +2026,18 @@
 void MapBasedPredictionNode::addReferencePaths(
   const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths,
   const float path_probability, const ManeuverProbability & maneuver_probability,
-  const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths)
+  const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
+  const double speed_limit)
 {
   if (!candidate_paths.empty()) {
     updateFuturePossibleLanelets(object, candidate_paths);
     const auto converted_paths = convertPathType(candidate_paths);
     for (const auto & converted_path : converted_paths) {
       PredictedRefPath predicted_path;
       predicted_path.probability = maneuver_probability.at(maneuver) * path_probability;
       predicted_path.path = converted_path;
       predicted_path.maneuver = maneuver;
+      predicted_path.speed_limit = speed_limit;
       reference_paths.push_back(predicted_path);
     }
   }