From e808fdc0a565e80161c03f36489822bda3478e8c Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Mon, 16 Dec 2024 19:32:40 +0900 Subject: [PATCH] feat(pid_longitudinal_controller): update trajectory_adaptive; add debug_values, adopt rate limit fillter (#9656) Signed-off-by: yuki-takagi-66 --- .../debug_values.hpp | 6 +++-- .../pid_longitudinal_controller.hpp | 12 ++++++---- .../src/pid_longitudinal_controller.cpp | 23 ++++++++++++++----- 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index a0bceda625b21..84f2c76815a3a 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -34,9 +34,9 @@ class DebugValues NEAREST_VEL = 4, NEAREST_ACC = 5, SHIFT = 6, - PITCH_LPF_RAD = 7, + PITCH_USING_RAD = 7, PITCH_RAW_RAD = 8, - PITCH_LPF_DEG = 9, + PITCH_USING_DEG = 9, PITCH_RAW_DEG = 10, ERROR_VEL = 11, ERROR_VEL_FILTERED = 12, @@ -61,6 +61,8 @@ class DebugValues ERROR_ACC = 31, ERROR_ACC_FILTERED = 32, ACC_CMD_ACC_FB_APPLIED = 33, + PITCH_LPF_RAD = 34, + PITCH_LPF_DEG = 35, SIZE // this is the number of enum elements }; diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 7daf3013bab4a..8c78b2b871fda 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -216,6 +216,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; + std::optional m_previous_slope_angle{std::nullopt}; // ego nearest index search double m_ego_nearest_dist_threshold; @@ -411,11 +412,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief update variables for debugging about pitch - * @param [in] pitch current pitch of the vehicle (filtered) - * @param [in] traj_pitch current trajectory pitch - * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) + * @param [in] pitch_using + * @param [in] traj_pitch + * @param [in] localization_pitch + * @param [in] localization_pitch_lpf */ - void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch); + void updatePitchDebugValues( + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf); /** * @brief update variables for velocity and acceleration diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 7f6da35527290..6057408674744 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -584,13 +584,21 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData } else { control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); } + if (m_previous_slope_angle.has_value()) { + constexpr double gravity_const = 9.8; + control_data.slope_angle = std::clamp( + control_data.slope_angle, + m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const, + m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const); + } } else { RCLCPP_ERROR_THROTTLE( logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default"); control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); } - updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); + m_previous_slope_angle = control_data.slope_angle; + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue()); return control_data; } @@ -1189,13 +1197,16 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont } void PidLongitudinalController::updatePitchDebugValues( - const double pitch, const double traj_pitch, const double raw_pitch) + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf) { const double to_degrees = (180.0 / static_cast(M_PI)); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_RAD, pitch_using); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_DEG, pitch_using * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, localization_pitch_lpf); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, localization_pitch_lpf * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, localization_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, localization_pitch * to_degrees); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); }