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..6d0248978 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() @@ -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(); 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()