[RST-2128] fixed lag smoother reset

fuse_core/include/fuse_core/graph.h

@@ -93,6 +93,13 @@ class Graph
    */
   virtual ~Graph() = default;
 
+  /**
+   * @brief Clear all variables and constraints from the graph object.
+   *
+   * The object should be equivalent to a newly constructed object after clear() has been called.
+   */
+  virtual void clear() = 0;
+
   /**
    * @brief Return a deep copy of the graph object.
    *

fuse_graphs/include/fuse_graphs/hash_graph.h

@@ -96,6 +96,13 @@ class HashGraph : public fuse_core::Graph
    */
   HashGraph& operator=(const HashGraph& other);
 
+  /**
+   * @brief Clear all variables and constraints from the graph object.
+   *
+   * The object should be equivalent to a newly constructed object after clear() has been called.
+   */
+  void clear() override;
+
   /**
    * @brief Return a deep copy of the graph object.
    *

fuse_graphs/src/hash_graph.cpp

@@ -88,6 +88,14 @@ HashGraph& HashGraph::operator=(const HashGraph& other)
   return *this;
 }
 
+void HashGraph::clear()
+{
+  constraints_.clear();
+  constraints_by_variable_uuid_.clear();
+  variables_.clear();
+  variables_on_hold_.clear();
+}
+
 fuse_core::Graph::UniquePtr HashGraph::clone() const
 {
   return HashGraph::make_unique(*this);

fuse_optimizers/CMakeLists.txt

@@ -7,6 +7,7 @@ set(build_depends
   fuse_graphs
   pluginlib
   roscpp
+  std_srvs
 )
 
 find_package(catkin REQUIRED COMPONENTS

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h

@@ -40,6 +40,7 @@
 #include <fuse_optimizers/variable_stamp_index.h>
 #include <fuse_optimizers/vector_queue.h>
 #include <ros/ros.h>
+#include <std_srvs/Empty.h>
 
 #include <atomic>
 #include <condition_variable>
@@ -158,6 +159,7 @@ class FixedLagSmoother : public Optimizer
   ros::Duration lag_duration_;  //!< Parameter that controls the time window of the fixed lag smoother. Variables
                                 //!< older than the lag duration will be marginalized out
   ros::Time optimization_deadline_;  //!< The deadline for the optimization to complete. Triggers a warning if exceeded.
+  std::mutex optimization_mutex_;  //!< Mutex held while the graph is begin optimized
   ros::Duration optimization_period_;  //!< The expected time between optimization cycles
   std::atomic<bool> optimization_request_;  //!< Flag to trigger a new optimization
   std::condition_variable optimization_requested_;  //!< Condition variable used by the optimization thread to wait
@@ -177,6 +179,12 @@ class FixedLagSmoother : 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
+
+  /**
+   * @brief Automatically start the smoother if no ignition sensors are specified
+   */
+  void autostart();
 
   /**
    * @brief Perform any required preprocessing steps before \p computeVariablesToMarginalize() is called
@@ -240,6 +248,11 @@ class FixedLagSmoother : public Optimizer
    */
   void processQueue(fuse_core::Transaction& transaction);
 
+  /**
+   * @brief Service callback that resets the optimizer to its original state
+   */
+  bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
   /**
    * @brief Callback fired every time the SensorModel plugin creates a new transaction
    *

fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h

@@ -87,6 +87,15 @@ class VariableStampIndex
     return stamped_index_.size() + unstamped_index_.size();
   }
 
+  /**
+   * @brief Clear all tracked state
+   */
+  void clear()
+  {
+    stamped_index_.clear();
+    unstamped_index_.clear();
+  }
+
   /**
    * @brief Returns the most recent timestamp associated with any variable
    */

fuse_optimizers/include/fuse_optimizers/vector_queue.h

@@ -165,6 +165,14 @@ class VectorQueue
     container_.pop_back();
   }
 
+  /**
+   * @brief Remove all elements from the vector
+   */
+  void clear()
+  {
+    container_.clear();
+  }
+
 protected:
   using Container = std::vector<std::pair<key_type, value_type>>;
 

fuse_optimizers/package.xml

@@ -19,6 +19,7 @@
   <depend>fuse_graphs</depend>
   <depend>pluginlib</depend>
   <depend>roscpp</depend>
+  <depend>std_srvs</depend>
   <test_depend>roslint</test_depend>
   <test_depend>rostest</test_depend>
 

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -88,26 +88,19 @@ FixedLagSmoother::FixedLagSmoother(
   transaction_timeout_.fromSec(transaction_timeout);
 
   private_node_handle_.getParam("ignition_sensors", ignition_sensors_);
-  if (ignition_sensors_.empty())
-  {
-    // No ignition sensors were provided. Auto-start.
-    started_ = true;
-    start_time_ = ros::Time(0, 0);
-    ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately.");
-  }
-  else
+  // Sort the sensors for efficient lookups
+  std::sort(ignition_sensors_.begin(), ignition_sensors_.end());
+  // Warn about possible configuration errors
+  for (const auto& sensor_model_name : ignition_sensors_)
   {
-    for (const auto& sensor_model_name : ignition_sensors_)
+    if (sensor_models_.find(sensor_model_name) == sensor_models_.end())
     {
-      if (sensor_models_.find(sensor_model_name) == sensor_models_.end())
-      {
-        ROS_WARN_STREAM("Sensor '" << sensor_model_name << "' is configured as an ignition sensor, but no sensor "
-                        "model with that name currently exists. This is likely a configuration error.");
-      }
+      ROS_WARN_STREAM("Sensor '" << sensor_model_name << "' is configured as an ignition sensor, but no sensor "
+                      "model with that name currently exists. This is likely a configuration error.");
     }
-    // Sort the sensors for efficient lookups
-    std::sort(ignition_sensors_.begin(), ignition_sensors_.end());
   }
+  // Test for auto-start
+  autostart();
 
   // Start the optimization thread
   optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this);
