feat(pid_longitudinal_controller): update trajectory_adaptive; add debug_values, adopt rate limit fillter (#9656)

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

Author: yuki-takagi-66

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

@@ -34,9 +34,9 @@ class DebugValues
     NEAREST_VEL = 4,
     NEAREST_ACC = 5,
     SHIFT = 6,
-    PITCH_LPF_RAD = 7,
+    PITCH_USING_RAD = 7,
     PITCH_RAW_RAD = 8,
-    PITCH_LPF_DEG = 9,
+    PITCH_USING_DEG = 9,
     PITCH_RAW_DEG = 10,
     ERROR_VEL = 11,
     ERROR_VEL_FILTERED = 12,
@@ -61,6 +61,8 @@ class DebugValues
     ERROR_ACC = 31,
     ERROR_ACC_FILTERED = 32,
     ACC_CMD_ACC_FB_APPLIED = 33,
+    PITCH_LPF_RAD = 34,
+    PITCH_LPF_DEG = 35,
 
     SIZE  // this is the number of enum elements
   };

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

@@ -216,6 +216,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
   double m_max_pitch_rad;
   double m_min_pitch_rad;
+  std::optional<double> m_previous_slope_angle{std::nullopt};
 
   // ego nearest index search
   double m_ego_nearest_dist_threshold;
@@ -411,11 +412,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   /**
    * @brief update variables for debugging about pitch
-   * @param [in] pitch current pitch of the vehicle (filtered)
-   * @param [in] traj_pitch current trajectory pitch
-   * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered)
+   * @param [in] pitch_using
+   * @param [in] traj_pitch
+   * @param [in] localization_pitch
+   * @param [in] localization_pitch_lpf
    */
-  void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch);
+  void updatePitchDebugValues(
+    const double pitch_using, const double traj_pitch, const double localization_pitch,
+    const double localization_pitch_lpf);
 
   /**
    * @brief update variables for velocity and acceleration

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -584,13 +584,21 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
     } else {
       control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
     }
+    if (m_previous_slope_angle.has_value()) {
+      constexpr double gravity_const = 9.8;
+      control_data.slope_angle = std::clamp(
+        control_data.slope_angle,
+        m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const,
+        m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const);
+    }
   } else {
     RCLCPP_ERROR_THROTTLE(
       logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default");
     control_data.slope_angle = m_lpf_pitch->filter(raw_pitch);
   }
 
-  updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch);
+  m_previous_slope_angle = control_data.slope_angle;
+  updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue());
 
   return control_data;
 }
@@ -1189,13 +1197,16 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
 }
 
 void PidLongitudinalController::updatePitchDebugValues(
-  const double pitch, const double traj_pitch, const double raw_pitch)
+  const double pitch_using, const double traj_pitch, const double localization_pitch,
+  const double localization_pitch_lpf)
 {
   const double to_degrees = (180.0 / static_cast<double>(M_PI));
-  m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch);
-  m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees);
-  m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch);
-  m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_RAD, pitch_using);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_DEG, pitch_using * to_degrees);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, localization_pitch_lpf);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, localization_pitch_lpf * to_degrees);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, localization_pitch);
+  m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, localization_pitch * to_degrees);
   m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch);
   m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees);
 }