feat(pid_longitudinal_controller): update trajectory_adaptive; add de…

…bug_values, adopt rate limit fillter (autowarefoundation#9656) Signed-off-by: yuki-takagi-66 <[email protected]>
tier4 · Dec 16, 2024 · 1ede26e · 1ede26e
1 parent 3326ba5
commit 1ede26e
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 12 deletions.
diff --git a/...pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/...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,
@@ -58,6 +58,8 @@ class DebugValues
     STOP_DIST = 28,
     FF_SCALE = 29,
     ACC_CMD_FF = 30,
+    PITCH_LPF_RAD = 31,
+    PITCH_LPF_DEG = 32,
     SIZE  // this is the number of enum elements
   };
 

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
@@ -211,6 +211,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;
@@ -393,11 +394,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

diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -556,13 +556,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;
 }
@@ -1139,13 +1147,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);
 }