add option to activate on and off acceleration feature

Signed-off-by: Daniel Sanchez <[email protected]>

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -16,6 +16,7 @@
     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
     # 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

@@ -177,6 +177,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   double max_lateral_accel_;
   double min_acceleration_before_curve_;
 
+  bool use_vehicle_acceleration_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -99,12 +99,18 @@ class PathGenerator
   PredictedPath generatePathToTargetPoint(
     const TrackedObject & object, const Eigen::Vector2d & point) const;
 
+  void setUseVehicleAcceleration(const bool use_vehicle_acceleration)
+  {
+    use_vehicle_acceleration_ = use_vehicle_acceleration;
+  }
+
 private:
   // Parameters
   double time_horizon_;
   double lateral_time_horizon_;
   double sampling_time_interval_;
   double min_crosswalk_user_velocity_;
+  bool use_vehicle_acceleration_;
 
   // Member functions
   PredictedPath generateStraightPath(const TrackedObject & object) const;

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -783,10 +783,14 @@ 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");
+
   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_);
+
   sub_objects_ = this->create_subscription<TrackedObjects>(
     "~/input/objects", 1,
     std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1));
@@ -812,6 +816,9 @@ 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_);
+
+  path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -1551,9 +1558,11 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     object.kinematics.twist_with_covariance.twist.linear.y);
 
   // Use a decaying acceleration model
-  const double obj_acc = std::hypot(
-    object.kinematics.acceleration_with_covariance.accel.linear.x,
-    object.kinematics.acceleration_with_covariance.accel.linear.y);
+  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;
 
   // The decay constant λ = ln(2) / exponential_half_life
   const double T = prediction_time_horizon_;

perception/map_based_prediction/src/path_generator.cpp

@@ -402,9 +402,13 @@ FrenetPoint PathGenerator::getFrenetPoint(
   const float delta_yaw = obj_yaw - lane_yaw;
 
   const float ax =
-    static_cast<float>(object.kinematics.acceleration_with_covariance.accel.linear.x);
+    (use_vehicle_acceleration_)
+      ? static_cast<float>(object.kinematics.acceleration_with_covariance.accel.linear.x)
+      : 0.0;
   const float ay =
-    static_cast<float>(object.kinematics.acceleration_with_covariance.accel.linear.y);
+    (use_vehicle_acceleration_)
+      ? static_cast<float>(object.kinematics.acceleration_with_covariance.accel.linear.y)
+      : 0.0;
 
   // The decay constant λ = ln(2) / exponential_half_life
   // a(t) = acc - acc(1-e^(-λt)) = acc(e^(-λt))