Increase docking coverage && fix backward docking (#5097)

ajtudela · web-flow · commit 4b9b7110b1dd · 2025-04-25T12:11:51.000-07:00
* Add dock_database tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add utils test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve filter tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added simple charging tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve comments Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix backward and redo main test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Change test period to reduce test time Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert "Change test period to reduce test time" This reverts commit ef1555e. Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Delete print pose Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -169,7 +169,7 @@ install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
 
-install(FILES test/test_dock_file.yaml
+install(DIRECTORY test/dock_files
   DESTINATION share/${PROJECT_NAME}/
 )
 
diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp
@@ -203,7 +203,7 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share
       node->get_logger(), "Loading dock from database file  %s.", dock_filepath.c_str());
     try {
       return utils::parseDockFile(dock_filepath, node, dock_instances_);
-    } catch (YAML::ParserException & e) {
+    } catch (YAML::BadConversion & e) {
       RCLCPP_ERROR(
         node->get_logger(),
         "Dock database (%s) is malformed: %s.", dock_filepath.c_str(), e.what());
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -455,13 +455,6 @@ bool DockingServer::approachDock(
     geometry_msgs::msg::PoseStamped target_pose = dock_pose;
     target_pose.header.stamp = rclcpp::Time(0);
 
-    // Make sure that the target pose is pointing at the robot when moving backwards
-    // This is to ensure that the robot doesn't try to dock from the wrong side
-    if (backward) {
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
-        tf2::getYaw(target_pose.pose.orientation) + M_PI);
-    }
-
     // The control law can get jittery when close to the end when atan2's can explode.
     // Thus, we backward project the controller's target pose a little bit after the
     // dock so that the robot never gets to the end of the spiral before its in contact
@@ -472,6 +465,13 @@ bool DockingServer::approachDock(
     target_pose.pose.position.y += sin(yaw) * backward_projection;
     tf2_buffer_->transform(target_pose, target_pose, base_frame_);
 
+    // Make sure that the target pose is pointing at the robot when moving backwards
+    // This is to ensure that the robot doesn't try to dock from the wrong side
+    if (backward) {
+      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+        tf2::getYaw(target_pose.pose.orientation) + M_PI);
+    }
+
     // Compute and publish controls
     auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();
@@ -635,9 +635,19 @@ void DockingServer::undockRobot()
       return;
     }
 
+    bool dock_backward = dock_backwards_.has_value() ?
+      dock_backwards_.value() :
+      (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
+
     // Get "dock pose" by finding the robot pose
     geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);
 
+    // Make sure that the staging pose is pointing in the same direction when moving backwards
+    if (dock_backward) {
+      dock_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+        tf2::getYaw(dock_pose.pose.orientation) + M_PI);
+    }
+
     // Get staging pose (in fixed frame)
     geometry_msgs::msg::PoseStamped staging_pose =
       dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id);
@@ -669,9 +679,6 @@ void DockingServer::undockRobot()
       // Get command to approach staging pose
       auto command = std::make_unique<geometry_msgs::msg::TwistStamped>();
       command->header.stamp = now();
