From 415971ac515c1d7a39cc40c177eb4ea4844521b6 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Wed, 20 Mar 2024 22:25:18 -0400 Subject: [PATCH] Add a reset service to the batch optimizer, similar to the fixed-lag smoother (#360) --- .../include/fuse_optimizers/batch_optimizer.h | 8 ++ .../fuse_optimizers/batch_optimizer_params.h | 9 ++- fuse_optimizers/src/batch_optimizer.cpp | 78 +++++++++++++++---- 3 files changed, 78 insertions(+), 17 deletions(-) diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h index 39452b2f2..d1d0668ab 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -142,6 +143,7 @@ class BatchOptimizer : public Optimizer */ using TransactionQueue = std::multimap; + std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized fuse_core::Transaction::SharedPtr combined_transaction_; //!< Transaction used aggregate constraints and variables //!< from multiple sensors and motions models before being //!< applied to the graph. @@ -159,6 +161,7 @@ class BatchOptimizer : public Optimizer std::mutex pending_transactions_mutex_; //!< Synchronize modification of the pending_transactions_ container 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 /** * @brief Generate motion model constraints for pending transactions @@ -209,6 +212,11 @@ class BatchOptimizer : public Optimizer * @param[in] status The diagnostic status */ void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; + + /** + * @brief Service callback that resets the optimizer to its original state + */ + bool resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); }; } // 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 cc92ee4c8..f61fc68e5 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h @@ -45,10 +45,8 @@ #include #include - namespace fuse_optimizers { - /** * @brief Defines the set of parameters required by the fuse_optimizers::FixedLagSmoother class */ @@ -64,6 +62,11 @@ struct BatchOptimizerParams */ ros::Duration optimization_period { 0.1 }; + /** + * @brief The topic name of the advertised reset service + */ + std::string reset_service { "~reset" }; + /** * @brief The maximum time to wait for motion models to be generated for a received transaction. * @@ -96,6 +99,8 @@ struct BatchOptimizerParams fuse_core::getPositiveParam(nh, "optimization_period", optimization_period); } + nh.getParam("reset_service", reset_service); + fuse_core::getPositiveParam(nh, "transaction_timeout", transaction_timeout); fuse_core::loadSolverOptionsFromROS(ros::NodeHandle(nh, "solver_options"), solver_options); diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index fb39aea39..5b05415c9 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -66,6 +66,12 @@ BatchOptimizer::BatchOptimizer( // Start the optimization thread optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); + + // Advertise a service that resets the optimizer to its initial state + reset_service_server_ = node_handle_.advertiseService( + ros::names::resolve(params_.reset_service), + &BatchOptimizer::resetServiceCallback, + this); } BatchOptimizer::~BatchOptimizer() @@ -137,23 +143,26 @@ void BatchOptimizer::optimizationLoop() { break; } - // Copy the combined transaction so it can be shared with all the plugins - fuse_core::Transaction::ConstSharedPtr const_transaction; { - std::lock_guard lock(combined_transaction_mutex_); - const_transaction = std::move(combined_transaction_); - combined_transaction_ = fuse_core::Transaction::make_shared(); + std::lock_guard lock(optimization_mutex_); + // Copy the combined transaction so it can be shared with all the plugins + fuse_core::Transaction::ConstSharedPtr const_transaction; + { + std::lock_guard lock(combined_transaction_mutex_); + const_transaction = std::move(combined_transaction_); + combined_transaction_ = fuse_core::Transaction::make_shared(); + } + // Update the graph + graph_->update(*const_transaction); + // Optimize the entire graph + graph_->optimize(params_.solver_options); + // Make a copy of the graph to share + fuse_core::Graph::ConstSharedPtr const_graph = graph_->clone(); + // Optimization is complete. Notify all the things about the graph changes. + notify(const_transaction, const_graph); + // Clear the request flag now that this optimization cycle is complete + optimization_request_ = false; } - // Update the graph - graph_->update(*const_transaction); - // Optimize the entire graph - graph_->optimize(params_.solver_options); - // Make a copy of the graph to share - fuse_core::Graph::ConstSharedPtr const_graph = graph_->clone(); - // Optimization is complete. Notify all the things about the graph changes. - notify(const_transaction, const_graph); - // Clear the request flag now that this optimization cycle is complete - optimization_request_ = false; } } @@ -238,4 +247,43 @@ void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& } } +bool BatchOptimizer::resetServiceCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +{ + // Tell all the plugins to stop + stopPlugins(); + // Reset the optimizer state + { + std::lock_guard lock(optimization_requested_mutex_); + optimization_request_ = false; + } + started_ = false; + // DANGER: The optimizationLoop() function obtains the lock optimization_mutex_ lock and the + // combined_transaction_mutex_ lock at the same time. We perform a parallel locking scheme here to + // prevent the possibility of deadlocks. + { + std::lock_guard lock(optimization_mutex_); + // Clear the combined transation + { + std::lock_guard lock(combined_transaction_mutex_); + combined_transaction_ = fuse_core::Transaction::make_shared(); + } + // Clear the graph and marginal tracking states + graph_->clear(); + } + // Clear all pending transactions + // The transaction callback and the optimization timer callback are the only other locations where + // the pending_transactions_ variable is modified. As long as the BatchOptimizer node handle is + // single-threaded, then pending_transactions_ variable cannot be modified while the reset callback + // is running. Therefore, there are no timing or sequence issues with exactly where inside the reset + // service callback the pending_transactions_ are cleared. + { + std::lock_guard lock(pending_transactions_mutex_); + pending_transactions_.clear(); + } + // Tell all the plugins to start + startPlugins(); + + return true; +} + } // namespace fuse_optimizers