Modified the plugin start/stop order, moving the sensors last

locusrobotics · Aug 2, 2024 · d99d41f · d99d41f
1 parent bbac22e
commit d99d41f
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 10 deletions.
diff --git 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
@@ -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();
 }