From f8ef146562c12f6063a31b08bc8f8455c14f4d2c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 17 Dec 2024 22:08:28 +0900 Subject: [PATCH] feat(pid_longitudinal_controller): add smooth_stop mode in debug_values (#9681) Signed-off-by: Takayuki Murooka --- .../debug_values.hpp | 4 ++ .../smooth_stop.hpp | 6 ++- .../src/pid_longitudinal_controller.cpp | 2 +- .../src/smooth_stop.cpp | 10 +++- .../test/test_smooth_stop.cpp | 54 ++++++++++++------- 5 files changed, 54 insertions(+), 22 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 84f2c76815a3a..e1cc4d2fd1690 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 @@ -63,6 +63,7 @@ class DebugValues ACC_CMD_ACC_FB_APPLIED = 33, PITCH_LPF_RAD = 34, PITCH_LPF_DEG = 35, + SMOOTH_STOP_MODE = 36, SIZE // this is the number of enum elements }; @@ -78,6 +79,9 @@ class DebugValues * @return array of all debug values */ std::array(TYPE::SIZE)> getValues() const { return m_values; } + double getValue(const size_t index) const { return m_values.at(index); } + double getValue(const TYPE type) const { return m_values.at(static_cast(type)); } + /** * @brief set the given type to the given value * @param [in] type TYPE of the value diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp index e84b44d95c886..8f8fc57e278ef 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#include "autoware/pid_longitudinal_controller/debug_values.hpp" #include "rclcpp/rclcpp.hpp" #include // NOLINT @@ -85,7 +86,8 @@ class SmoothStop */ double calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time); + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values); private: struct Params @@ -106,6 +108,8 @@ class SmoothStop }; Params m_params; + enum class Mode { STRONG = 0, WEAK, WEAK_STOP, STRONG_STOP }; + double m_strong_acc; rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; 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 6057408674744..4338b73ccef19 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -871,7 +871,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } else if (m_control_state == ControlState::STOPPING) { raw_ctrl_cmd.acc = m_smooth_stop.calculate( control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, - m_vel_hist, m_delay_compensation_time); + m_vel_hist, m_delay_compensation_time, m_debug_values); raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( diff --git a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 9ee4763c6c05f..2dd2950d4356e 100644 --- a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -116,7 +116,8 @@ std::experimental::optional SmoothStop::calcTimeToStop( double SmoothStop::calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time) + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values) { if (!m_is_set_params) { throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); @@ -132,8 +133,11 @@ double SmoothStop::calculate( // when exceeding the stopline (stop_dist is negative in these cases.) if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much + debug_values.setValues( + DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK_STOP)); return m_params.weak_stop_acc; } @@ -143,19 +147,23 @@ double SmoothStop::calculate( if ( (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) || (!time_to_stop && is_fast_vel)) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG)); return m_strong_acc; } m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // for 0.5 seconds after the car stopped if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // when the car is not running + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index a6d03b8032fe6..30d582bbbf1ef 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -21,6 +21,7 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) { + using ::autoware::motion::control::pid_longitudinal_controller::DebugValues; using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop; using rclcpp::Duration; using rclcpp::Time; @@ -40,9 +41,10 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) const double delay_time = 0.17; SmoothStop ss; + DebugValues debug_values; // Cannot calculate before setting parameters - EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error); + EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time, debug_values), std::runtime_error); ss.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -62,34 +64,45 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) stop_dist = strong_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist) stop_dist = weak_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, weak_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 2); // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc rclcpp::Rate rate_quart(1.0 / 0.25); rclcpp::Rate rate_half(1.0 / 0.5); stop_dist = 0.0; current_vel = 0.0; - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_quart.sleep(); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_half.sleep(); - EXPECT_NE( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_NE(accel, weak_acc); + EXPECT_NE(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); // strong stop when the car is not running (and is at least 0.5seconds after initialization) - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - strong_stop_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // accel between min/max_strong_acc when the car is running: // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay @@ -97,19 +110,22 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) current_vel = 1.0; vel_in_target = 1.0; ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - max_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, max_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); vel_in_target = std::sqrt(2.0); ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - min_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, min_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) { ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_GT(accel, min_strong_acc); EXPECT_LT(accel, max_strong_acc); }