Adding stop and start functionality to the fixed lag smoother (#362)

* Adding a pause

* Pr feedback

* Pr Feedback:Adding start stop to batch optimizer

* Removing a debug log I forgot about

* Fixing lint errors

fuse_optimizers/include/fuse_optimizers/batch_optimizer.h

@@ -162,6 +162,8 @@ class BatchOptimizer : public Optimizer
   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
+  ros::ServiceServer stop_service_server_;  //!< Service that stops and clears the optimizer
+  ros::ServiceServer start_service_server_;  //!< Service that restarts the optimizer
 
   /**
    * @brief Generate motion model constraints for pending transactions
@@ -217,6 +219,26 @@ class BatchOptimizer : public Optimizer
    * @brief Service callback that resets the optimizer to its original state
    */
   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 Start the optimizer
+   */
+  void start();
+
+  /**
+   * @brief Stop the optimizer
+   */
+  void stop();
 };
 
 }  // namespace fuse_optimizers

fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h

@@ -67,6 +67,16 @@ struct BatchOptimizerParams
    */
   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.
    *
@@ -100,6 +110,8 @@ struct BatchOptimizerParams
     }
 
     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/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,27 @@ 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 Start the optimizer
+   */
+  void start();
+
+  /**
+   * @brief Stop the optimizer
+   */
+  void stop();
+
   /**
    * @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/batch_optimizer.cpp

@@ -72,6 +72,16 @@ BatchOptimizer::BatchOptimizer(
     ros::names::resolve(params_.reset_service),
     &BatchOptimizer::resetServiceCallback,
     this);
+
+  stop_service_server_ = node_handle_.advertiseService(
+    ros::names::resolve(params_.stop_service),
+    &BatchOptimizer::stopServiceCallback,
+    this);
+
+  start_service_server_ = node_handle_.advertiseService(
+    ros::names::resolve(params_.start_service),
+    &BatchOptimizer::startServiceCallback,
+    this);
 }
 
 BatchOptimizer::~BatchOptimizer()
@@ -248,6 +258,31 @@ void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper&
 }
 
 bool BatchOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  stop();
+  start();
+  return true;
+}
+
+bool BatchOptimizer::stopServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  stop();
+  return true;
+}
+
+bool BatchOptimizer::startServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  start();
+  return true;
+}
+
+void BatchOptimizer::start()
+{
+  // Tell all the plugins to start
+  startPlugins();
+}
+
+void BatchOptimizer::stop()
 {
   // Tell all the plugins to stop
   stopPlugins();
@@ -280,10 +315,6 @@ bool BatchOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::E
     std::lock_guard<std::mutex> lock(pending_transactions_mutex_);
     pending_transactions_.clear();
   }
-  // Tell all the plugins to start
-  startPlugins();
-
-  return true;
 }
 
 }  // namespace fuse_optimizers

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()
@@ -425,6 +435,34 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r
 }
 
 bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  stop();
+  start();
+
+  return true;
+}
+
+bool FixedLagSmoother::stopServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  stop();
+  return true;
+}
+
+bool FixedLagSmoother::startServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  start();
+  return true;
+}
+
+void FixedLagSmoother::start()
+{
+  // Tell all the plugins to start
+  startPlugins();
+  // Test for auto-start
+  autostart();
+}
+
+void FixedLagSmoother::stop()
 {
   // Tell all the plugins to stop
   stopPlugins();
@@ -452,12 +490,6 @@ bool FixedLagSmoother::resetServiceCallback(std_srvs::Empty::Request&, std_srvs:
     timestamp_tracking_.clear();
     lag_expiration_ = ros::Time(0, 0);
   }
-  // Tell all the plugins to start
-  startPlugins();
-  // Test for auto-start
-  autostart();
-
-  return true;
 }
 
 void FixedLagSmoother::transactionCallback(