From d99d41fb592ccdc9694ef463d1233107f426c839 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Fri, 2 Aug 2024 13:57:55 -0700 Subject: [PATCH] Modified the plugin start/stop order, moving the sensors last --- fuse_optimizers/src/fixed_lag_smoother.cpp | 5 +++-- fuse_optimizers/src/optimizer.cpp | 16 ++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 6d0248978..5ea4c93a0 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 b13df5365..ac544b3c5 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(); }