-      bool dock_backward = dock_backwards_.has_value() ?
-        dock_backwards_.value() :
-        (dock->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD);
 
       if (getCommandToPose(
           command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false,
diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt
@@ -111,4 +111,20 @@ target_link_libraries(test_docking_server_unit
 )
 
 # Test docking server (smoke)
-ament_add_pytest_test("test_docking_server" test_docking_server.py)
+ament_add_pytest_test(test_docking_server_with_charging_dock test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=False
+  BACKWARD=False
+)
+
+ament_add_pytest_test(test_docking_server_with_non_charging_dock test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=True
+  BACKWARD=False
+)
+
+ament_add_pytest_test(test_docking_server_backward test_docking_server.py
+  ENV
+  NON_CHARGING_DOCK=False
+  BACKWARD=True
+)
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_conversion_file.yaml
@@ -0,0 +1,5 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "map"
+    pose: [0.3, map, 0.0]
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_bad_pose_file.yaml
@@ -0,0 +1,5 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
+    pose: [0.3, 0.3]
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_file.yaml
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_pose_file.yaml
@@ -0,0 +1,4 @@
+docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_dock_no_type_file.yaml
@@ -0,0 +1,4 @@
+docks:
+  dock1:
+    frame: "mapA"
+    pose: [0.3, 0.3, 0.0]
diff --git a/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml b/nav2_docking/opennav_docking/test/dock_files/test_no_docks_file.yaml
@@ -0,0 +1,5 @@
+no_docks:
+  dock1:
+    type: "dockv3"
+    frame: "mapA"
+    pose: [0.3, 0.3, 0.0]
diff --git a/nav2_docking/opennav_docking/test/docking_params.yaml b/nav2_docking/opennav_docking/test/docking_params.yaml
@@ -0,0 +1,18 @@
+docking_server:
+  ros__parameters:
+    wait_charge_timeout: 1.0
+    controller:
+      use_collision_detection: False
+      transform_tolerance: 0.5
+    dock_plugins: [test_dock_plugin]
+    test_dock_plugin:
+      plugin: opennav_docking::SimpleChargingDock
+      use_battery_status: True
+      dock_direction: forward
+      staging_yaw_offset: 0.0
+    docks: [test_dock]
+    test_dock:
+      type: test_dock_plugin
+      frame: odom
+      pose: [10.0, 0.0, 0.0]
+      tolerance: 0.5
diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp
@@ -91,6 +91,47 @@ TEST(DatabaseTests, findTests)
   db.findDockPlugin("");
 }
 
+TEST(DatabaseTests, getDockInstancesBadConversionFile)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // Set a valid path with a malformed file
+  node->declare_parameter(
+    "dock_database",
+    rclcpp::ParameterValue(ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_bad_conversion_file.yaml"));
+
+  opennav_docking::DockDatabase db;
+  db.initialize(node, nullptr);
+
+  EXPECT_EQ(db.plugin_size(), 1u);
+  EXPECT_EQ(db.instance_size(), 0u);
+}
+
+TEST(DatabaseTests, getDockInstancesWrongPath)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // Set a wrong path
+  node->declare_parameter("dock_database", rclcpp::ParameterValue("file_does_not_exist.yaml"));
+
+  opennav_docking::DockDatabase db;
+  db.initialize(node, nullptr);
+
+  EXPECT_EQ(db.plugin_size(), 1u);
+  EXPECT_EQ(db.instance_size(), 0u);
+}
+
 TEST(DatabaseTests, reloadDbService)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
@@ -108,7 +149,7 @@ TEST(DatabaseTests, reloadDbService)
 
   auto request = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
   request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
-    "/test_dock_file.yaml";
+    "/dock_files/test_dock_file.yaml";
   EXPECT_TRUE(client->wait_for_service(1s));
   auto result = client->async_send_request(request);
   EXPECT_EQ(
@@ -128,6 +169,38 @@ TEST(DatabaseTests, reloadDbService)
   EXPECT_FALSE(result2.get()->success);
 }
 
+TEST(DatabaseTests, reloadDbMutexLocked)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::vector<std::string> plugins{"dockv1"};
+  node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
+
+  // This mutex is locked when dock / undock is called
+  auto mutex = std::make_shared<std::mutex>();
+  mutex->lock();
+  opennav_docking::DockDatabase db(mutex);
+  db.initialize(node, nullptr);
+
+  // Call service with a filepath
+  auto client =
+    node->create_client<nav2_msgs::srv::ReloadDockDatabase>("test/reload_database");
+
+  auto request = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
+  request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
+    "/dock_files/test_dock_file.yaml";
+  EXPECT_TRUE(client->wait_for_service(1s));
+  auto result = client->async_send_request(request);
+  EXPECT_EQ(
+    rclcpp::spin_until_future_complete(node, result, 2s),
+    rclcpp::FutureReturnCode::SUCCESS);
+  EXPECT_FALSE(result.get()->success);
+
+  mutex->unlock();
+}
+
 }  // namespace opennav_docking
 
 int main(int argc, char **argv)
diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -13,19 +13,22 @@
 # limitations under the License.
 
 from math import acos, cos, sin
+import os
 import time
 import unittest
 
 from action_msgs.msg import GoalStatus
 from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
 # from launch.actions import SetEnvironmentVariable
+from launch.launch_context import LaunchContext
 from launch_ros.actions import Node
 import launch_testing
 import launch_testing.actions
 import launch_testing.asserts
 import launch_testing.markers
 import launch_testing.util
+from nav2_common.launch import RewrittenYaml
 from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot
 import pytest
 import rclpy
@@ -47,29 +50,39 @@
 # @pytest.mark.flaky(max_runs=5, min_passes=3)
 def generate_test_description() -> LaunchDescription:
 
