Add mutex to reloadDbCb to fail if called during docking (#4574) (#4576)

Signed-off-by: redvinaa <redvinaa@gmail.com>
(cherry picked from commit 57415d6)

Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com>

nav2_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<nav2_msgs::srv::ReloadDockDatabase::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_;

nav2_docking/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_;

nav2_docking/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()
@@ -82,6 +83,12 @@ void DockDatabase::reloadDbCb(
   const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
+  if (!mutex_->try_lock()) {
+    RCLCPP_ERROR(node_.lock()->get_logger(), "Cannot reload database while docking!");
+    response->success = false;
+    return;
+  }
+
   auto node = node_.lock();
   DockMap dock_instances;
   if (utils::parseDockFile(request->filepath, node, dock_instances)) {
@@ -90,9 +97,11 @@ void DockDatabase::reloadDbCb(
     RCLCPP_INFO(
       node->get_logger(),
       "Dock database reloaded from file %s.", request->filepath.c_str());
+    mutex_->unlock();
     return;
   }
   response->success = false;
+  mutex_->unlock();
 }
 
 Dock * DockDatabase::findDock(const std::string & dock_id)

nav2_docking/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_);
 
@@ -546,7 +547,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_);
 
@@ -694,7 +695,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) {