Adding a pause

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h

@@ -190,6 +190,8 @@ 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
+  ros::ServiceServer stop_service_server_;  //!< Service that stops and clears the optimizer
+  ros::ServiceServer start_service_server_;  //!< Service that restarts the optimizer
 
   /**
    * @brief Automatically start the smoother if no ignition sensors are specified
@@ -270,6 +272,17 @@ class FixedLagSmoother : public Optimizer
    */
   bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 
+  /**
+   * @brief Service callback that stops the optimizer plugins and clears the existing graph. Essentially performs a reset,
+   * but doesn't immediately restart optimization.
+   */
+  bool stopServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+  /**
+   * @brief Service callback that starts the optimizer plugins after they have been stopped.
+   */
+  bool startServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
   /**
    * @brief Thread-safe read-only access to the optimizer start time
    */

fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h

@@ -74,6 +74,16 @@ struct FixedLagSmootherParams
    */
   std::string reset_service { "~reset" };
 
+  /**
+   * @brief The topic name of the advertised stop service
+   */
+  std::string stop_service { "~stop" };
+
+  /**
+   * @brief The topic name of the advertised restart service
+   */
+  std::string start_service { "~start" };
+
   /**
    * @brief The maximum time to wait for motion models to be generated for a received transaction.
    *
@@ -109,6 +119,8 @@ struct FixedLagSmootherParams
     }
 
     nh.getParam("reset_service", reset_service);
+    nh.getParam("stop_service", stop_service);
+    nh.getParam("start_service", start_service);
 
     fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout);
 

fuse_optimizers/src/fixed_lag_smoother.cpp

@@ -107,6 +107,16 @@ FixedLagSmoother::FixedLagSmoother(
     ros::names::resolve(params_.reset_service),
     &FixedLagSmoother::resetServiceCallback,
     this);
+
+  stop_service_server_ = node_handle_.advertiseService(
+    ros::names::resolve(params_.stop_service),
+    &FixedLagSmoother::stopServiceCallback,
+    this);
+
+  start_service_server_ = node_handle_.advertiseService(
+    ros::names::resolve(params_.start_service),
+    &FixedLagSmoother::startServiceCallback,
+    this);
 }
 
 FixedLagSmoother::~FixedLagSmoother()
@@ -460,6 +470,48 @@ bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs:
   return true;
 }
 
+bool FixedLagSmoother::stopServiceCallback(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;
+  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();
+    timestamp_tracking_.clear();
+    lag_expiration_ = ros::Time(0, 0);
+  }
+
+  return true;
+}
+
+bool FixedLagSmoother::startServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  // Tell all the plugins to start
+  startPlugins();
+  // Test for auto-start
+  autostart();
+
+  return true;
+}
+
 void FixedLagSmoother::transactionCallback(
   const std::string& sensor_name,
   fuse_core::Transaction::SharedPtr transaction)
@@ -497,6 +549,7 @@ void FixedLagSmoother::transactionCallback(
       // ...check if we should
       if (sensor_models_.at(sensor_name).ignition)
       {
+        ROS_INFO_STREAM("Ignition occured");
         started_ = true;
         ignited_ = true;
         start_time = position->minStamp();