add params

Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Jan 11, 2024 · 7f8c2bd · 7f8c2bd
1 parent 257adad
commit 7f8c2bd
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 4 deletions.
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
@@ -178,6 +178,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   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_;

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
@@ -104,13 +104,19 @@ class PathGenerator
     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;

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -784,12 +784,16 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     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,
@@ -817,8 +821,12 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
   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;
@@ -1557,7 +1565,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     object.kinematics.twist_with_covariance.twist.linear.x,
     object.kinematics.twist_with_covariance.twist.linear.y);
 
-  // Use a decaying acceleration model
+  // Using a decaying acceleration model
+  // a(t) = obj_acc - obj_acc(1-e^(-λt)) = obj_acc(e^(-λt))
   const double obj_acc = (use_vehicle_acceleration_)
                            ? std::hypot(
                                object.kinematics.acceleration_with_covariance.accel.linear.x,
@@ -1566,7 +1575,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
 
   // The decay constant λ = ln(2) / exponential_half_life
   const double T = prediction_time_horizon_;
-  const double exponential_half_life = T / 4.0;
+  const double exponential_half_life = acceleration_exponential_half_life_;
   const double λ = std::log(2) / exponential_half_life;
 
   auto get_search_distance_with_decaying_acc = [&]() -> double {
@@ -1611,7 +1620,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     double final_speed_after_acceleration =
       obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * T));
 
-    const double final_speed_limit = legal_speed_limit * 1.5;
+    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;
 

diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp
@@ -417,7 +417,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
   // x(t) = Xo + (Vo + acc(1/λ)) * t  + acc(1/λ^2)e^(-λt)
   // acceleration_distance = acc(1/λ) * t  + acc(1/λ^2)e^(-λt)
   const double T = time_horizon_;
-  const float exponential_half_life = T / 4.0;
+  const float exponential_half_life = acceleration_exponential_half_life_;
   const float λ = std::log(2) / exponential_half_life;
 
   auto have_same_sign = [](double a, double b) -> bool {