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 index f392feec17a0a..30f9976655e5d 100644 --- 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 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 index 8467f480acfed..6dfc4a8db9e20 100644 --- 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,6 +104,11 @@ 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_; @@ -111,6 +116,7 @@ class PathGenerator 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 index 034129c807f4a..ef115311f79d2 100644 --- 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("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); use_vehicle_acceleration_ = declare_parameter("use_vehicle_acceleration"); + speed_limit_multiplier_ = declare_parameter("speed_limit_multiplier"); + acceleration_exponential_half_life_ = + declare_parameter("acceleration_exponential_half_life"); path_generator_ = std::make_shared( 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( "~/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 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 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 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 index fd5847b9c8180..b86430faaa4fd 100644 --- 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 {