diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index e86af19e3..6784a5a7f 100644 --- 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 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 index 0fd47ab13..8e423ed82 100644 --- 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(