Add a reset service to the batch optimizer, similar to the fixed-lag …

…smoother (#360)

fuse_optimizers/include/fuse_optimizers/batch_optimizer.h

@@ -40,6 +40,7 @@
 #include <fuse_optimizers/batch_optimizer_params.h>
 #include <fuse_optimizers/optimizer.h>
 #include <ros/ros.h>
+#include <std_srvs/Empty.h>
 
 #include <atomic>
 #include <condition_variable>
@@ -142,6 +143,7 @@ class BatchOptimizer : public Optimizer
    */
   using TransactionQueue = std::multimap<ros::Time, TransactionQueueElement>;
 
+  std::mutex optimization_mutex_;  //!< Mutex held while the graph is begin optimized
   fuse_core::Transaction::SharedPtr combined_transaction_;  //!< Transaction used aggregate constraints and variables
                                                             //!< from multiple sensors and motions models before being
                                                             //!< applied to the graph.
@@ -159,6 +161,7 @@ class BatchOptimizer : public Optimizer
   std::mutex pending_transactions_mutex_;  //!< Synchronize modification of the pending_transactions_ container
   ros::Time start_time_;  //!< The timestamp of the first ignition sensor transaction
   bool started_;  //!< Flag indicating the optimizer is ready/has received a transaction from an ignition sensor
+  ros::ServiceServer reset_service_server_;  //!< Service that resets the optimizer to its initial state
 
   /**
    * @brief Generate motion model constraints for pending transactions
@@ -209,6 +212,11 @@ class BatchOptimizer : public Optimizer
    * @param[in] status The diagnostic status
    */
   void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override;
+
+  /**
+   * @brief Service callback that resets the optimizer to its original state
+   */
+  bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 };
 
 }  // namespace fuse_optimizers

fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h

@@ -45,10 +45,8 @@
 #include <string>
 #include <vector>
 
-
 namespace fuse_optimizers
 {
-
 /**
  * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class
  */
@@ -64,6 +62,11 @@ struct BatchOptimizerParams
    */
   ros::Duration optimization_period { 0.1 };
 
+  /**
+   * @brief The topic name of the advertised reset service
+   */
+  std::string reset_service { "~reset" };
+
   /**
    * @brief The maximum time to wait for motion models to be generated for a received transaction.
    *
@@ -96,6 +99,8 @@ struct BatchOptimizerParams
       fuse_core::getPositiveParam(nh, "optimization_period", optimization_period);
     }
 
+    nh.getParam("reset_service", reset_service);
+
     fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout);
 
     fuse_core::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options);

fuse_optimizers/src/batch_optimizer.cpp

@@ -66,6 +66,12 @@ BatchOptimizer::BatchOptimizer(
 
   // Start the optimization thread
   optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this);
+
+  // Advertise a service that resets the optimizer to its initial state
+  reset_service_server_ = node_handle_.advertiseService(
+    ros::names::resolve(params_.reset_service),
+    &BatchOptimizer::resetServiceCallback,
+    this);
 }
 
 BatchOptimizer::~BatchOptimizer()
@@ -137,23 +143,26 @@ void BatchOptimizer::optimizationLoop()
     {
       break;
     }
-    // Copy the combined transaction so it can be shared with all the plugins
-    fuse_core::Transaction::ConstSharedPtr const_transaction;
     {
-      std::lock_guard<std::mutex> lock(combined_transaction_mutex_);
-      const_transaction = std::move(combined_transaction_);
-      combined_transaction_ = fuse_core::Transaction::make_shared();
+      std::lock_guard<std::mutex> lock(optimization_mutex_);
+      // Copy the combined transaction so it can be shared with all the plugins
+      fuse_core::Transaction::ConstSharedPtr const_transaction;
+      {
+        std::lock_guard<std::mutex> lock(combined_transaction_mutex_);
+        const_transaction = std::move(combined_transaction_);
+        combined_transaction_ = fuse_core::Transaction::make_shared();
+      }
+      // Update the graph
+      graph_->update(*const_transaction);
+      // Optimize the entire graph
+      graph_->optimize(params_.solver_options);
+      // Make a copy of the graph to share
+      fuse_core::Graph::ConstSharedPtr const_graph = graph_->clone();
+      // Optimization is complete. Notify all the things about the graph changes.
+      notify(const_transaction, const_graph);
+      // Clear the request flag now that this optimization cycle is complete
+      optimization_request_ = false;
     }
-    // Update the graph
-    graph_->update(*const_transaction);
-    // Optimize the entire graph
-    graph_->optimize(params_.solver_options);
-    // Make a copy of the graph to share
-    fuse_core::Graph::ConstSharedPtr const_graph = graph_->clone();
-    // Optimization is complete. Notify all the things about the graph changes.
-    notify(const_transaction, const_graph);
-    // Clear the request flag now that this optimization cycle is complete
-    optimization_request_ = false;
   }
 }
 
@@ -238,4 +247,43 @@ void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper&
   }
 }
 
+bool BatchOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  // 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;
+  // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the
+  //         combined_transaction_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 the combined transation
+    {
+      std::lock_guard<std::mutex> lock(combined_transaction_mutex_);
+      combined_transaction_ = fuse_core::Transaction::make_shared();
+    }
+    // Clear the graph and marginal tracking states
+    graph_->clear();
+  }
+  // Clear all pending transactions
+  // The transaction callback and the optimization timer callback are the only other locations where
+  // the pending_transactions_ variable is modified. As long as the BatchOptimizer node handle is
+  // single-threaded, then pending_transactions_ variable cannot be modified while the reset callback
+  // is running. Therefore, there are no timing or sequence issues with exactly where inside the reset
+  // service callback the pending_transactions_ are cleared.
+  {
+    std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
+    pending_transactions_.clear();
+  }
+  // Tell all the plugins to start
+  startPlugins();
+
+  return true;
+}
+
 }  // namespace fuse_optimizers