diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 6d024897..5ea4c93a 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -438,6 +438,7 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { + ROS_INFO_STREAM("Resetting optimizer."); stop(); start(); @@ -469,7 +470,7 @@ void FixedLagSmoother::start() startPlugins(); // Test for auto-start autostart(); - ROS_INFO_STREAM("Starting optimizer complete."); + ROS_INFO_STREAM("Started optimizer."); } void FixedLagSmoother::stop() @@ -501,7 +502,7 @@ void FixedLagSmoother::stop() timestamp_tracking_.clear(); lag_expiration_ = ros::Time(0, 0); } - ROS_INFO_STREAM("Optimizer stopping complete."); + ROS_INFO_STREAM("Stopped Optimizer."); } void FixedLagSmoother::transactionCallback( diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index b13df536..ac544b3c 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -437,6 +437,10 @@ void Optimizer::clearCallbacks() void Optimizer::startPlugins() { + for (const auto& name_plugin : publishers_) + { + name_plugin.second->start(); + } for (const auto& name_plugin : motion_models_) { name_plugin.second->start(); @@ -445,20 +449,12 @@ void Optimizer::startPlugins() { name_plugin.second.model->start(); } - for (const auto& name_plugin : publishers_) - { - name_plugin.second->start(); - } diagnostic_updater_.force_update(); } void Optimizer::stopPlugins() { - for (const auto& name_plugin : publishers_) - { - name_plugin.second->stop(); - } for (const auto& name_plugin : sensor_models_) { name_plugin.second.model->stop(); @@ -467,6 +463,10 @@ void Optimizer::stopPlugins() { name_plugin.second->stop(); } + for (const auto& name_plugin : publishers_) + { + name_plugin.second->stop(); + } diagnostic_updater_.force_update(); }