From 8646daf30048f24a71bf8f97f27db596a1c2db99 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 27 Mar 2024 20:18:13 +0300 Subject: [PATCH] refactor(autoware_node): perameterize register timer period, use SUCCESS from Status.msg Signed-off-by: Alexey Panferov --- common/autoware_node/src/autoware_node.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/common/autoware_node/src/autoware_node.cpp b/common/autoware_node/src/autoware_node.cpp index 97504092..b17be9ec 100644 --- a/common/autoware_node/src/autoware_node.cpp +++ b/common/autoware_node/src/autoware_node.cpp @@ -18,6 +18,7 @@ #include #include "autoware_control_center_msgs/srv/autoware_node_register.hpp" +#include "autoware_control_center_msgs/msg/status.hpp" #include @@ -34,8 +35,10 @@ AutowareNode::AutowareNode( : LifecycleNode(node_name, ns, options) { RCLCPP_INFO(get_logger(), "AutowareNode::AutowareNode()"); - declare_parameter("period", 200); // TODO(lexavtanke): remove default and add schema - std::chrono::milliseconds heartbeat_period(get_parameter("period").as_int()); + declare_parameter("heartbeat_period", 200); // TODO(lexavtanke): remove default and add schema + declare_parameter("register_timer_period", 500); + std::chrono::milliseconds heartbeat_period(get_parameter("heartbeat_period").as_int()); + std::chrono::milliseconds register_timer_period(get_parameter("register_timer_period").as_int()); std::string self_namespace(this->get_namespace()); std::string name(this->get_name()); if (self_namespace.length() > 1) { @@ -66,7 +69,7 @@ AutowareNode::AutowareNode( callback_group_mut_ex_); register_timer_ = - this->create_wall_timer(500ms, std::bind(&AutowareNode::register_callback, this)); + this->create_wall_timer(register_timer_period, std::bind(&AutowareNode::register_callback, this)); using std::placeholders::_1; using std::placeholders::_2; @@ -169,7 +172,7 @@ void AutowareNode::node_register_future_callback(AutowareNodeRegisterServiceResp std::string str_uuid = autoware_utils::to_hex_string(response->uuid_node); RCLCPP_INFO(get_logger(), "response: %d, %s", response->status.status, str_uuid.c_str()); - if (response->status.status == 1) { + if (response->status.status == autoware_control_center_msgs::msg::Status::SUCCESS) { registered = true; self_uuid = response->uuid_node; RCLCPP_INFO(get_logger(), "Node was registered");