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(