Fix FixedLagSmoother crash on first optimization cycle in auto-start mode

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

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.

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)

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

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 <gtest/gtest.h>
+
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+class FixedLagAutostartFixture : public ::testing::Test
+{
+public:
+  FixedLagAutostartFixture()
+  {
+  }
+
+  void SetUp() override
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    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<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "/absolute_pose", 5);
+
+  auto odom_subscriber =
+    node->create_subscription<nav_msgs::msg::Odometry>(
+    "/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<bool>(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;
+}

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'