feat(pid_long, vehicle_cmd_gate)!: quick pedal change (#1425)

* feat(simple_planning_simulator): add new vehicle model with falling down (autowarefoundation#7651) * add new vehicle model Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * refactor(vehicle_cmd_gate)!: delete rate limit skipping function for vehicle departure (autowarefoundation#7720) * delete a fucntion block. More appropriate function can be achieved by the parameter settings. * add notation to readme --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * feat(pid_longitudinal_controller): re-organize diff limit structure and fix state change condition (autowarefoundation#7718) change diff limit structure change stopped condition define a new param Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
tier4 · Jul 25, 2024 · 19af48e · 19af48e
1 parent 664b2e1
commit 19af48e
Show file tree

Hide file tree

Showing 15 changed files with 487 additions and 141 deletions.
diff --git a/...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -183,7 +183,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   {
     double vel;
     double acc;
-    double jerk;
   };
   StoppedStateParams m_stopped_state_params;
 
@@ -203,6 +202,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // jerk limit
   double m_max_jerk;
   double m_min_jerk;
+  double m_max_acc_cmd_diff;
 
   // slope compensation
   enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
@@ -291,7 +291,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @brief calculate control command in emergency state
    * @param [in] dt time between previous and current one
    */
-  Motion calcEmergencyCtrlCmd(const double dt) const;
+  Motion calcEmergencyCtrlCmd(const double dt);
 
   /**
    * @brief update control state according to the current situation

diff --git a/...ol/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/...ol/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml
@@ -53,12 +53,11 @@
 
     # stopped state
     stopped_vel: 0.0
-    stopped_acc: -3.4
-    stopped_jerk: -5.0
+    stopped_acc: -3.4 # denotes pedal position
 
     # emergency state
     emergency_vel: 0.0
-    emergency_acc: -5.0
+    emergency_acc: -5.0 # denotes acceleration
     emergency_jerk: -3.0
 
     # acceleration limit
@@ -68,6 +67,7 @@
     # jerk limit
     max_jerk: 2.0
     min_jerk: -5.0
+    max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1]
 
     # slope compensation
     lpf_pitch_gain: 0.95

diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -150,9 +150,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   // parameters for stop state
   {
     auto & p = m_stopped_state_params;
-    p.vel = node.declare_parameter<double>("stopped_vel");    // [m/s]
-    p.acc = node.declare_parameter<double>("stopped_acc");    // [m/s^2]
-    p.jerk = node.declare_parameter<double>("stopped_jerk");  // [m/s^3]
+    p.vel = node.declare_parameter<double>("stopped_vel");  // [m/s]
+    p.acc = node.declare_parameter<double>("stopped_acc");  // [m/s^2]
   }
 
   // parameters for emergency state
@@ -168,8 +167,9 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   m_min_acc = node.declare_parameter<double>("min_acc");  // [m/s^2]
 
   // parameters for jerk limit
-  m_max_jerk = node.declare_parameter<double>("max_jerk");  // [m/s^3]
-  m_min_jerk = node.declare_parameter<double>("min_jerk");  // [m/s^3]
+  m_max_jerk = node.declare_parameter<double>("max_jerk");                  // [m/s^3]
+  m_min_jerk = node.declare_parameter<double>("min_jerk");                  // [m/s^3]
+  m_max_acc_cmd_diff = node.declare_parameter<double>("max_acc_cmd_diff");  // [m/s^3]
 
   // parameters for slope compensation
   m_adaptive_trajectory_velocity_th =
@@ -353,7 +353,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
     auto & p = m_stopped_state_params;
     update_param("stopped_vel", p.vel);
     update_param("stopped_acc", p.acc);
-    update_param("stopped_jerk", p.jerk);
   }
 
   // emergency state
@@ -370,6 +369,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
   // jerk limit
   update_param("max_jerk", m_max_jerk);
   update_param("min_jerk", m_min_jerk);
+  update_param("max_acc_cmd_diff", m_max_acc_cmd_diff);
 
   // slope compensation
   update_param("max_pitch_rad", m_max_pitch_rad);
@@ -565,26 +565,30 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
   return control_data;
 }
 
-PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd(
-  const double dt) const
+PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCmd(const double dt)
 {
   // These accelerations are without slope compensation
   const auto & p = m_emergency_state_params;
-  const double vel =
-    longitudinal_utils::applyDiffLimitFilter(p.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc);
-  const double acc =
-    longitudinal_utils::applyDiffLimitFilter(p.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk);
+  Motion raw_ctrl_cmd{p.vel, p.acc};
+
+  raw_ctrl_cmd.vel =
+    longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.vel, m_prev_raw_ctrl_cmd.vel, dt, p.acc);
+  raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc);
+  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc);
+  raw_ctrl_cmd.acc =
+    longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk);
+  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc);
 
   RCLCPP_ERROR_THROTTLE(
-    logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", vel, acc);
+    logger_, *clock_, 3000, "[Emergency stop] vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel,
+    raw_ctrl_cmd.acc);
 
-  return Motion{vel, acc};
+  return raw_ctrl_cmd;
 }
 
 void PidLongitudinalController::updateControlState(const ControlData & control_data)
 {
   const double current_vel = control_data.current_motion.vel;
-  const double current_acc = control_data.current_motion.acc;
   const double stop_dist = control_data.stop_dist;
 
   // flags for state transition
@@ -599,8 +603,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
-  const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel &&
-                          std::abs(current_acc) < p.stopped_state_entry_acc;
+  const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel;
+
   // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also
   // considered as a stop
   const bool is_not_running = [&]() {
@@ -701,7 +705,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
       // prevent the car from taking a long time to start to move
-      m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc);
+      m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_raw_ctrl_cmd.acc);
       return changeState(ControlState::DRIVE);
     }
     return;
@@ -747,8 +751,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
-      // prevent the car from taking a long time to start to move
-      m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc);
       return changeState(ControlState::DRIVE);
     }
 
@@ -780,55 +782,85 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
   const size_t target_idx = control_data.target_idx;
 
   // velocity and acceleration command
-  Motion raw_ctrl_cmd{
+  Motion ctrl_cmd_as_pedal_pos{
     control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps,
     control_data.interpolated_traj.points.at(target_idx).acceleration_mps2};
 
-  if (m_control_state == ControlState::DRIVE) {
-    raw_ctrl_cmd.vel =
-      control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps;
-    raw_ctrl_cmd.acc = applyVelocityFeedback(control_data);
-    raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx);
+  if (m_control_state == ControlState::STOPPED) {
+    const auto & p = m_stopped_state_params;
+    ctrl_cmd_as_pedal_pos.vel = p.vel;
+    ctrl_cmd_as_pedal_pos.acc = p.acc;
 
-    RCLCPP_DEBUG(
-      logger_,
-      "[feedback control]  vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f "
-      "feedback_ctrl_cmd.ac: %3.3f",
-      raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel,
-      control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps,
-      raw_ctrl_cmd.acc);
-  } 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);
-    raw_ctrl_cmd.vel = m_stopped_state_params.vel;
+    m_prev_raw_ctrl_cmd.vel = 0.0;
+    m_prev_raw_ctrl_cmd.acc = 0.0;
+
+    m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, ctrl_cmd_as_pedal_pos.acc);
+    m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, ctrl_cmd_as_pedal_pos.acc);
+    m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc);
 
     RCLCPP_DEBUG(
-      logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel,
-      raw_ctrl_cmd.acc);
-  } else if (m_control_state == ControlState::STOPPED) {
-    // This acceleration is without slope compensation
-    const auto & p = m_stopped_state_params;
-    raw_ctrl_cmd.vel = p.vel;
-    raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter(
-      p.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, p.jerk);
+      logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", ctrl_cmd_as_pedal_pos.vel,
+      ctrl_cmd_as_pedal_pos.acc);
+  } else {
+    Motion raw_ctrl_cmd{
+      control_data.interpolated_traj.points.at(target_idx).longitudinal_velocity_mps,
+      control_data.interpolated_traj.points.at(target_idx).acceleration_mps2};
+    if (m_control_state == ControlState::EMERGENCY) {
+      raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt);
+    } else {
+      if (m_control_state == ControlState::DRIVE) {
+        raw_ctrl_cmd.vel = control_data.interpolated_traj.points.at(control_data.target_idx)
+                             .longitudinal_velocity_mps;
+        raw_ctrl_cmd.acc = applyVelocityFeedback(control_data);
+        raw_ctrl_cmd = keepBrakeBeforeStop(control_data, raw_ctrl_cmd, target_idx);
+
+        RCLCPP_DEBUG(
+          logger_,
+          "[feedback control]  vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f "
+          "feedback_ctrl_cmd.ac: %3.3f",
+          raw_ctrl_cmd.vel, raw_ctrl_cmd.acc, control_data.dt, control_data.current_motion.vel,
+          control_data.interpolated_traj.points.at(control_data.target_idx)
+            .longitudinal_velocity_mps,
+          raw_ctrl_cmd.acc);
+      } 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);
+        raw_ctrl_cmd.vel = m_stopped_state_params.vel;
+
+        RCLCPP_DEBUG(
+          logger_, "[smooth stop]: Smooth stopping. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel,
+          raw_ctrl_cmd.acc);
+      }
+      raw_ctrl_cmd.acc = std::clamp(raw_ctrl_cmd.acc, m_min_acc, m_max_acc);
+      m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, raw_ctrl_cmd.acc);
+      raw_ctrl_cmd.acc = longitudinal_utils::applyDiffLimitFilter(
+        raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk);
+      m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc);
+    }
+
+    // store acceleration without slope compensation
+    m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
 
-    RCLCPP_DEBUG(logger_, "[Stopped]. vel: %3.3f, acc: %3.3f", raw_ctrl_cmd.vel, raw_ctrl_cmd.acc);
-  } else if (m_control_state == ControlState::EMERGENCY) {
-    raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt);
+    ctrl_cmd_as_pedal_pos.acc =
+      applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift);
+    m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc);
+    ctrl_cmd_as_pedal_pos.vel = raw_ctrl_cmd.vel;
   }
 
-  // store acceleration without slope compensation
-  m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
+  storeAccelCmd(m_prev_raw_ctrl_cmd.acc);
 
-  // apply slope compensation and filter acceleration and jerk
-  const double filtered_acc_cmd = calcFilteredAcc(raw_ctrl_cmd.acc, control_data);
-  const Motion filtered_ctrl_cmd{raw_ctrl_cmd.vel, filtered_acc_cmd};
+  ctrl_cmd_as_pedal_pos.acc = longitudinal_utils::applyDiffLimitFilter(
+    ctrl_cmd_as_pedal_pos.acc, m_prev_ctrl_cmd.acc, control_data.dt, m_max_acc_cmd_diff);
 
   // update debug visualization
   updateDebugVelAcc(control_data);
 
-  return filtered_ctrl_cmd;
+  RCLCPP_DEBUG(
+    logger_, "[final output]: acc: %3.3f, v_curr: %3.3f", ctrl_cmd_as_pedal_pos.acc,
+    control_data.current_motion.vel);
+
+  return ctrl_cmd_as_pedal_pos;
 }
 
 // Do not use nearest_idx here
@@ -912,28 +944,6 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift
   return m_prev_shift;
 }
 
-double PidLongitudinalController::calcFilteredAcc(
-  const double raw_acc, const ControlData & control_data)
-{
-  const double acc_max_filtered = std::clamp(raw_acc, m_min_acc, m_max_acc);
-  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_LIMITED, acc_max_filtered);
-
-  // store ctrl cmd without slope filter
-  storeAccelCmd(acc_max_filtered);
-
-  // apply slope compensation
-  const double acc_slope_filtered =
-    applySlopeCompensation(acc_max_filtered, control_data.slope_angle, control_data.shift);
-  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, acc_slope_filtered);
-
-  // This jerk filter must be applied after slope compensation
-  const double acc_jerk_filtered = longitudinal_utils::applyDiffLimitFilter(
-    acc_slope_filtered, m_prev_ctrl_cmd.acc, control_data.dt, m_max_jerk, m_min_jerk);
-  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, acc_jerk_filtered);
-
-  return acc_jerk_filtered;
-}
-
 void PidLongitudinalController::storeAccelCmd(const double accel)
 {
   if (m_control_state == ControlState::DRIVE) {

diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml
@@ -53,12 +53,11 @@
 
     # stopped state
     stopped_vel: 0.0
-    stopped_acc: -3.4
-    stopped_jerk: -5.0
+    stopped_acc: -3.4 # denotes pedal position
 
     # emergency state
     emergency_vel: 0.0
-    emergency_acc: -5.0
+    emergency_acc: -5.0 # denotes acceleration
     emergency_jerk: -3.0
 
     # acceleration limit
@@ -68,6 +67,7 @@
     # jerk limit
     max_jerk: 2.0
     min_jerk: -5.0
+    max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1]
 
     # slope compensation
     lpf_pitch_gain: 0.95

diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md
@@ -78,6 +78,10 @@ The limitation values are calculated based on the 1D interpolation of the limita
 
 Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published.
 
+Notation 2: If you use vehicles in which the driving force is controlled by the accelerator/brake pedal, the jerk limit, denoting the pedal rate limit, must be sufficiently relaxed at low speeds.
+Otherwise, quick pedal changes at start/stop will not be possible, resulting in slow starts and creep down on hills.
+This functionality for starting/stopping was embedded in the source code but was removed because it was complex and could be achieved by parameters.
+
 ## Assumptions / Known limits
 
 The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules.

diff --git a/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
@@ -16,14 +16,14 @@
     stop_check_duration: 1.0
     nominal:
       vel_lim: 25.0
-      reference_speed_points: [20.0, 30.0]
-      steer_lim: [1.0, 0.8]
-      steer_rate_lim: [1.0, 0.8]
-      lon_acc_lim: [5.0, 4.0]
-      lon_jerk_lim: [5.0, 4.0]
-      lat_acc_lim: [5.0, 4.0]
-      lat_jerk_lim: [7.0, 6.0]
-      actual_steer_diff_lim: [1.0, 0.8]
+      reference_speed_points: [0.1, 0.3, 20.0, 30.0]
+      steer_lim: [1.0, 1.0, 1.0, 0.8]
+      steer_rate_lim: [1.0, 1.0, 1.0, 0.8]
+      lon_acc_lim: [5.0, 5.0, 5.0, 4.0]
+      lon_jerk_lim: [80.0, 5.0, 5.0, 4.0] # The first element is required for quick pedal changes when stopping and starting.
+      lat_acc_lim: [5.0, 5.0, 5.0, 4.0]
+      lat_jerk_lim: [7.0, 7.0, 7.0, 6.0]
+      actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8]
     on_transition:
       vel_lim: 50.0
       reference_speed_points: [20.0, 30.0]

diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -607,7 +607,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in)
   const auto mode = current_operation_mode_;
   const auto current_status_cmd = getActualStatusAsCommand();
   const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_);
-  const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0;
 
   filter_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x);
   filter_on_transition_.setCurrentSpeed(current_kinematics_.twist.twist.linear.x);
@@ -618,23 +617,6 @@ Control VehicleCmdGate::filterControlCommand(const Control & in)
   if (mode.is_in_transition) {
     filter_on_transition_.filterAll(dt, current_steer_, out, is_filter_activated);
   } else {
-    // When ego is stopped and the input command is not stopping,
-    // use the higher of actual vehicle longitudinal state
-    // and previous longitudinal command for the filtering
-    // this is to prevent the jerk limits being applied and causing
-    // a delay when restarting the vehicle.
-
-    if (ego_is_stopped && !input_cmd_is_stopping) {
-      auto prev_cmd = filter_.getPrevCmd();
-      prev_cmd.longitudinal.acceleration =
-        std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration);
-      // consider reverse driving
-      prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) >
-                                           std::fabs(current_status_cmd.longitudinal.velocity)
-                                         ? prev_cmd.longitudinal.velocity
-                                         : current_status_cmd.longitudinal.velocity;
-      filter_.setPrevCmd(prev_cmd);
-    }
     filter_.filterAll(dt, current_steer_, out, is_filter_activated);
   }