diff --git a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp index 95bde05e19..e6bcf96eac 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #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 mutex = std::make_shared()); /** * @brief A setup function to populate database @@ -127,6 +128,7 @@ class DockDatabase std::shared_ptr response); rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::shared_ptr mutex_; // Don't reload database while actively docking DockPluginMap dock_plugins_; DockMap dock_instances_; pluginlib::ClassLoader dock_loader_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index f49cb6ff90..bab26bef5a 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #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 mutex_; // Frequency to run control loops double controller_frequency_; diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 83ef42cd67..b6a1f88881 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/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 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 request, std::shared_ptr 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) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 52b9ad9d86..e1b7c19279 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/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(); controller_ = std::make_unique(node); navigator_ = std::make_unique(node); - dock_db_ = std::make_unique(); + dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { return nav2_util::CallbackReturn::FAILURE; } @@ -199,7 +200,7 @@ bool DockingServer::checkAndWarnIfPreempted( void DockingServer::dockRobot() { - std::lock_guard lock(dynamic_params_lock_); + std::lock_guard lock(*mutex_); action_start_time_ = this->now(); rclcpp::Rate loop_rate(controller_frequency_); @@ -547,7 +548,7 @@ bool DockingServer::getCommandToPose( void DockingServer::undockRobot() { - std::lock_guard lock(dynamic_params_lock_); + std::lock_guard lock(*mutex_); action_start_time_ = this->now(); rclcpp::Rate loop_rate(controller_frequency_); @@ -695,7 +696,7 @@ void DockingServer::publishDockingFeedback(uint16_t state) rcl_interfaces::msg::SetParametersResult DockingServer::dynamicParametersCallback(std::vector parameters) { - std::lock_guard lock(dynamic_params_lock_); + std::lock_guard lock(*mutex_); rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) {