From d99d41fb592ccdc9694ef463d1233107f426c839 Mon Sep 17 00:00:00 2001
From: Stephen Williams <swilliams@locusrobotics.com>
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();
}