feat(map_based_prediction): use obstacle acceleration for map predict…

…ion (autowarefoundation#6072)

* add acc filtering and decaying acc to model

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* fixed compilation problem, acc is used to predict search_dist

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Use an equivalent velocity to calculate paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* change decaying factor to T/4

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* coment out cerr for evaluation

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* simplify code

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* comments

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add missing constant to decaying acc model

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Use an equivalent velocity to calculate paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add missing constant to decaying acc model

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Implement lanelet speed limit for acc consideration

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add option to activate on and off acceleration feature

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add params

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add params

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* delete unused class

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* update docs

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete comments

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* fix comments

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* update comments, refactor code

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* remove unused line

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

---------

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

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
 

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 @@ class PathGenerator
   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 @@ class PathGenerator
     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

@@ -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 @@ bool withinRoadLanelet(
   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 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   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 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
   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 @@ 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);
+            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 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     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 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     // 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,7 +2026,8 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets(
 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);
@@ -1976,6 +2037,7 @@ void MapBasedPredictionNode::addReferencePaths(
       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);
     }
   }