Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add mutex to reloadDbCb to fail if called during docking #4574

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Add mutex to reloadDbCb to fail if called during docking
Signed-off-by: redvinaa <redvinaa@gmail.com>
  • Loading branch information
redvinaa committed Jul 29, 2024
commit 00047d2cda7f974a492714b87482b8b990c14db1
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <vector>
#include <memory>
#include <string>
#include <mutex>

#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_loader.hpp"
Expand All @@ -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
Expand Down Expand Up @@ -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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <vector>
#include <memory>
#include <string>
#include <mutex>

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
Expand Down Expand Up @@ -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_;
Expand Down
13 changes: 11 additions & 2 deletions nav2_docking/opennav_docking/src/dock_database.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -82,6 +83,12 @@
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;

Check warning on line 88 in nav2_docking/opennav_docking/src/dock_database.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_docking/opennav_docking/src/dock_database.cpp#L87-L88

Added lines #L87 - L88 were not covered by tests
return;
}

auto node = node_.lock();
DockMap dock_instances;
if (utils::parseDockFile(request->filepath, node, dock_instances)) {
Expand All @@ -90,9 +97,11 @@
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)
Expand Down
9 changes: 5 additions & 4 deletions nav2_docking/opennav_docking/src/docking_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -199,7 +200,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_);

Expand Down Expand Up @@ -547,7 +548,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_);

Expand Down Expand Up @@ -695,7 +696,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) {
Expand Down
Loading