From 734ad64589b0d949cb3dbfe3aa0510035865afcc Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 7 Aug 2024 14:19:42 -0700 Subject: [PATCH] PR feedback --- .../fuse_optimizers/fixed_lag_smoother.h | 7 +++++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 19 ++++++++++--------- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 4e6123626..83b8f16c7 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h @@ -199,6 +199,13 @@ class FixedLagSmoother : public Optimizer */ void autostart(); + /** + * @brief Publish the optimizer status message + * + * @param[in] running Flag indicating if the optimizer is running + */ + void publishStatus(const bool running); + /** * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called * diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a24e5e503..f8c2dbf0f 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -120,9 +120,7 @@ FixedLagSmoother::FixedLagSmoother( ros::names::resolve(params_.status_topic), 1, true); - auto status = std_msgs::Bool(); - status.data = false; - status_publisher_.publish(status); + publishStatus(false); if (!params_.disabled_at_startup) { @@ -154,6 +152,13 @@ void FixedLagSmoother::autostart() } } + void FixedLagSmoother::publishStatus(const bool running) + { + auto status = std_msgs::Bool(); + status.data = running; + status_publisher_.publish(status); + } + void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) { timestamp_tracking_.addNewTransaction(new_transaction); @@ -479,9 +484,7 @@ void FixedLagSmoother::start() // Test for auto-start autostart(); // Update status topic - auto status = std_msgs::Bool(); - status.data = true; - status_publisher_.publish(status); + publishStatus(true); ROS_INFO_STREAM("Started optimizer."); } @@ -516,9 +519,7 @@ void FixedLagSmoother::stop() lag_expiration_ = ros::Time(0, 0); } // Update status topic - auto status = std_msgs::Bool(); - status.data = false; - status_publisher_.publish(status); + publishStatus(false); ROS_INFO_STREAM("Stopped Optimizer."); }