diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h index d1d0668ab..b934f390c 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ b/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 diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h index f61fc68e5..955393b1d 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/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); diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h index eefacac2a..6a2d0e6b0 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ b/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 */ diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index a9c0972c1..2b07db863 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/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); diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index 5b05415c9..e86af19e3 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/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 lock(pending_transactions_mutex_); pending_transactions_.clear(); } - // Tell all the plugins to start - startPlugins(); - - return true; } } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index dad9058c6..0fd47ab13 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/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(