From ce2ad34d8d07a1ad77ff05d97cf6773cabcd3092 Mon Sep 17 00:00:00 2001 From: Daniil Khaninaev <21182465+ikhann@users.noreply.github.com> Date: Sat, 15 Aug 2026 15:57:51 +0300 Subject: [PATCH] Fix FixedLagSmoother crash on first optimization cycle in auto-start mode The lag_expiration_ member was default-constructed, which produces an rclcpp::Time with the RCL_SYSTEM_TIME clock type. On the first optimization cycle it is passed to processQueue() before ever being assigned, where it is compared against RCL_ROS_TIME sensor stamps. rclcpp throws "can't compare times with different time sources" for such comparisons, the exception escapes the optimization thread, and the node aborts via std::terminate. --- .../fuse_optimizers/fixed_lag_smoother.hpp | 3 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 1 - fuse_optimizers/test/CMakeLists.txt | 13 ++ .../config/fixed_lag_autostart_params.yaml | 39 ++++ .../launch_tests/test_fixed_lag_autostart.cpp | 178 ++++++++++++++++++ .../launch_tests/test_fixed_lag_autostart.py | 67 +++++++ 6 files changed, 299 insertions(+), 2 deletions(-) create mode 100644 fuse_optimizers/test/launch_tests/config/fixed_lag_autostart_params.yaml create mode 100644 fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.cpp create mode 100644 fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.py diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index ef23470e4..64c2a2a47 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -184,7 +184,8 @@ class FixedLagSmoother : public Optimizer // Guarded by optimization_mutex_ std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized // fuse_core::Graph* graph_ member from the base class - rclcpp::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window + rclcpp::Time lag_expiration_ {0, 0, RCL_ROS_TIME}; //!< The oldest stamp that is inside the + //!< fixed-lag smoother window fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next //!< optimization cycle VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index c7de2e75e..ce38f9f6e 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -227,7 +227,6 @@ void FixedLagSmoother::optimizationLoop() // pending_transactions queue and obtaining the lock for the graph. But we have now // obtained two different locks. If we are not extremely careful, we could get a // deadlock. - // TODO(CH3): We might have to make sure lag_expiration_ has been initialised processQueue(*new_transaction, lag_expiration_); // Skip this optimization cycle if the transaction is empty because something failed while // processing the pending transactions queue. diff --git a/fuse_optimizers/test/CMakeLists.txt b/fuse_optimizers/test/CMakeLists.txt index fec394154..578342f85 100644 --- a/fuse_optimizers/test/CMakeLists.txt +++ b/fuse_optimizers/test/CMakeLists.txt @@ -36,9 +36,22 @@ ament_add_pytest_test( "${CMAKE_CURRENT_BINARY_DIR}" ) +ament_add_gtest_executable(test_fixed_lag_autostart launch_tests/test_fixed_lag_autostart.cpp) +target_link_libraries(test_fixed_lag_autostart ${PROJECT_NAME} ${nav_msgs_TARGETS}) + +ament_add_pytest_test( + test_fixed_lag_autostart_py + "launch_tests/test_fixed_lag_autostart.py" + WORKING_DIRECTORY + "${CMAKE_CURRENT_BINARY_DIR}" +) + configure_file( "launch_tests/config/optimizer_params.yaml" "launch_tests/config/optimizer_params.yaml" COPYONLY) configure_file( "launch_tests/config/fixed_lag_ignition_params.yaml" "launch_tests/config/fixed_lag_ignition_params.yaml" COPYONLY) +configure_file( + "launch_tests/config/fixed_lag_autostart_params.yaml" + "launch_tests/config/fixed_lag_autostart_params.yaml" COPYONLY) diff --git a/fuse_optimizers/test/launch_tests/config/fixed_lag_autostart_params.yaml b/fuse_optimizers/test/launch_tests/config/fixed_lag_autostart_params.yaml new file mode 100644 index 000000000..c8c1dd655 --- /dev/null +++ b/fuse_optimizers/test/launch_tests/config/fixed_lag_autostart_params.yaml @@ -0,0 +1,39 @@ +fixed_lag_node: + ros__parameters: + optimization_frequency: 2.0 + transaction_timeout: 5.0 + lag_duration: 5.0 + + + motion_models: + unicycle_motion_model: + type: fuse_models::Unicycle2D + + + # No ignition sensors are configured on purpose, so the smoother auto-starts + sensor_models: + pose_sensor: + type: fuse_models::Pose2D + motion_models: [unicycle_motion_model] + + + publishers: + + odometry_publisher: + type: fuse_models::Odometry2DPublisher + + + unicycle_motion_model: + buffer_length: 5.0 + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + pose_sensor: + differential: false + topic: absolute_pose + position_dimensions: ['x', 'y'] + orientation_dimensions: ['yaw'] + + odometry_publisher: + topic: odom + world_frame_id: map + publish_tf: false diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.cpp new file mode 100644 index 000000000..494fcdb1a --- /dev/null +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.cpp @@ -0,0 +1,178 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2026, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include + +#include +#include +#include + +class FixedLagAutostartFixture : public ::testing::Test +{ +public: + FixedLagAutostartFixture() + { + } + + void SetUp() override + { + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + } + + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) + { + std::lock_guard lock(received_odom_mutex_); + received_odom_msg_ = msg; + } + + nav_msgs::msg::Odometry::SharedPtr get_last_odom_msg() + { + std::lock_guard lock(received_odom_mutex_); + return received_odom_msg_; + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + nav_msgs::msg::Odometry::SharedPtr received_odom_msg_; + std::mutex received_odom_mutex_; +}; + +TEST_F(FixedLagAutostartFixture, AutostartProcessesFirstTransaction) +{ + // No ignition sensors are configured, so the smoother auto-starts and must process the very + // first sensor transaction. This is a regression test for the optimizer thread terminating on + // the first optimization cycle because lag_expiration_ was constructed with the wrong clock type. + auto node = rclcpp::Node::make_shared("fixed_lag_autostart_test"); + executor_->add_node(node); + + auto pose_publisher = + node->create_publisher( + "/absolute_pose", 5); + + auto odom_subscriber = + node->create_subscription( + "/odom", 5, + std::bind(&FixedLagAutostartFixture::odom_callback, this, std::placeholders::_1)); + + // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify. + ASSERT_TRUE(node->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); + + // The smoother auto-starts, so the sensors subscribe to their topics on startup. + // I need to wait for those subscribers to be ready before sending them sensor data. + rclcpp::Time subscriber_timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((pose_publisher->get_subscription_count() < 1u) && + (node->now() < subscriber_timeout)) + { + rclcpp::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_GE(pose_publisher->get_subscription_count(), 1u); + + // Publish an absolute pose measurement + auto pose_msg1 = geometry_msgs::msg::PoseWithCovarianceStamped(); + pose_msg1.header.stamp = rclcpp::Time(2, 0, RCL_ROS_TIME); + pose_msg1.header.frame_id = "map"; + pose_msg1.pose.pose.position.x = 100.1; + pose_msg1.pose.pose.position.y = 100.2; + pose_msg1.pose.pose.position.z = 0.0; + pose_msg1.pose.pose.orientation.x = 0.0; + pose_msg1.pose.pose.orientation.y = 0.0; + pose_msg1.pose.pose.orientation.z = 0.8660; + pose_msg1.pose.pose.orientation.w = 0.5000; + pose_msg1.pose.covariance[0] = 1.0; + pose_msg1.pose.covariance[7] = 1.0; + pose_msg1.pose.covariance[35] = 1.0; + pose_publisher->publish(pose_msg1); + + // Force a delay between publishing, otherwise the subscriber does not receive all the messages + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + auto pose_msg2 = geometry_msgs::msg::PoseWithCovarianceStamped(); + pose_msg2.header.stamp = rclcpp::Time(3, 0, RCL_ROS_TIME); + pose_msg2.header.frame_id = "map"; + pose_msg2.pose.pose.position.x = 100.1; + pose_msg2.pose.pose.position.y = 100.2; + pose_msg2.pose.pose.position.z = 0.0; + pose_msg2.pose.pose.orientation.x = 0.0; + pose_msg2.pose.pose.orientation.y = 0.0; + pose_msg2.pose.pose.orientation.z = 0.8660; + pose_msg2.pose.pose.orientation.w = 0.5000; + pose_msg2.pose.covariance[0] = 1.0; + pose_msg2.pose.covariance[7] = 1.0; + pose_msg2.pose.covariance[35] = 1.0; + pose_publisher->publish(pose_msg2); + + // Wait for the optimizer to process all queued transactions and publish the last odometry msg + rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(5.0); + auto odom_msg = nav_msgs::msg::Odometry::SharedPtr(); + while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, + RCL_ROS_TIME)) && (node->now() < result_timeout)) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + odom_msg = this->get_last_odom_msg(); + } + ASSERT_TRUE(static_cast(odom_msg)); + ASSERT_EQ(rclcpp::Time(odom_msg->header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME)); + + // Both pose measurements are identical, so the optimized state should converge to them. + EXPECT_NEAR(100.1, odom_msg->pose.pose.position.x, 0.10); + EXPECT_NEAR(100.2, odom_msg->pose.pose.position.y, 0.10); + EXPECT_NEAR(0.8660, odom_msg->pose.pose.orientation.z, 0.10); + EXPECT_NEAR(0.5000, odom_msg->pose.pose.orientation.w, 0.10); +} + +// NOTE(CH3): This main is required because the test is manually run by a launch test +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.py b/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.py new file mode 100644 index 000000000..a426ca960 --- /dev/null +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_autostart.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python3 + +# Copyright 2026 Open Source Robotics Foundation, Inc. +# +# 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. + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.substitutions import PathJoinSubstitution + +import launch_pytest +from launch_pytest.actions import ReadyToTest +from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node + +import pytest + + +@pytest.fixture +def test_proc(): + test_root = '.' + test_path = os.path.join(test_root, 'test_fixed_lag_autostart') + cmd = [test_path] + return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) + + +# Must not be named `generate_test_description`, or launch_testing hijacks collection of this +# file as a classic launch test and fails on the `test_proc` fixture argument. +@launch_pytest.fixture +def make_launch_description(test_proc): + test_root = '.' + + return LaunchDescription( + [ + test_proc, + Node( + package='fuse_optimizers', + executable='fixed_lag_smoother_node', + name='fixed_lag_node', + output='screen', + parameters=[ + PathJoinSubstitution( + [test_root, 'launch_tests', 'config', 'fixed_lag_autostart_params.yaml'] + ) + ], + ), + ReadyToTest() + ] + ) + + +@pytest.mark.launch(fixture=make_launch_description) +async def test_no_failed_gtests(test_proc, launch_context): + await process_tools.wait_for_exit(launch_context, test_proc, timeout=30) + assert test_proc.return_code == 0, 'GTests failed'