Add ability to leave the fixed-lag smoother stopped on construction

locusrobotics · Jul 30, 2024 · 2c709db · 2c709db
1 parent 89be2bc
commit 2c709db
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 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
@@ -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
@@ -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
@@ -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
@@ -109,9 +109,6 @@ Optimizer::Optimizer(
   loadMotionModels();
   loadSensorModels();
   loadPublishers();
-
-  // Start all the plugins
-  startPlugins();
 }
 
 Optimizer::~Optimizer()