RST-10147 - optimizer logging changes (#372)

* Adding some logging to fuse * Adding same logs to batch optimizer
locusrobotics · Jul 18, 2024 · 0170b4a · 0170b4a
1 parent 32aae32
commit 0170b4a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp
@@ -278,12 +278,15 @@ bool BatchOptimizer::startServiceCallback(std_srvs::Empty::Request&, std_srvs::E
 
 void BatchOptimizer::start()
 {
+  ROS_INFO_STREAM("Starting optimizer.");
   // Tell all the plugins to start
   startPlugins();
+  ROS_INFO_STREAM("Starting optimizer complete.");
 }
 
 void BatchOptimizer::stop()
 {
+  ROS_INFO_STREAM("Stopping optimizer.");
   // Tell all the plugins to stop
   stopPlugins();
   // Reset the optimizer state
@@ -315,6 +318,7 @@ void BatchOptimizer::stop()
     std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
     pending_transactions_.clear();
   }
+  ROS_INFO_STREAM("Optimizer stopping complete.");
 }
 
 }  // namespace fuse_optimizers
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -456,14 +456,17 @@ bool FixedLagSmoother::startServiceCallback(std_srvs::Empty::Request&, std_srvs:
 
 void FixedLagSmoother::start()
 {
+  ROS_INFO_STREAM("Starting optimizer.");
   // Tell all the plugins to start
   startPlugins();
   // Test for auto-start
   autostart();
+  ROS_INFO_STREAM("Starting optimizer complete.");
 }
 
 void FixedLagSmoother::stop()
 {
+  ROS_INFO_STREAM("Stopping optimizer.");
   // Tell all the plugins to stop
   stopPlugins();
   // Reset the optimizer state
@@ -490,6 +493,7 @@ void FixedLagSmoother::stop()
     timestamp_tracking_.clear();
     lag_expiration_ = ros::Time(0, 0);
   }
+  ROS_INFO_STREAM("Optimizer stopping complete.");
 }
 
 void FixedLagSmoother::transactionCallback(