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 &) {
+ }
+}