From 07c3802c6384f3d634f687381859ddec99c9f626 Mon Sep 17 00:00:00 2001 From: josegarcia Date: Mon, 17 Jun 2024 11:06:08 +0200 Subject: [PATCH 1/5] feat: support for not needing to set dock_instances Signed-off-by: josegarcia --- .../opennav_docking/src/dock_database.cpp | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 6774e93620..e1f335e9c6 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -34,21 +34,32 @@ bool DockDatabase::initialize( node_ = parent; auto node = node_.lock(); - if (getDockPlugins(node, tf) && getDockInstances(node)) { - RCLCPP_INFO( + if (!getDockPlugins(node, tf)) { + RCLCPP_ERROR( node->get_logger(), - "Docking Server has %u dock types and %u dock instances available.", - this->plugin_size(), this->instance_size()); - return true; + "An error occurred while getting the dock plugins!"); + return false; } + if (!getDockInstances(node)) { + RCLCPP_ERROR( + node->get_logger(), + "An error occurred while getting the dock instances!"); + return false; + } + + RCLCPP_INFO( + node->get_logger(), + "Docking Server has %u dock types and %u dock instances available.", + this->plugin_size(), this->instance_size()); + reload_db_service_ = node->create_service( "~/reload_database", std::bind( &DockDatabase::reloadDbCb, this, std::placeholders::_1, std::placeholders::_2)); - return false; + return true; } void DockDatabase::activate() @@ -194,10 +205,12 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share return utils::parseDockParams(docks_param, node, dock_instances_); } - RCLCPP_ERROR( + RCLCPP_WARN( node->get_logger(), - "Dock database filepath nor dock parameters set. Unable to perform docking actions."); - return false; + "Dock database filepath nor dock parameters set. " + "Docking actions can only be executed specifying the dock pose via the action request. " + "Or update the dock database via the reload_database service."); + return true; } unsigned int DockDatabase::plugin_size() const From 9faf5f5e189c8f721b48bc66c5921886a8035c75 Mon Sep 17 00:00:00 2001 From: josegarcia Date: Mon, 17 Jun 2024 11:08:16 +0200 Subject: [PATCH 2/5] feat: improve logs for dock_database Signed-off-by: josegarcia --- .../opennav_docking/src/dock_database.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index e1f335e9c6..f115014ef9 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -82,12 +82,20 @@ void DockDatabase::reloadDbCb( const std::shared_ptr request, std::shared_ptr response) { + auto node = node_.lock(); DockMap dock_instances; if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) { dock_instances_ = dock_instances; response->success = true; + RCLCPP_INFO( + node->get_logger(), + "Dock database reloaded from file %s.", request->filepath.c_str()); return; } + + RCLCPP_ERROR( + node->get_logger(), + "Dock database reload failed from file %s.", request->filepath.c_str()); response->success = false; } @@ -110,6 +118,17 @@ Dock * DockDatabase::findDock(const std::string & dock_id) Dock * DockDatabase::findDockInstance(const std::string & dock_id) { + if (dock_instances_.empty()) { + auto node = node_.lock(); + RCLCPP_WARN( + node->get_logger(), + "Dock database filepath nor dock parameters set. " + "Docking actions can only be executed specifying the dock pose via the action request. " + "Or update the dock database via the reload_database service."); + + return nullptr; + } + auto it = dock_instances_.find(dock_id); if (it != dock_instances_.end()) { return &(it->second); From b52631d2c87535127b2b7b90d2cb0576b8701df0 Mon Sep 17 00:00:00 2001 From: josegarcia Date: Mon, 17 Jun 2024 11:08:53 +0200 Subject: [PATCH 3/5] chore: comment the dock instances out from nav2_params Signed-off-by: josegarcia --- nav2_bringup/params/nav2_params.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b43b70318c..4ef22f6d4c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -407,11 +407,12 @@ docking_server: filter_coef: 0.1 # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: - type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] + # The following example illustrates how you should configure the docks instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] controller: k_phi: 3.0 From ef7c466255b447ad5352260b1a761b4ef393efe7 Mon Sep 17 00:00:00 2001 From: josegarcia Date: Wed, 19 Jun 2024 09:12:29 +0200 Subject: [PATCH 4/5] chore: dock database cleanup Signed-off-by: josegarcia --- nav2_bringup/params/nav2_params.yaml | 2 +- .../opennav_docking/src/dock_database.cpp | 17 +---------------- 2 files changed, 2 insertions(+), 17 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 4ef22f6d4c..307eb2f37a 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -407,7 +407,7 @@ docking_server: filter_coef: 0.1 # Dock instances - # The following example illustrates how you should configure the docks instances. + # The following example illustrates configuring dock instances. # docks: ['home_dock'] # Input your docks here # home_dock: # type: 'simple_charging_dock' diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index f115014ef9..83ef42cd67 100644 --- a/nav2_docking/opennav_docking/src/dock_database.cpp +++ b/nav2_docking/opennav_docking/src/dock_database.cpp @@ -84,7 +84,7 @@ void DockDatabase::reloadDbCb( { auto node = node_.lock(); DockMap dock_instances; - if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) { + if (utils::parseDockFile(request->filepath, node, dock_instances)) { dock_instances_ = dock_instances; response->success = true; RCLCPP_INFO( @@ -92,10 +92,6 @@ void DockDatabase::reloadDbCb( "Dock database reloaded from file %s.", request->filepath.c_str()); return; } - - RCLCPP_ERROR( - node->get_logger(), - "Dock database reload failed from file %s.", request->filepath.c_str()); response->success = false; } @@ -118,17 +114,6 @@ Dock * DockDatabase::findDock(const std::string & dock_id) Dock * DockDatabase::findDockInstance(const std::string & dock_id) { - if (dock_instances_.empty()) { - auto node = node_.lock(); - RCLCPP_WARN( - node->get_logger(), - "Dock database filepath nor dock parameters set. " - "Docking actions can only be executed specifying the dock pose via the action request. " - "Or update the dock database via the reload_database service."); - - return nullptr; - } - auto it = dock_instances_.find(dock_id); if (it != dock_instances_.end()) { return &(it->second); From 9ceccb5fef0d6084c0a68eb247fcaf8a9dbfa9fa Mon Sep 17 00:00:00 2001 From: josegarcia Date: Thu, 20 Jun 2024 10:54:32 +0200 Subject: [PATCH 5/5] fix: update reloadDBService from DatabaseTests Signed-off-by: josegarcia --- nav2_docking/opennav_docking/test/test_dock_database.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp index 2753f34f1b..0285e30f4a 100644 --- a/nav2_docking/opennav_docking/test/test_dock_database.cpp +++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp @@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests) TEST(DatabaseTests, reloadDbService) { auto node = std::make_shared("test"); - std::vector plugins{"dockv1", "dockv2"}; + std::vector plugins{"dockv1"}; node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins)); + node->declare_parameter( + "dockv1.plugin", + rclcpp::ParameterValue("opennav_docking::SimpleChargingDock")); opennav_docking::DockDatabase db; db.initialize(node, nullptr);