Add mutex to reloadDbCb to fail if called during docking

opennav_docking/include/opennav_docking/dock_database.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <mutex>
 
 #include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -42,7 +43,7 @@ class DockDatabase
   /**
    * @brief A constructor for opennav_docking::DockDatabase
    */
-  DockDatabase();
+  explicit DockDatabase(std::shared_ptr<std::mutex> mutex = std::make_shared<std::mutex>());
 
   /**
    * @brief A setup function to populate database
@@ -127,6 +128,7 @@ class DockDatabase
     std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Response> response);
 
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  std::shared_ptr<std::mutex> mutex_;  // Don't reload database while actively docking
   DockPluginMap dock_plugins_;
   DockMap dock_instances_;
   pluginlib::ClassLoader<opennav_docking_core::ChargingDock> dock_loader_;

opennav_docking/include/opennav_docking/docking_server.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 #include <memory>
 #include <string>
+#include <mutex>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
@@ -217,7 +218,9 @@ class DockingServer : public nav2_util::LifecycleNode
 
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
-  std::mutex dynamic_params_lock_;
+
+  // Mutex for dynamic parameters and dock database
+  std::shared_ptr<std::mutex> mutex_;
 
   // Frequency to run control loops
   double controller_frequency_;

opennav_docking/src/dock_database.cpp

@@ -17,8 +17,9 @@
 namespace opennav_docking
 {
 
-DockDatabase::DockDatabase()
-: dock_loader_("opennav_docking_core", "opennav_docking_core::ChargingDock")
+DockDatabase::DockDatabase(std::shared_ptr<std::mutex> mutex)
+: mutex_(mutex),
+  dock_loader_("opennav_docking_core", "opennav_docking_core::ChargingDock")
 {}
 
 DockDatabase::~DockDatabase()
@@ -71,13 +72,21 @@ void DockDatabase::reloadDbCb(
   const std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Request> request,
   std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Response> response)
 {
+  if (!mutex_->try_lock()) {
+    RCLCPP_ERROR(node_.lock()->get_logger(), "Cannot reload database while docking!");
+    response->success = false;
+    return;
+  }
+
   DockMap dock_instances;
   if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) {
     dock_instances_ = dock_instances;
     response->success = true;
+    mutex_->unlock();
     return;
   }
   response->success = false;
+  mutex_->unlock();
 }
 
 Dock * DockDatabase::findDock(const std::string & dock_id)

opennav_docking/src/docking_server.cpp

@@ -85,9 +85,10 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     true, server_options);
 
   // Create composed utilities
+  mutex_ = std::make_shared<std::mutex>();
   controller_ = std::make_unique<Controller>(node);
   navigator_ = std::make_unique<Navigator>(node);
-  dock_db_ = std::make_unique<DockDatabase>();
+  dock_db_ = std::make_unique<DockDatabase>(mutex_);
   if (!dock_db_->initialize(node, tf2_buffer_)) {
     return nav2_util::CallbackReturn::FAILURE;
   }
@@ -198,7 +199,7 @@ bool DockingServer::checkAndWarnIfPreempted(
 
 void DockingServer::dockRobot()
 {
-  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+  std::lock_guard<std::mutex> lock(*mutex_);
   action_start_time_ = this->now();
   rclcpp::Rate loop_rate(controller_frequency_);
 
@@ -540,7 +541,7 @@ bool DockingServer::getCommandToPose(
 
 void DockingServer::undockRobot()
 {
-  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+  std::lock_guard<std::mutex> lock(*mutex_);
   action_start_time_ = this->now();
   rclcpp::Rate loop_rate(controller_frequency_);
 
@@ -682,7 +683,7 @@ void DockingServer::publishDockingFeedback(uint16_t state)
 rcl_interfaces::msg::SetParametersResult
 DockingServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
 {
-  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+  std::lock_guard<std::mutex> lock(*mutex_);
 
   rcl_interfaces::msg::SetParametersResult result;
   for (auto parameter : parameters) {