diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index b43b70318c9..307eb2f37a6 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 configuring dock 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 diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp index 6774e936208..83ef42cd676 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() @@ -71,10 +82,14 @@ 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)) { + if (utils::parseDockFile(request->filepath, node, 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; } response->success = false; @@ -194,10 +209,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 diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp index 2753f34f1b3..0285e30f4a1 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);