feat(pid_longitudinal_controller): use Integrated error when vehicle is stopped

control/pid_longitudinal_controller/README.md

@@ -90,8 +90,13 @@ This PID logic has a maximum value for the output of each term. This is to preve
 - Large integral terms may cause unintended behavior by users.
 - Unintended noise may cause the output of the derivative term to be very large.
 
-Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start.
-On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers.
+Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures.
+
+However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true.
+
+When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter.
+
+The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller.
 
 At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance.
 This may be replaced by a higher performance controller (adaptive control or robust control) in future development.
@@ -207,7 +212,9 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | max_d_effort                          | double | max value of acceleration with d gain                                                                                                                              | 0.0           |
 | min_d_effort                          | double | min value of acceleration with d gain                                                                                                                              | 0.0           |
 | lpf_vel_error_gain                    | double | gain of low-pass filter for velocity error                                                                                                                         | 0.9           |
-| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s]                                      | 0.5           |
+| enable_integration_at_low_speed       | bool   | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not.                          |
+| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s]                                      |
+| time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s]                                                                            | 5.0           |
 | brake_keeping_acc                     | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2           |
 
 ### STOPPING Parameter (smooth stop)

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -42,6 +42,7 @@
 
 #include <deque>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -97,6 +98,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   // vehicle info
   double m_wheel_base;
+  bool m_prev_vehicle_is_under_control{false};
+  std::shared_ptr<rclcpp::Time> m_under_control_starting_time{nullptr};
 
   // control state
   enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY };
@@ -145,7 +148,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // drive
   PIDController m_pid_vel;
   std::shared_ptr<LowpassFilter1d> m_lpf_vel_error{nullptr};
+  bool m_enable_integration_at_low_speed;
   double m_current_vel_threshold_pid_integrate;
+  double m_time_threshold_before_pid_integrate;
   bool m_enable_brake_keeping_before_stop;
   double m_brake_keeping_acc;
 
@@ -384,6 +389,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   void updateDebugVelAcc(
     const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose,
     const ControlData & control_data);
+
+  double getTimeUnderControl();
 };
 }  // namespace autoware::motion::control::pid_longitudinal_controller
 

control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml

@@ -32,7 +32,9 @@
     max_d_effort: 0.0
     min_d_effort: 0.0
     lpf_vel_error_gain: 0.9
+    enable_integration_at_low_speed: false
     current_vel_threshold_pid_integration: 0.5
+    time_threshold_before_pid_integration: 2.0
     enable_brake_keeping_before_stop: false
     brake_keeping_acc: -0.2
 

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -100,9 +100,14 @@
     const double lpf_vel_error_gain{node.declare_parameter<double>("lpf_vel_error_gain")};
     m_lpf_vel_error = std::make_shared<LowpassFilter1d>(0.0, lpf_vel_error_gain);
 
+    m_enable_integration_at_low_speed =
+      node.declare_parameter<bool>("enable_integration_at_low_speed");
     m_current_vel_threshold_pid_integrate =
       node.declare_parameter<double>("current_vel_threshold_pid_integration");  // [m/s]
 
+    m_time_threshold_before_pid_integrate =
+      node.declare_parameter<double>("time_threshold_before_pid_integration");  // [s]
+
     m_enable_brake_keeping_before_stop =
       node.declare_parameter<bool>("enable_brake_keeping_before_stop");         // [-]
     m_brake_keeping_acc = node.declare_parameter<double>("brake_keeping_acc");  // [m/s^2]
@@ -284,6 +289,7 @@
     m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d);
 
     update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate);
+    update_param("time_threshold_before_pid_integration", m_time_threshold_before_pid_integrate);
   }
 
   // stopping state
@@ -556,6 +562,11 @@
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
 
+  if (is_under_control != m_prev_vehicle_is_under_control) {
+    m_prev_vehicle_is_under_control = is_under_control;
+    m_under_control_starting_time =
+      is_under_control ? std::make_shared<rclcpp::Time>(clock_->now()) : nullptr;
+  }
   // transit state
   // in DRIVE state
   if (m_control_state == ControlState::DRIVE) {
@@ -959,8 +970,15 @@
   const double diff_vel = (target_motion.vel - current_vel) * vel_sign;
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
+
+  const bool vehicle_is_moving = std::abs(current_vel) > m_current_vel_threshold_pid_integrate;
+  const double time_under_control = getTimeUnderControl();
+  const bool vehicle_is_stuck =
+    !vehicle_is_moving && time_under_control > m_time_threshold_before_pid_integrate;
+
   const bool enable_integration =
-    (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control;
+    (vehicle_is_moving || (m_enable_integration_at_low_speed && vehicle_is_stuck)) &&
+    is_under_control;
   const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel);
 
   std::vector<double> pid_contributions(3);
@@ -1059,4 +1077,10 @@
   stat.summary(level, msg);
 }
 
+double PidLongitudinalController::getTimeUnderControl()
+{
+  if (!m_under_control_starting_time) return 0.0;
+  return (clock_->now() - *m_under_control_starting_time).seconds();
+}
+
 }  // namespace autoware::motion::control::pid_longitudinal_controller

control/trajectory_follower_node/param/longitudinal/pid.param.yaml

@@ -32,7 +32,9 @@
     max_d_effort: 0.0
     min_d_effort: 0.0
     lpf_vel_error_gain: 0.9
+    enable_integration_at_low_speed: false
     current_vel_threshold_pid_integration: 0.5
+    time_threshold_before_pid_integration: 2.0
     enable_brake_keeping_before_stop: false
     brake_keeping_acc: -0.2