From 74edf5432a82d3cd3bd310e69b84b6692e139a17 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 13 Feb 2024 16:43:51 +0300 Subject: [PATCH] feat(autoware_control_center): update startup callback Signed-off-by: Alexey Panferov --- .../src/autoware_control_center.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/common/autoware_control_center/src/autoware_control_center.cpp b/common/autoware_control_center/src/autoware_control_center.cpp index 7766651fe8..6b0356f891 100644 --- a/common/autoware_control_center/src/autoware_control_center.cpp +++ b/common/autoware_control_center/src/autoware_control_center.cpp @@ -48,7 +48,7 @@ AutowareControlCenter::AutowareControlCenter(const rclcpp::NodeOptions & options std::chrono::milliseconds lease_duration_(get_parameter("lease_duration").as_int()); callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - + using std::placeholders::_1; using std::placeholders::_2; srv_register_ = create_service( @@ -123,9 +123,15 @@ void AutowareControlCenter::deregister_node( void AutowareControlCenter::startup_callback() { // wait for 10 sec and + // check if some node has been registered + if (node_registry_.is_empty()) { + RCLCPP_INFO(get_logger(), "Node register map is empty. Countdown is %d", countdown); + } if (countdown < 1 && node_registry_.is_empty() && startup) { RCLCPP_INFO( get_logger(), "Startup timeout is over. Map is empty. Start re-registering procedure."); + this->startup_timer_->cancel(); + RCLCPP_INFO(get_logger(), "Startup timer stop."); // list auwoware nodes std::map> srv_list = this->get_service_names_and_types(); auto it = srv_list.begin(); @@ -143,7 +149,7 @@ void AutowareControlCenter::startup_callback() rclcpp::Client::SharedPtr dereg_client_ = create_client( - pair.first); + pair.first); autoware_control_center_msgs::srv::AutowareControlCenterDeregister::Request::SharedPtr req = std::make_shared< autoware_control_center_msgs::srv::AutowareControlCenterDeregister::Request>(); @@ -156,7 +162,7 @@ void AutowareControlCenter::startup_callback() auto response_received_callback = [this](ServiceResponseFuture future) { auto response = future.get(); RCLCPP_INFO( - get_logger(), "response: %d, %s", response->status.status, response->name_node.c_str()); + get_logger(), "Deregister response: %d, %s", response->status.status, response->name_node.c_str()); if (response->status.status == 1) { RCLCPP_INFO(get_logger(), "Node was deregistered"); @@ -172,13 +178,6 @@ void AutowareControlCenter::startup_callback() } startup = false; } - // check if some node has been registered - if (node_registry_.is_empty()) { - RCLCPP_INFO(get_logger(), "Node register map is empty. Countdown is %d", countdown); - } else { - RCLCPP_INFO(get_logger(), "Some node was registered. Stop timer."); - this->startup_timer_->cancel(); - } countdown -= 1; }