Port the "Add reset to batch optimizer" patch to ROS 2 Rolling

fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp

@@ -38,6 +38,7 @@
 #include <atomic>
 #include <condition_variable>
 #include <map>
+#include <memory>
 #include <mutex>
 #include <set>
 #include <string>
@@ -52,6 +53,9 @@
 #include <fuse_optimizers/optimizer.hpp>
 #include <fuse_graphs/hash_graph.hpp>
 
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/empty.hpp>
+
 
 namespace fuse_optimizers
 {
@@ -147,30 +151,49 @@ class BatchOptimizer : public Optimizer
  */
  using TransactionQueue = std::multimap<rclcpp::Time, TransactionQueueElement>;
 
- fuse_core::Transaction::SharedPtr combined_transaction_; //!< Transaction used aggregate
- //!< constraints and variables from
- //!< multiple sensors and motions models
- //!< before being applied to the graph.
- std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction
- //!< across different threads
+ // Read-only after construction
  ParameterType params_; //!< Configuration settings for this optimizer
+ std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process
+
+ // Only used inside a single thread
+ rclcpp::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
+
+ // Inherently thread-safe
  std::atomic<bool> optimization_request_; //!< Flag to trigger a new optimization
+
+ // Guarded by optimization_mutex_
+ std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized
+ // fuse_core::Graph* graph_ member from the base class
+
+ // Guarded by optimization_requested_mutex_
  std::condition_variable optimization_requested_; //!< Condition variable used by the optimization
  //!< thread to wait until a new optimization is
  //!< requested by the main thread
  std::mutex optimization_requested_mutex_; //!< Required condition variable mutex
- std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process
- rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed
- //!< frequency
+
+ // Guarded by combined_transaction_mutex_
+ fuse_core::Transaction::SharedPtr combined_transaction_; //!< Transaction used aggregate
+ //!< constraints and variables from
+ //!< multiple sensors and motions models
+ //!< before being applied to the graph.
+ std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction
+ //!< across different threads
+
+ // Guarded by pending_transactions_mutex_
  TransactionQueue pending_transactions_; //!< The set of received transactions that have not been
  //!< added to the optimizer yet. Transactions are added
  //!< by the main thread, and removed and processed by the
  //!< optimization thread.
  std::mutex pending_transactions_mutex_; //!< Synchronize modification of the
  //!< pending_transactions_ container
- rclcpp::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
+
+ // Ordering ROS objects with callbacks last
+ rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed
+ //!< frequency
+ //!< Service that resets the optimizer to its initial state
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_server_;
 
  /**
  * @brief Generate motion model constraints for pending transactions
@@ -200,6 +223,14 @@ class BatchOptimizer : public Optimizer
  */
  void optimizerTimerCallback();
 
+ /**
+ * @brief Service callback that resets the optimizer to its original state
+ */
+ bool resetServiceCallback(
+ const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::Empty::Response>
+ );
+
  /**
  * @brief Callback fired every time the SensorModel plugin creates a new transaction
  *

fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp

@@ -65,6 +65,11 @@ struct BatchOptimizerParams
  */
  rclcpp::Duration optimization_period {0, static_cast<uint32_t>(RCUTILS_S_TO_NS(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.
  *
@@ -109,6 +114,8 @@ struct BatchOptimizerParams
  rclcpp::Duration::from_seconds(1.0 / optimization_frequency);
  }
 
+ reset_service = fuse_core::getParam(interfaces, "reset_service", reset_service);
+
  fuse_core::getPositiveParam(interfaces, "transaction_timeout", transaction_timeout);
 
  fuse_core::loadSolverOptionsFromROS(interfaces, solver_options, "solver_options");

fuse_optimizers/src/batch_optimizer.cpp

@@ -39,6 +39,7 @@
 #include <utility>
 
 #include <fuse_core/transaction.hpp>
+#include <fuse_core/util.hpp>
 #include <fuse_optimizers/batch_optimizer.hpp>
 #include <fuse_optimizers/optimizer.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -52,10 +53,10 @@ BatchOptimizer::BatchOptimizer(
  fuse_core::Graph::UniquePtr graph
 )
 : fuse_optimizers::Optimizer(interfaces, std::move(graph)),
- combined_transaction_(fuse_core::Transaction::make_shared()),
- optimization_request_(false),
  start_time_(rclcpp::Time::max()),
- started_(false)
+ started_(false),
+ optimization_request_(false),
+ combined_transaction_(fuse_core::Transaction::make_shared())
 {
  params_.loadFromROS(interfaces_);
 
@@ -68,6 +69,23 @@ BatchOptimizer::BatchOptimizer(
  interfaces_.get_node_base_interface()->get_default_callback_group()
  );
 
+ // Advertise a service that resets the optimizer to its initial state
+ reset_service_server_ = rclcpp::create_service<std_srvs::srv::Empty>(
+ interfaces_.get_node_base_interface(),
+ interfaces_.get_node_services_interface(),
+ fuse_core::joinTopicName(
+ interfaces_.get_node_base_interface()->get_name(),
+ params_.reset_service),
+ std::bind(
+ &BatchOptimizer::resetServiceCallback,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2
+ ),
+ rclcpp::ServicesQoS(),
+ interfaces_.get_node_base_interface()->get_default_callback_group()
+ );
+
  // Start the optimization thread
  optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this);
 }
@@ -148,23 +166,27 @@ void BatchOptimizer::optimizationLoop()
  if (!interfaces_.get_node_base_interface()->get_context()->is_valid()) {
  break;
  }
- // Copy the combined transaction so it can be shared with all the plugins
- fuse_core::Transaction::ConstSharedPtr const_transaction;
+
  {
- std::lock_guard<std::mutex> lock(combined_transaction_mutex_);
- const_transaction = std::move(combined_transaction_);
- combined_transaction_ = fuse_core::Transaction::make_shared();
+ std::lock_guard<std::mutex> 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<std::mutex> 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;
  }
 }
 
@@ -189,6 +211,48 @@ void BatchOptimizer::optimizerTimerCallback()
  }
 }
 
+bool BatchOptimizer::resetServiceCallback(
+ const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::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;
+ // 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<std::mutex> lock(optimization_mutex_);
+ // Clear the combined transation
+ {
+ std::lock_guard<std::mutex> 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<std::mutex> lock(pending_transactions_mutex_);
+ pending_transactions_.clear();
+ }
+ // Tell all the plugins to start
+ startPlugins();
+
+ return true;
+}
+
 void BatchOptimizer::transactionCallback(
  const std::string & sensor_name,
  fuse_core::Transaction::SharedPtr transaction)

fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp

@@ -131,6 +131,11 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState)
  pose_msg1.pose.covariance[35] = 1.0;
  relative_pose_publisher->publish(pose_msg1);
 
+ /// @todo(swilliams) There is a timing or message queuing issue. This sleep "solves"
+ /// the problem on my local machine, but this isn't a very robust
+ /// solution.
+ rclcpp::sleep_for(std::chrono::milliseconds(10));
+
  auto pose_msg2 = geometry_msgs::msg::PoseWithCovarianceStamped();
  pose_msg2.header.stamp = rclcpp::Time(3, 0, RCL_ROS_TIME);
  pose_msg2.header.frame_id = "base_link";
@@ -146,6 +151,8 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState)
  pose_msg2.pose.covariance[35] = 1.0;
  relative_pose_publisher->publish(pose_msg2);
 
+ rclcpp::sleep_for(std::chrono::milliseconds(10));
+
  // Wait for the optimizer to process all queued transactions
  rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(1.0);
  auto odom_msg = nav_msgs::msg::Odometry();