From b9c3c77dc157e1db7ee3daf53171da57d8c433ce Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 4 Aug 2024 16:38:29 -0700 Subject: [PATCH 1/2] 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 | 23 +++++++++++++++++-- 5 files changed, 30 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 @@ 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 6d0248978..a24e5e503 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,14 @@ FixedLagSmoother::FixedLagSmoother( &FixedLagSmoother::startServiceCallback, this); + status_publisher_ = node_handle_.advertise( + 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 +478,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 +515,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( From a18d68bff0523e50dd8ef429b1f2f1b1700fe3e3 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 7 Aug 2024 14:19:42 -0700 Subject: [PATCH 2/2] PR feedback --- .../fuse_optimizers/fixed_lag_smoother.h | 7 +++++++ fuse_optimizers/src/fixed_lag_smoother.cpp | 19 ++++++++++--------- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index 4e6123626..83b8f16c7 100644 --- 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 index a24e5e503..cd07778fb 100644 --- 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."); }