From afecb8ce838621576a610442bbce517d69ccdaf1 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 23 Oct 2023 11:22:49 +0900 Subject: [PATCH 1/2] feat(pid_longitudinal_controller): constant feedback for moving backward Signed-off-by: Takayuki Murooka --- .../pid_longitudinal_controller.hpp | 2 ++ .../src/pid_longitudinal_controller.cpp | 14 +++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 078da4dff4675..c1f95ab514569 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -364,6 +364,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel, const Shift & shift); + double applySlidingDownFeedback(const double current_vel) const; + /** * @brief update variables for debugging about pitch * @param [in] pitch current pitch of the vehicle (filtered) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index f088a3a5cee56..adeb6e49d70bf 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -666,8 +666,12 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = + + const double vel_feedback_acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); + const double sliding_down_feedback_acc = applySlidingDownFeedback(current_vel); + raw_ctrl_cmd.acc = vel_feedback_acc + sliding_down_feedback_acc; + RCLCPP_DEBUG( node_->get_logger(), "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " @@ -982,6 +986,14 @@ double PidLongitudinalController::applyVelocityFeedback( return feedback_acc; } +double PidLongitudinalController::applySlidingDownFeedback(const double current_vel) const +{ + if (control_data.shift != Shift::Forward || 0.0 < current_vel) { + return 0.0; + } + return 0.5; +} + void PidLongitudinalController::updatePitchDebugValues( const double pitch, const double traj_pitch, const double raw_pitch) { From bfd05ace2b823570ece3379b7348f1ea6d24fcf0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 23 Oct 2023 12:00:28 +0900 Subject: [PATCH 2/2] fix Signed-off-by: Takayuki Murooka --- .../pid_longitudinal_controller.hpp | 2 +- .../src/pid_longitudinal_controller.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index c1f95ab514569..38fbe161e8dbe 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -364,7 +364,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel, const Shift & shift); - double applySlidingDownFeedback(const double current_vel) const; + double applySlidingDownFeedback(const double current_vel, const ControlData & control_data) const; /** * @brief update variables for debugging about pitch diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index adeb6e49d70bf..b0645a5805b3e 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -669,7 +669,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( const double vel_feedback_acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); - const double sliding_down_feedback_acc = applySlidingDownFeedback(current_vel); + const double sliding_down_feedback_acc = applySlidingDownFeedback(current_vel, control_data); raw_ctrl_cmd.acc = vel_feedback_acc + sliding_down_feedback_acc; RCLCPP_DEBUG( @@ -986,7 +986,8 @@ double PidLongitudinalController::applyVelocityFeedback( return feedback_acc; } -double PidLongitudinalController::applySlidingDownFeedback(const double current_vel) const +double PidLongitudinalController::applySlidingDownFeedback( + const double current_vel, const ControlData & control_data) const { if (control_data.shift != Shift::Forward || 0.0 < current_vel) { return 0.0;