feat(pid_longitudinal_controller): add smooth_stop mode in debug_values (#9681)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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<double, static_cast<size_t>(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<size_t>(type)); }
+
   /**
    * @brief set the given type to the given value
    * @param [in] type TYPE of the value

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 <experimental/optional>  // NOLINT
@@ -85,7 +86,8 @@ class SmoothStop
    */
   double calculate(
     const double stop_dist, const double current_vel, const double current_acc,
-    const std::vector<std::pair<rclcpp::Time, double>> & vel_hist, const double delay_time);
+    const std::vector<std::pair<rclcpp::Time, double>> & 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;

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(

control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp

@@ -116,7 +116,8 @@ std::experimental::optional<double> SmoothStop::calcTimeToStop(
 
 double SmoothStop::calculate(
   const double stop_dist, const double current_vel, const double current_acc,
-  const std::vector<std::pair<rclcpp::Time, double>> & vel_hist, const double delay_time)
+  const std::vector<std::pair<rclcpp::Time, double>> & 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<int>(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<int>(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<int>(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<int>(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<int>(Mode::WEAK));
     return m_params.weak_acc;
   }
 
   // when the car is not running
+  debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast<int>(Mode::STRONG_STOP));
   return m_params.strong_stop_acc;
 }
 }  // namespace autoware::motion::control::pid_longitudinal_controller

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,54 +64,68 @@ 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
   stop_dist = 1.0;
   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);
   }