[RST-10727] Publish the start/stop status of the fixed-lag smoother

fuse_optimizers/CMakeLists.txt

@@ -9,6 +9,7 @@ set(build_depends
   fuse_variables
   pluginlib
   roscpp
+  std_msgs
   std_srvs
 )
 

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h

@@ -192,12 +192,20 @@ class FixedLagSmoother : public Optimizer
   ros::ServiceServer reset_service_server_;  //!< Service that resets the optimizer to its initial state
   ros::ServiceServer stop_service_server_;  //!< Service that stops and clears the optimizer
   ros::ServiceServer start_service_server_;  //!< Service that restarts the optimizer
+  ros::Publisher status_publisher_;  //!< Publishing the started/stopped status of the optimizer
 
   /**
    * @brief Automatically start the smoother if no ignition sensors are specified
    */
   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
    *

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h

@@ -89,6 +89,11 @@ struct FixedLagSmootherParams
    */
   std::string start_service { "~start" };
 
+  /**
+   * @brief The topic name of the started/stopped status topic
+   */
+  std::string status_topic { "~running" };
+
   /**
    * @brief The maximum time to wait for motion models to be generated for a received transaction.
    *
@@ -128,6 +133,7 @@ struct FixedLagSmootherParams
     nh.getParam("reset_service", reset_service);
     nh.getParam("stop_service", stop_service);
     nh.getParam("start_service", start_service);
+    nh.getParam("status_topic", status_topic);
 
     fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout);
 

fuse_optimizers/package.xml

@@ -21,6 +21,7 @@
   <depend>fuse_variables</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
+  <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <test_depend>fuse_models</test_depend>
   <test_depend>geometry_msgs</test_depend>

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -39,6 +39,7 @@
 #include <fuse_core/uuid.h>
 #include <fuse_optimizers/optimizer.h>
 #include <ros/ros.h>
+#include <std_msgs/Bool.h>
 
 #include <algorithm>
 #include <iterator>
@@ -115,6 +116,12 @@ FixedLagSmoother::FixedLagSmoother(
     &FixedLagSmoother::startServiceCallback,
     this);
 
+  status_publisher_ = node_handle_.advertise<std_msgs::Bool>(
+    ros::names::resolve(params_.status_topic),
+    1,
+    true);
+  publishStatus(false);
+
   if (!params_.disabled_at_startup)
   {
     start();
@@ -145,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);
@@ -469,7 +483,10 @@ void FixedLagSmoother::start()
   startPlugins();
   // Test for auto-start
   autostart();
-  ROS_INFO_STREAM("Starting optimizer complete.");
+  // Update status topic
+  publishStatus(true);
+
+  ROS_INFO_STREAM("Started optimizer.");
 }
 
 void FixedLagSmoother::stop()
@@ -501,7 +518,10 @@ void FixedLagSmoother::stop()
     timestamp_tracking_.clear();
     lag_expiration_ = ros::Time(0, 0);
   }
-  ROS_INFO_STREAM("Optimizer stopping complete.");
+  // Update status topic
+  publishStatus(false);
+
+  ROS_INFO_STREAM("Stopped Optimizer.");
 }
 
 void FixedLagSmoother::transactionCallback(