[Docking] Feature: support for not needing to set dock instances (ros…

…-navigation#4442) * feat: support for not needing to set dock_instances Signed-off-by: josegarcia <[email protected]> * feat: improve logs for dock_database Signed-off-by: josegarcia <[email protected]> * chore: comment the dock instances out from nav2_params Signed-off-by: josegarcia <[email protected]> * chore: dock database cleanup Signed-off-by: josegarcia <[email protected]> * fix: update reloadDBService from DatabaseTests Signed-off-by: josegarcia <[email protected]> --------- Signed-off-by: josegarcia <[email protected]> Co-authored-by: josegarcia <[email protected]>
ajtudela · Jun 21, 2024 · 03aed17 · 03aed17
1 parent 3d48c4d
commit 03aed17
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 16 deletions.
diff --git 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
@@ -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<nav2_msgs::srv::ReloadDockDatabase>(
     "~/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<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> 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
@@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests)
 TEST(DatabaseTests, reloadDbService)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
-  std::vector<std::string> plugins{"dockv1", "dockv2"};
+  std::vector<std::string> 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);