From b1f0b38308f866139ec7fff1a74c6b2ac59895a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 23 Oct 2026 16:07:45 +0200 Subject: [PATCH] FusionLocalizer: wait for the clock with a warning, stop on shutdown MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Refs #143 Signed-off-by: Francisco Martín Rico --- .../easynav_fusion_localizer/package.xml | 1 + .../easynav_fusion_localizer/ukf_wrapper.cpp | 20 ++- .../tests/CMakeLists.txt | 9 + .../fusion_localizer_clock_wait_tests.cpp | 165 ++++++++++++++++++ 4 files changed, 192 insertions(+), 3 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/tests/fusion_localizer_clock_wait_tests.cpp diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 5cc4358e..8ca41f1a 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -37,6 +37,7 @@ ament_lint_auto ament_lint_common robot_localization + rosgraph_msgs ament_cmake diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 0b80da65..4e2f1a90 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -2313,9 +2313,23 @@ void UkfWrapper::poseCallback( void UkfWrapper::initialize() { - if (!parent_node_->get_clock()->started()) { - RCLCPP_INFO(parent_node_->get_logger(), "Waiting for clock to start..."); - parent_node_->get_clock()->wait_until_started(); + // With use_sim_time and no /clock this blocks the configuration of every EasyNav node: say + // so until it starts, and stop waiting if rclcpp is shut down (a second signal) + const auto context = parent_node_->get_node_base_interface()->get_context(); + const rclcpp::Duration step = rclcpp::Duration::from_seconds(5.0); + auto started = [&]() { + try { + return parent_node_->get_clock()->wait_until_started(step, context); + } catch (const std::runtime_error &) { + return false; // the context was shut down + } + }; + double waited = 0.0; + while (rclcpp::ok(context) && !started() && rclcpp::ok(context)) { + waited += step.seconds(); + RCLCPP_WARN( + parent_node_->get_logger(), "Waiting for the ROS clock to start (%.0f s): use_sim_time is " + "true and no /clock reaches this node. EasyNav does not configure until it does", waited); } angular_acceleration_.setZero(); diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt index bdbeca1c..6cc4e0ed 100644 --- a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -26,3 +26,12 @@ target_link_libraries(fusion_localizer_local_filter_tests rclcpp::rclcpp easynav_localizer::easynav_localizer ) + +find_package(rosgraph_msgs REQUIRED) +ament_add_gtest(fusion_localizer_clock_wait_tests fusion_localizer_clock_wait_tests.cpp) +target_link_libraries(fusion_localizer_clock_wait_tests + fusion_localizer + rclcpp::rclcpp + easynav_localizer::easynav_localizer + ${rosgraph_msgs_TARGETS} +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_clock_wait_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_clock_wait_tests.cpp new file mode 100644 index 00000000..17ccfc2b --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_clock_wait_tests.cpp @@ -0,0 +1,165 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief With use_sim_time, the filters wait for /clock while configuring, but a shutdown +/// (a second signal to system_main) stops the wait. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcutils/logging.h" +#include "rosgraph_msgs/msg/clock.hpp" + +using namespace std::chrono_literals; + +namespace +{ + +std::atomic g_clock_warnings{0}; + +void count_clock_warnings( + const rcutils_log_location_t *, int severity, const char *, rcutils_time_point_value_t, + const char * format, va_list * args) +{ + char msg[512]; + va_list copy; + va_copy(copy, *args); + std::vsnprintf(msg, sizeof(msg), format, copy); + va_end(copy); + if (severity == RCUTILS_LOG_SEVERITY_WARN && + std::string(msg).find("Waiting for the ROS clock to start") != std::string::npos) + { + ++g_clock_warnings; + } +} + +} // namespace + +class FusionClockWaitTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override + { + if (rclcpp::ok()) {rclcpp::shutdown();} + } + + // Initializes a local filter in another thread; the node is kept in node_. + std::future initialize(bool use_sim_time) + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", use_sim_time}, + {"fusion.local_filter.frequency", 30.0}, + {"fusion.local_filter.two_d_mode", true}, + {"fusion.local_filter.publish_tf", false}, + {"fusion.local_filter.world_frame", "odom"}, + {"fusion.local_filter.odom0", "clock_wait_odom"}, + }); + node_ = std::make_shared(options); + node_->declare_parameter>( + "localizer_types", std::vector{}); + localizer_ = std::make_shared(); + return std::async( + std::launch::async, [this]() {localizer_->initialize(node_, "fusion");}); + } + + std::shared_ptr node_; + std::shared_ptr localizer_; +}; + +TEST_F(FusionClockWaitTest, WithoutSimTimeItDoesNotWait) +{ + auto done = initialize(false); + ASSERT_EQ(done.wait_for(5s), std::future_status::ready); + EXPECT_NO_THROW(done.get()); +} + +TEST_F(FusionClockWaitTest, WithSimTimeItWaitsUntilTheClockStarts) +{ + auto done = initialize(true); + EXPECT_EQ(done.wait_for(1s), std::future_status::timeout); // no /clock yet + + auto clock_node = std::make_shared("clock_wait_publisher"); + auto pub = clock_node->create_publisher("/clock", 10); + rosgraph_msgs::msg::Clock clock; + clock.clock.sec = 100; + const auto end = std::chrono::steady_clock::now() + 10s; + while (done.wait_for(50ms) != std::future_status::ready && + std::chrono::steady_clock::now() < end) + { + pub->publish(clock); + } + ASSERT_EQ(done.wait_for(0s), std::future_status::ready); + EXPECT_NO_THROW(done.get()); + EXPECT_EQ(node_->now().seconds(), 100.0); +} + +TEST_F(FusionClockWaitTest, WhileItWaitsItSaysWhy) +{ + const auto handler = rcutils_logging_get_output_handler(); + rcutils_logging_set_output_handler(count_clock_warnings); + g_clock_warnings = 0; + auto done = initialize(true); + EXPECT_EQ(done.wait_for(4s), std::future_status::timeout); + EXPECT_EQ(g_clock_warnings, 0); // every 5 s + EXPECT_EQ(done.wait_for(2s), std::future_status::timeout); + EXPECT_EQ(g_clock_warnings, 1); + rclcpp::shutdown(); + ASSERT_EQ(done.wait_for(5s), std::future_status::ready); + rcutils_logging_set_output_handler(handler); + try { + done.get(); + } catch (const std::exception &) { + } +} + +TEST_F(FusionClockWaitTest, AShutdownStopsTheWait) +{ + auto done = initialize(true); + EXPECT_EQ(done.wait_for(1s), std::future_status::timeout); + rclcpp::shutdown(); + // Stops waiting; what is left of the initialization may fail without a context + ASSERT_EQ(done.wait_for(5s), std::future_status::ready); + try { + done.get(); + } catch (const std::exception &) { + } +} + +// Before or while it waits, whichever comes first +TEST_F(FusionClockWaitTest, AShutdownRightAwayDoesNotHang) +{ + auto done = initialize(true); + rclcpp::shutdown(); + ASSERT_EQ(done.wait_for(5s), std::future_status::ready); + try { + done.get(); + } catch (const std::exception &) { + } +}