feat(pid_longitudinal_controller)!: add acceleration feedback block (autowarefoundation#8325)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml

@@ -61,6 +61,10 @@
     emergency_acc: -5.0 # denotes acceleration
     emergency_jerk: -3.0
 
+    # acceleration feedback
+    lpf_acc_error_gain: 0.98
+    acc_feedback_gain: 0.0
+
     # acceleration limit
     max_acc: 3.0
     min_acc: -5.0

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp

@@ -58,6 +58,10 @@ class DebugValues
     STOP_DIST = 28,
     FF_SCALE = 29,
     ACC_CMD_FF = 30,
+    ERROR_ACC = 31,
+    ERROR_ACC_FILTERED = 32,
+    ACC_CMD_ACC_FB_APPLIED = 33,
+
     SIZE  // this is the number of enum elements
   };
 

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp

@@ -61,6 +61,8 @@ class LowpassFilter1d
     m_x = ret;
     return ret;
   }
+
+  void setGain(const double g) { m_gain = g; }
 };
 }  // namespace autoware::motion::control::pid_longitudinal_controller
 #endif  // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -195,6 +195,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   };
   EmergencyStateParams m_emergency_state_params;
 
+  // acc feedback
+  double m_acc_feedback_gain;
+  std::shared_ptr<LowpassFilter1d> m_lpf_acc_error{nullptr};
+
   // acceleration limit
   double m_max_acc;
   double m_min_acc;

control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json

@@ -251,6 +251,16 @@
           "description": "target jerk in an EMERGENCY state [m/s^3]",
           "default": "-3.0"
         },
+        "lpf_acc_error_gain": {
+          "type": "number",
+          "description": "gain of low-pass filter for acceleration",
+          "default": "0.98"
+        },
+        "acc_feedback_gain": {
+          "type": "number",
+          "description": "acceleration feedback gain",
+          "default": "0.0"
+        },
         "max_acc": {
           "type": "number",
           "description": "max value of output acceleration [m/s^2]",

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -162,6 +162,13 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
     p.jerk = node.declare_parameter<double>("emergency_jerk");  // [m/s^3]
   }
 
+  // parameters for acc feedback
+  {
+    const double lpf_acc_error_gain{node.declare_parameter<double>("lpf_acc_error_gain")};
+    m_lpf_acc_error = std::make_shared<LowpassFilter1d>(0.0, lpf_acc_error_gain);
+    m_acc_feedback_gain = node.declare_parameter<double>("acc_feedback_gain");
+  }
+
   // parameters for acceleration limit
   m_max_acc = node.declare_parameter<double>("max_acc");  // [m/s^2]
   m_min_acc = node.declare_parameter<double>("min_acc");  // [m/s^2]
@@ -289,6 +296,10 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
     update_param("kd", kd);
     m_pid_vel.setGains(kp, ki, kd);
 
+    double lpf_vel_error_gain{node_parameters_->get_parameter("lpf_vel_error_gain").as_double()};
+    update_param("lpf_vel_error_gain", lpf_vel_error_gain);
+    m_lpf_vel_error->setGain(lpf_vel_error_gain);
+
     double max_pid{node_parameters_->get_parameter("max_out").as_double()};
     double min_pid{node_parameters_->get_parameter("min_out").as_double()};
     double max_p{node_parameters_->get_parameter("max_p_effort").as_double()};
@@ -363,6 +374,12 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
     update_param("emergency_jerk", p.jerk);
   }
 
+  // acceleration feedback
+  update_param("acc_feedback_gain", m_acc_feedback_gain);
+  double lpf_acc_error_gain{node_parameters_->get_parameter("lpf_acc_error_gain").as_double()};
+  update_param("lpf_acc_error_gain", lpf_acc_error_gain);
+  m_lpf_acc_error->setGain(lpf_acc_error_gain);
+
   // acceleration limit
   update_param("min_acc", m_min_acc);
 
@@ -751,6 +768,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
+      m_lpf_acc_error->reset(0.0);
       return changeState(ControlState::DRIVE);
     }
 
@@ -842,8 +860,22 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     // store acceleration without slope compensation
     m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
 
+    // calc acc feedback
+    const double acc_err = control_data.current_motion.acc - raw_ctrl_cmd.acc;
+    m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC, acc_err);
+    m_lpf_acc_error->filter(acc_err);
+    m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC_FILTERED, m_lpf_acc_error->getValue());
+
+    const double acc_cmd = raw_ctrl_cmd.acc - m_lpf_acc_error->getValue() * m_acc_feedback_gain;
+    m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_FB_APPLIED, acc_cmd);
+    RCLCPP_DEBUG(
+      logger_,
+      "[acc feedback]: raw_ctrl_cmd.acc: %1.3f, control_data.current_motion.acc: %1.3f, acc_cmd: "
+      "%1.3f",
+      raw_ctrl_cmd.acc, control_data.current_motion.acc, acc_cmd);
+
     ctrl_cmd_as_pedal_pos.acc =
-      applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift);
+      applySlopeCompensation(acc_cmd, 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;
   }