@@ -117,6 +110,9 @@ FixedLagSmoother::FixedLagSmoother(
     optimization_period_,
     &FixedLagSmoother::optimizerTimerCallback,
     this);
+
+  // Advertise a service that resets the optimizer to its initial state
+  reset_service_server_ = private_node_handle_.advertiseService("reset", &FixedLagSmoother::resetServiceCallback, this);
 }
 
 FixedLagSmoother::~FixedLagSmoother()
@@ -131,6 +127,17 @@ FixedLagSmoother::~FixedLagSmoother()
   }
 }
 
+void FixedLagSmoother::autostart()
+{
+  if (ignition_sensors_.empty())
+  {
+    // No ignition sensors were provided. Auto-start.
+    started_ = true;
+    start_time_ = ros::Time(0, 0);
+    ROS_INFO_STREAM("No ignition sensors were specified. Optimization will begin immediately.");
+  }
+}
+
 void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction)
 {
   timestamp_tracking_.addNewTransaction(new_transaction);
@@ -179,27 +186,31 @@ void FixedLagSmoother::optimizationLoop()
     // Apply motion models
     auto new_transaction = fuse_core::Transaction::make_shared();
     processQueue(*new_transaction);
-    // Prepare for selecting the marginal variables
-    preprocessMarginalization(*new_transaction);
-    // Combine the new transactions with any marginal transaction from the end of the last cycle
-    new_transaction->merge(marginal_transaction);
-    // Update the graph
-    graph_->update(*new_transaction);
-    // Optimize the entire graph
-    graph_->optimize();
-    // Optimization is complete. Notify all the things about the graph changes.
-    notify(std::move(new_transaction), graph_->clone());
-    // Compute a transaction that marginalizes out those variables.
-    marginal_transaction = fuse_constraints::marginalizeVariables(computeVariablesToMarginalize(), *graph_);
-    // Perform any post-marginal cleanup
-    postprocessMarginalization(marginal_transaction);
-    // Note: The marginal transaction will not be applied until the next optimization iteration
-    // Log a warning if the optimization took too long
-    auto optimization_complete = ros::Time::now();
-    if (optimization_complete > optimization_deadline)
+    // Optimize
     {
-      ROS_WARN_STREAM("Optimization exceeded the configured duration by " <<
-                      (optimization_complete - optimization_deadline) << "s");
+      std::lock_guard<std::mutex> lock(optimization_mutex_);
+      // Prepare for selecting the marginal variables
+      preprocessMarginalization(*new_transaction);
+      // Combine the new transactions with any marginal transaction from the end of the last cycle
+      new_transaction->merge(marginal_transaction);
+      // Update the graph
+      graph_->update(*new_transaction);
+      // Optimize the entire graph
+      graph_->optimize();
+      // Optimization is complete. Notify all the things about the graph changes.
+      notify(std::move(new_transaction), graph_->clone());
+      // Compute a transaction that marginalizes out those variables.
+      marginal_transaction = fuse_constraints::marginalizeVariables(computeVariablesToMarginalize(), *graph_);
+      // Perform any post-marginal cleanup
+      postprocessMarginalization(marginal_transaction);
+      // Note: The marginal transaction will not be applied until the next optimization iteration
+      // Log a warning if the optimization took too long
+      auto optimization_complete = ros::Time::now();
+      if (optimization_complete > optimization_deadline)
+      {
+        ROS_WARN_STREAM("Optimization exceeded the configured duration by " <<
+                        (optimization_complete - optimization_deadline) << "s");
+      }
     }
     // Clear the request flag now that this optimization cycle is complete
     optimization_request_ = false;
@@ -268,6 +279,28 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction)
   }
 }
 
+bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  started_ = false;
+  start_time_ = ros::TIME_MAX;
+  optimization_request_ = false;
+  // Clear all pending transactions
+  {
+    std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
+    pending_transactions_.clear();
+  }
+  // Clear the graph and marginal tracking states
+  {
+    std::lock_guard<std::mutex> lock(optimization_mutex_);
+    graph_->clear();
+    timestamp_tracking_.clear();
+  }
+  // Test for auto-start
+  autostart();
+
+  return true;
+}
+
 void FixedLagSmoother::transactionCallback(
   const std::string& sensor_name,
   fuse_core::Transaction::SharedPtr transaction)