From c0d68b5bb80be667c0f99c6dd7f7e4b77ea5a11e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Dec 2022 08:38:32 +0900 Subject: [PATCH] add vel lpf and disable_target_acceleration Signed-off-by: Takayuki Murooka --- .../pid_based_planner/pid_based_planner.hpp | 2 ++ .../src/pid_based_planner/pid_based_planner.cpp | 16 +++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index c2308adc1ca17..f94516407e3c5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -102,6 +102,7 @@ class PIDBasedPlanner : public PlannerInterface double output_ratio_during_accel; double vel_to_acc_weight; bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; }; VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; @@ -131,6 +132,7 @@ class PIDBasedPlanner : public PlannerInterface std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; // lpf for output + std::shared_ptr lpf_output_vel_ptr_; std::shared_ptr lpf_output_acc_ptr_; std::shared_ptr lpf_output_jerk_ptr_; diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 56491004775f9..0cb74755e6dac 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -82,6 +82,9 @@ PIDBasedPlanner::PIDBasedPlanner( velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = node.declare_parameter( "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); } { // velocity insertion based planner @@ -139,6 +142,7 @@ PIDBasedPlanner::PIDBasedPlanner( lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); lpf_obstacle_vel_ptr_ = std::make_shared(0.5); lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); lpf_output_acc_ptr_ = std::make_shared(0.5); lpf_output_jerk_ptr_ = std::make_shared(0.5); } @@ -324,6 +328,7 @@ Trajectory PIDBasedPlanner::planCruise( prev_target_acc_ = {}; lpf_normalized_error_cruise_dist_ptr_->reset(); lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); lpf_output_jerk_ptr_->reset(); // delete marker @@ -357,10 +362,15 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( }(); const double positive_target_vel = - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); + lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); // calculate target acceleration const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + double target_acc = p.vel_to_acc_weight * additional_vel; // apply acc limit @@ -629,6 +639,10 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet tier4_autoware_utils::updateParam( parameters, "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", + p.disable_target_acceleration); } { // velocity insertion based planner