po

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
autowarefoundation · Aug 26, 2024 · 6d450f0 · 6d450f0
1 parent 5e8d2d5
commit 6d450f0
Show file tree

Hide file tree

Showing 4 changed files with 140 additions and 425 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
@@ -169,7 +169,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   // drive
   PIDController m_pid_vel;
-  std::unique_ptr<LowpassFilter1d> m_lpf_vel_error{nullptr};
+  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;
@@ -198,7 +198,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 
   // acc feedback
   double m_acc_feedback_gain;
-  std::unique_ptr<LowpassFilter1d> m_lpf_acc_error{nullptr};
+  std::shared_ptr<LowpassFilter1d> m_lpf_acc_error{nullptr};
 
   // acceleration limit
   double m_max_acc;
@@ -213,7 +213,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE };
   SlopeSource m_slope_source{SlopeSource::RAW_PITCH};
   double m_adaptive_trajectory_velocity_th;
-  std::unique_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
+  std::shared_ptr<LowpassFilter1d> m_lpf_pitch{nullptr};
   double m_max_pitch_rad;
   double m_min_pitch_rad;
 

diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -104,7 +104,7 @@ PidLongitudinalController::PidLongitudinalController(
 
     // set lowpass filter for vel error and pitch
     const double lpf_vel_error_gain{node.declare_parameter<double>("lpf_vel_error_gain")};
-    m_lpf_vel_error = std::make_unique<LowpassFilter1d>(0.0, 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");
@@ -167,7 +167,7 @@ PidLongitudinalController::PidLongitudinalController(
   // parameters for acc feedback
   {
     const double lpf_acc_error_gain{node.declare_parameter<double>("lpf_acc_error_gain")};
-    m_lpf_acc_error = std::make_unique<LowpassFilter1d>(0.0, 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");
   }
 
@@ -184,7 +184,7 @@ PidLongitudinalController::PidLongitudinalController(
   m_adaptive_trajectory_velocity_th =
     node.declare_parameter<double>("adaptive_trajectory_velocity_th");  // [m/s^2]
   const double lpf_pitch_gain{node.declare_parameter<double>("lpf_pitch_gain")};
-  m_lpf_pitch = std::make_unique<LowpassFilter1d>(0.0, lpf_pitch_gain);
+  m_lpf_pitch = std::make_shared<LowpassFilter1d>(0.0, lpf_pitch_gain);
   m_max_pitch_rad = node.declare_parameter<double>("max_pitch_rad");  // [rad]
   m_min_pitch_rad = node.declare_parameter<double>("min_pitch_rad");  // [rad]
 
@@ -872,9 +872,9 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     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, "
-      "m_lpf_acc_error.getValue(): %1.3f, acc_cmd: %1.3f",
-      raw_ctrl_cmd.acc, control_data.current_motion.acc, 0.0, acc_cmd);
+      "[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(acc_cmd, control_data.slope_angle, control_data.shift);