diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt
index 3bb31f6f..61039cd2 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 6a2d0e6b..83b8f16c 100644
--- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h
+++ b/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
*
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 3e045b6d..025b6741 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 df240ffa..a2698f5f 100644
--- a/fuse_optimizers/package.xml
+++ b/fuse_optimizers/package.xml
@@ -21,6 +21,7 @@
fuse_variables
pluginlib
roscpp
+ std_msgs
std_srvs
fuse_models
geometry_msgs
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
index 6d024897..cd07778f 100644
--- a/fuse_optimizers/src/fixed_lag_smoother.cpp
+++ b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -39,6 +39,7 @@
#include
#include
#include
+#include
#include
#include
@@ -115,6 +116,12 @@ FixedLagSmoother::FixedLagSmoother(
&FixedLagSmoother::startServiceCallback,
this);
+ status_publisher_ = node_handle_.advertise(
+ 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(