[RST-10642] Allow the optimizer to remain disabled at startup

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"))

fuse_optimizers/src/batch_optimizer.cpp

@@ -82,6 +82,8 @@ BatchOptimizer::BatchOptimizer(
     ros::names::resolve(params_.start_service),
     &BatchOptimizer::startServiceCallback,
     this);
+
+  startPlugins();
 }
 
 BatchOptimizer::~BatchOptimizer()

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()
@@ -456,6 +458,12 @@ bool FixedLagSmoother::startServiceCallback(std_srvs::Empty::Request&, std_srvs:
 
 void FixedLagSmoother::start()
 {
+  if (started_)
+  {
+    ROS_WARN_STREAM("Requested to start the optimizer while it is already running. Ignoring request.");
+    return;
+  }
+
   ROS_INFO_STREAM("Starting optimizer.");
   // Tell all the plugins to start
   startPlugins();

fuse_optimizers/src/optimizer.cpp

@@ -109,9 +109,6 @@ Optimizer::Optimizer(
   loadMotionModels();
   loadSensorModels();
   loadPublishers();
-
-  // Start all the plugins
-  startPlugins();
 }
 
 Optimizer::~Optimizer()