From 02524d599d002a46bee09d364a43daf3db249d29 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Carlos=20Garc=C3=ADa?=
 <40342885+jcarlosgm30@users.noreply.github.com>
Date: Thu, 20 Jun 2024 17:54:59 +0200
Subject: [PATCH] [Docking] Feature: support for not needing to set dock
 instances (#4442)

* feat: support for not needing to set dock_instances

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* feat: improve logs for dock_database

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* chore: comment the dock instances out from nav2_params

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* chore: dock database cleanup

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* fix: update reloadDBService from DatabaseTests

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

---------

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>
Co-authored-by: josegarcia <josegarcia@pal-robotics.com>
---
 nav2_bringup/params/nav2_params.yaml          | 11 +++---
 .../opennav_docking/src/dock_database.cpp     | 37 ++++++++++++++-----
 .../test/test_dock_database.cpp               |  5 ++-
 3 files changed, 37 insertions(+), 16 deletions(-)

diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index b43b70318c..307eb2f37a 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 6774e93620..83ef42cd67 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<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
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<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);