From f2b6519e0c272b21258a41ee03aca738110eac43 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 24 Jun 2024 14:07:14 +0900 Subject: [PATCH 1/3] diag_updater_ added in PID Signed-off-by: Zhe Shen --- .../pid_longitudinal_controller.hpp | 6 +++--- .../src/pid_longitudinal_controller.cpp | 13 +++++++------ .../src/controller_node.cpp | 2 +- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 9291d538b022f..15706f12a7a84 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -64,7 +64,7 @@ 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 diag_updater); private: struct Motion @@ -236,8 +236,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic - - diagnostic_updater::Updater diagnostic_updater_; + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. struct DiagnosticData { double trans_deviation{0.0}; // translation deviation between nearest point and current_pose diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 78c7ccf832514..f462be3c33246 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -27,14 +27,15 @@ namespace autoware::motion::control::pid_longitudinal_controller { -PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) +PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node, std::shared_ptr 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 +433,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( publishDebugData(ctrl_cmd, control_data); // diagnostic - diagnostic_updater_.force_update(); + diag_updater_.force_update(); return output; } @@ -1150,8 +1151,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( diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index c1e5fe646cdaa..1add357332b16 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -57,7 +57,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control switch (longitudinal_controller_mode) { case LongitudinalControllerMode::PID: { longitudinal_controller_ = - std::make_shared(*this); + std::make_shared(*this, diag_updater_); break; } default: From 35c9563ceff81dc923bb8b6f7057c550f40eb19a Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Mon, 24 Jun 2024 14:10:01 +0900 Subject: [PATCH 2/3] correct the pointer form Signed-off-by: Zhe Shen --- .../src/pid_longitudinal_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index f462be3c33246..bc53e1486164d 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -433,7 +433,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( publishDebugData(ctrl_cmd, control_data); // diagnostic - diag_updater_.force_update(); + diag_updater_->force_update(); return output; } From e71b8f4cdc2129d19b0c3788706af60fbf51e584 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Tue, 25 Jun 2024 10:56:48 +0900 Subject: [PATCH 3/3] pre-commit Signed-off-by: Zhe Shen --- .../pid_longitudinal_controller.hpp | 3 ++- .../src/pid_longitudinal_controller.cpp | 3 ++- .../autoware_trajectory_follower_node/src/controller_node.cpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 15706f12a7a84..5805ef7db9f86 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/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, std::shared_ptr diag_updater); + explicit PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater); private: struct Motion diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index bc53e1486164d..93496c85c741b 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -27,7 +27,8 @@ namespace autoware::motion::control::pid_longitudinal_controller { -PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node, std::shared_ptr diag_updater) +PidLongitudinalController::PidLongitudinalController( + rclcpp::Node & node, std::shared_ptr diag_updater) : node_parameters_(node.get_node_parameters_interface()), clock_(node.get_clock()), logger_(node.get_logger().get_child("longitudinal_controller")) diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index 1add357332b16..d5167f5096786 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/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(*this, diag_updater_); + std::make_shared( + *this, diag_updater_); break; } default: