PR feedback

locusrobotics · Aug 7, 2024 · a18d68b · a18d68b
1 parent b9c3c77
commit a18d68b
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 9 deletions.
diff --git 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
@@ -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.");
 }