Pr Feedback:Adding start stop to batch optimizer

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/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