From 3118a0928a67e747946f5bd07dba28394a73b7e5 Mon Sep 17 00:00:00 2001 From: anhnv3991 Date: Tue, 21 May 2024 10:59:39 +0700 Subject: [PATCH] Clean code Signed-off-by: anhnv3991 --- .../src/map_update_module.cpp | 36 ++++++++----------- 1 file changed, 15 insertions(+), 21 deletions(-) diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 925fc9d95de0b..97e9f069bcc1d 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -217,31 +217,25 @@ bool MapUpdateModule::update_ndt( // Wait for maximum 10 milliseconds std::chrono::milliseconds timeout(10); - auto start = std::chrono::system_clock::now(); + std::future_status status = result.wait_for(timeout); - std::future_status status = result.wait_for(std::chrono::seconds(0)); - while (status != std::future_status::ready) { - // check is_succeed_call_pcd_loader - if (!rclcpp::ok()) { - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); - - std::stringstream message; - message << "pcd_loader service is not working."; - diagnostics_ptr->updateLevelAndMessage( - diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - return false; // No update - } - - auto cur = std::chrono::system_clock::now(); + if (status == std::future_status::timeout) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 1000, "Waited for incoming PCDs for too long!"); + } - // Report an error if wait for too long - if (cur - start >= timeout) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 1000, "Waited for incoming PCDs for too long..."); - } + if (status != std::future_status::ready) + { + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); - status = result.wait_for(std::chrono::seconds(1)); + std::stringstream message; + message << "pcd_loader service is not working."; + diagnostics_ptr->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + + return false; } + diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); auto & maps_to_add = result.get()->new_pointcloud_with_ids;