fix(autoware_pid_longitudinal_controller, autoware_trajectory_followe…

…r_node): unite diagnostic_updater_ in PID and MPC. (autowarefoundation#7674)

* diag_updater_ added in PID

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* correct the pointer form

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

* pre-commit

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

---------

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>
Signed-off-by: Simon Eisenmann <simon.eisenmann@driveblocks.ai>

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

@@ -64,7 +64,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 {
 public:
   /// \param node Reference to the node used only for the component and parameter initialization.
-  explicit PidLongitudinalController(rclcpp::Node & node);
+  explicit PidLongitudinalController(
+    rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater);
 
 private:
   struct Motion
@@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};
 
   // Diagnostic
-
-  diagnostic_updater::Updater diagnostic_updater_;
+  std::shared_ptr<diagnostic_updater::Updater>
+    diag_updater_{};  // Diagnostic updater for publishing diagnostic data.
   struct DiagnosticData
   {
     double trans_deviation{0.0};  // translation deviation between nearest point and current_pose

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -27,14 +27,16 @@
 
 namespace autoware::motion::control::pid_longitudinal_controller
 {
-PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
+PidLongitudinalController::PidLongitudinalController(
+  rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater)
 : node_parameters_(node.get_node_parameters_interface()),
   clock_(node.get_clock()),
-  logger_(node.get_logger().get_child("longitudinal_controller")),
-  diagnostic_updater_(&node)
+  logger_(node.get_logger().get_child("longitudinal_controller"))
 {
   using std::placeholders::_1;
 
+  diag_updater_ = diag_updater;
+
   // parameters timer
   m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();
 
@@ -432,7 +434,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
   publishDebugData(ctrl_cmd, control_data);
 
   // diagnostic
-  diagnostic_updater_.force_update();
+  diag_updater_->force_update();
 
   return output;
 }
@@ -1150,8 +1152,8 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da
 
 void PidLongitudinalController::setupDiagnosticUpdater()
 {
-  diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller");
-  diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState);
+  diag_updater_->setHardwareID("autoware_pid_longitudinal_controller");
+  diag_updater_->add("control_state", this, &PidLongitudinalController::checkControlState);
 }
 
 void PidLongitudinalController::checkControlState(

control/autoware_trajectory_follower_node/src/controller_node.cpp

@@ -57,7 +57,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
   switch (longitudinal_controller_mode) {
     case LongitudinalControllerMode::PID: {
       longitudinal_controller_ =
-        std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(*this);
+        std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(
+          *this, diag_updater_);
       break;
     }
     default: