From 517f17d44110b4daaefae53324c6ebeb5165f577 Mon Sep 17 00:00:00 2001
From: Stephen Williams <swilliams@locusrobotics.com>
Date: Sun, 4 Aug 2024 16:38:29 -0700
Subject: [PATCH] Add a topic that reports when the optimizer is started or
 stopped

---
 fuse_optimizers/CMakeLists.txt                |  1 +
 .../fuse_optimizers/fixed_lag_smoother.h      |  1 +
 .../fixed_lag_smoother_params.h               |  6 +++++
 fuse_optimizers/package.xml                   |  1 +
 fuse_optimizers/src/fixed_lag_smoother.cpp    | 24 +++++++++++++++++--
 5 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt
index 3bb31f6f7..61039cd29 100644
--- a/fuse_optimizers/CMakeLists.txt
+++ b/fuse_optimizers/CMakeLists.txt
@@ -9,6 +9,7 @@ set(build_depends
   fuse_variables
   pluginlib
   roscpp
+  std_msgs
   std_srvs
 )
 
diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
index 6a2d0e6b0..4e6123626 100644
--- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
+++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
@@ -192,6 +192,7 @@ 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
diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
index 3e045b6dc..025b67410 100644
--- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h
+++ b/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);
 
diff --git a/fuse_optimizers/package.xml b/fuse_optimizers/package.xml
index df240ffa5..a2698f5f6 100644
--- a/fuse_optimizers/package.xml
+++ b/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>
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
index 6d0248978..8cd46b164 100644
--- a/fuse_optimizers/src/fixed_lag_smoother.cpp
+++ b/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,15 @@ FixedLagSmoother::FixedLagSmoother(
     &FixedLagSmoother::startServiceCallback,
     this);
 
+  status_publisher_ = node_handle_.advertise<std_msgs::Bool>(
+    ros::names::resolve(params_.status_topic),
+    1,
+    true
+  );
+  auto status = std_msgs::Bool();
+  status.data = false;
+  status_publisher_.publish(status);
+
   if (!params_.disabled_at_startup)
   {
     start();
@@ -469,7 +479,12 @@ void FixedLagSmoother::start()
   startPlugins();
   // Test for auto-start
   autostart();
-  ROS_INFO_STREAM("Starting optimizer complete.");
+  // Update status topic
+  auto status = std_msgs::Bool();
+  status.data = true;
+  status_publisher_.publish(status);
+
+  ROS_INFO_STREAM("Started optimizer.");
 }
 
 void FixedLagSmoother::stop()
@@ -501,7 +516,12 @@ void FixedLagSmoother::stop()
     timestamp_tracking_.clear();
     lag_expiration_ = ros::Time(0, 0);
   }
-  ROS_INFO_STREAM("Optimizer stopping complete.");
+  // Update status topic
+  auto status = std_msgs::Bool();
+  status.data = false;
+  status_publisher_.publish(status);
+
+  ROS_INFO_STREAM("Stopped Optimizer.");
 }
 
 void FixedLagSmoother::transactionCallback(