Add reset message callback for resets called within other callbacks

Author: jakemclaughlin6

fuse_optimizers/include/fuse_optimizers/windowed_optimizer.h

@@ -40,6 +40,7 @@
 #include <fuse_optimizers/windowed_optimizer_params.h>
 #include <ros/ros.h>
 #include <std_srvs/Empty.h>
+#include <std_msgs/Empty.h>
 
 #include <atomic>
 #include <condition_variable>
@@ -184,6 +185,7 @@ class WindowedOptimizer : public Optimizer
   // Ordering ROS objects with callbacks last
   ros::Timer optimize_timer_;  //!< Trigger an optimization operation at a fixed frequency
   ros::ServiceServer reset_service_server_;  //!< Service that resets the optimizer to its initial state
+  ros::Subscriber reset_subscriber_; //!< Subscriber that resets the optimizer to its initial state
 
   /**
    * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called
@@ -281,6 +283,11 @@ class WindowedOptimizer : public Optimizer
    */
   void processQueue(fuse_core::Transaction& transaction);
 
+  /**
+   * @brief Message callback that resets the optimizer to its original state
+   */
+  void resetMessageCallback(const std_msgs::Empty::ConstPtr&);
+
   /**
    * @brief Service callback that resets the optimizer to its original state
    */

fuse_optimizers/src/windowed_optimizer.cpp

@@ -103,6 +103,12 @@ WindowedOptimizer::WindowedOptimizer(
     ros::names::resolve(params_->reset_service),
     &WindowedOptimizer::resetServiceCallback,
     this);
+
+  // Subscribe to a reset message that will reset the optimizer to the initial state
+  reset_subscriber_ = node_handle_.subscribe(
+    ros::names::resolve(params_->reset_service), 10,
+    &WindowedOptimizer::resetMessageCallback, 
+    this);
 }
 
 WindowedOptimizer::~WindowedOptimizer()
@@ -416,6 +422,40 @@ void WindowedOptimizer::processQueue(fuse_core::Transaction& transaction)
   }
 }
 
+void WindowedOptimizer::resetMessageCallback(const std_msgs::Empty::ConstPtr&){
+  // Tell all the plugins to stop
+  stopPlugins();
+  // Reset the optimizer state
+  {
+    std::lock_guard<std::mutex> lock(optimization_requested_mutex_);
+    optimization_request_ = false;
+  }
+  started_ = false;
+  ignited_ = false;
+  setStartTime(ros::Time(0, 0));
+  // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the
+  //         pending_transactions_mutex_ lock at the same time. We perform a parallel locking scheme here to
+  //         prevent the possibility of deadlocks.
+  {
+    std::lock_guard<std::mutex> lock(optimization_mutex_);
+    // Clear all pending transactions
+    {
+      std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
+      pending_transactions_.clear();
+    }
+    // Clear the graph and marginal tracking states
+    graph_->clear();
+    marginal_transaction_ = fuse_core::Transaction();
+  }
+  // Perform any required reset operations for derived classes
+  onReset();
+  // Tell all the plugins to start
+  startPlugins();
+  // Test for auto-start
+  autostart();
+  return;
+}
+
 bool WindowedOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
   // Tell all the plugins to stop