+    # Use local param file
+    launch_dir = os.path.dirname(os.path.realpath(__file__))
+    params_file = os.path.join(launch_dir, 'docking_params.yaml')
+
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if os.getenv('NON_CHARGING_DOCK') == 'True':
+        param_substitutions.update({'plugin': 'opennav_docking::SimpleNonChargingDock'})
+
+    if os.getenv('BACKWARD') == 'True':
+        param_substitutions.update({'dock_direction': 'backward'})
+        param_substitutions.update({'staging_yaw_offset': '3.14'})
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True,
+    )
+
+    new_yaml = configured_params.perform(context)
+
     return LaunchDescription([
         # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
         # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
         Node(
             package='opennav_docking',
             executable='opennav_docking',
             name='docking_server',
-            parameters=[{'wait_charge_timeout': 1.0,
-                         'controller': {
-                             'use_collision_detection': False,
-                             'transform_tolerance': 0.5,
-                         },
-                         'dock_plugins': ['test_dock_plugin'],
-                         'test_dock_plugin': {
-                             'plugin': 'opennav_docking::SimpleChargingDock',
-                             'use_battery_status': True,
-                             'dock_direction': 'forward'},
-                         'docks': ['test_dock'],
-                         'test_dock': {
-                             'type': 'test_dock_plugin',
-                             'frame': 'odom',
-                             'pose': [10.0, 0.0, 0.0]
-                         }}],
+            parameters=[new_yaml],
             output='screen',
         ),
         Node(
@@ -100,6 +113,9 @@ def setUp(self) -> None:
         self.x = 0.0
         self.y = 0.0
         self.theta = 0.0
+        # If BACKWARD is set, start facing backward
+        if os.getenv('BACKWARD') == 'True':
+            self.theta = 3.14
         # Track charge state
         self.is_charging = False
         # Latest command velocity
@@ -133,13 +149,14 @@ def publish(self) -> None:
         t.transform.rotation.z = sin(self.theta / 2.0)
         t.transform.rotation.w = cos(self.theta / 2.0)
         self.tf_broadcaster.sendTransform(t)
-        # Publish battery state
-        b = BatteryState()
-        if self.is_charging:
-            b.current = 1.0
-        else:
-            b.current = -1.0
-        self.battery_state_pub.publish(b)
+        # Publish the battery state if we are using a charging dock
+        if os.getenv('NON_CHARGING_DOCK') == 'False':
+            b = BatteryState()
+            if self.is_charging:
+                b.current = 1.0
+            else:
+                b.current = -1.0
+            self.battery_state_pub.publish(b)
 
     def action_feedback_callback(self, msg: DockRobot.Feedback) -> None:
         # Force the docking action to run a full recovery loop and then
@@ -186,12 +203,13 @@ def test_docking_server(self) -> None:
             10
         )
 
-        # Create publisher for battery state message
-        self.battery_state_pub = self.node.create_publisher(
-            BatteryState,
-            'battery_state',
-            10
-        )
+        # Create publisher for battery state message if we are using a charging dock
+        if os.getenv('NON_CHARGING_DOCK') == 'False':
+            self.battery_state_pub = self.node.create_publisher(
+                BatteryState,
+                'battery_state',
+                10
+            )
 
         # Mock out navigation server (just teleport the robot)
         self.action_server = ActionServer(
@@ -278,7 +296,10 @@ def test_docking_server(self) -> None:
         if self.action_result[2] is not None:
             self.assertEqual(self.action_result[2].status, GoalStatus.STATUS_SUCCEEDED)
             self.assertTrue(self.action_result[2].result.success)
-            self.assertEqual(self.action_result[2].result.num_retries, 1)
+            if os.getenv('NON_CHARGING_DOCK') == 'False':
+                self.assertEqual(self.action_result[2].result.num_retries, 1)
+            else:
+                self.assertEqual(self.action_result[2].result.num_retries, 0)
 
         # Test undocking action
         self.is_charging = False
diff --git a/nav2_docking/opennav_docking/test/test_pose_filter.cpp b/nav2_docking/opennav_docking/test/test_pose_filter.cpp
diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp
diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp
diff --git a/nav2_docking/opennav_docking/test/test_utils.cpp b/nav2_docking/opennav_docking/test/test_utils.cpp

Original file line number	Diff line number	Diff line change
`@@ -169,7 +169,7 @@ install(DIRECTORY include/`
`169`	`169`	`DESTINATION include/${PROJECT_NAME}`
`170`	`170`	`)`
`171`	`171`
`172`		`-install(FILES test/test_dock_file.yaml`
	`172`	`+install(DIRECTORY test/dock_files`
`173`	`173`	`DESTINATION share/${PROJECT_NAME}/`
`174`	`174`	`)`
`175`	`175`
-Original file line number
+Diff line change
@@ @@ -0,0 +1,4 @@ @@
 +docks:
 +  dock1:
 +    type: "dockv3"
 +    frame: "mapA"