From 2c709dbd8a798a8e5b33588bc71f16cbeb239095 Mon Sep 17 00:00:00 2001
From: Stephen Williams <swilliams@locusrobotics.com>
Date: Mon, 29 Jul 2024 17:44:14 -0700
Subject: [PATCH] Add ability to leave the fixed-lag smoother stopped on
 construction

---
 .../include/fuse_optimizers/fixed_lag_smoother_params.h   | 7 +++++++
 fuse_optimizers/src/batch_optimizer.cpp                   | 2 ++
 fuse_optimizers/src/fixed_lag_smoother.cpp                | 8 +++++---
 fuse_optimizers/src/optimizer.cpp                         | 3 ---
 4 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
index 2b07db863..3e045b6dc 100644
--- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
+++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
@@ -55,6 +55,11 @@ namespace fuse_optimizers
 struct FixedLagSmootherParams
 {
 public:
+  /**
+   * @brief If true, the state estimator will not start until the start or reset service is called
+   */
+  bool disabled_at_startup { false };
+
   /**
    * @brief The duration of the smoothing window in seconds
    */
@@ -105,6 +110,8 @@ struct FixedLagSmootherParams
   void loadFromROS(const ros::NodeHandle& nh)
   {
     // Read settings from the parameter server
+    nh.getParam("disabled_at_startup", disabled_at_startup);
+
     fuse_core::getPositiveParam(nh, "lag_duration", lag_duration);
 
     if (nh.hasParam("optimization_frequency"))
diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp
index 6784a5a7f..969749ea7 100644
--- a/fuse_optimizers/src/batch_optimizer.cpp
+++ b/fuse_optimizers/src/batch_optimizer.cpp
@@ -82,6 +82,8 @@ BatchOptimizer::BatchOptimizer(
     ros::names::resolve(params_.start_service),
     &BatchOptimizer::startServiceCallback,
     this);
+
+  startPlugins();
 }
 
 BatchOptimizer::~BatchOptimizer()
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
index 8e423ed82..d094be08a 100644
--- a/fuse_optimizers/src/fixed_lag_smoother.cpp
+++ b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -90,9 +90,6 @@ FixedLagSmoother::FixedLagSmoother(
 {
   params_.loadFromROS(private_node_handle);
 
-  // Test for auto-start
-  autostart();
-
   // Start the optimization thread
   optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this);
 
@@ -117,6 +114,11 @@ FixedLagSmoother::FixedLagSmoother(
     ros::names::resolve(params_.start_service),
     &FixedLagSmoother::startServiceCallback,
     this);
+
+  if (!params_.disabled_at_startup)
+  {
+    start();
+  }
 }
 
 FixedLagSmoother::~FixedLagSmoother()
diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp
index 203ee1769..b13df5365 100644
--- a/fuse_optimizers/src/optimizer.cpp
+++ b/fuse_optimizers/src/optimizer.cpp
@@ -109,9 +109,6 @@ Optimizer::Optimizer(
   loadMotionModels();
   loadSensorModels();
   loadPublishers();
-
-  // Start all the plugins
-  startPlugins();
 }
 
 Optimizer::~Optimizer()