Fix unexpected behavior of docking when already in charger (ros-navig…

…ation#4448) * Check isDocked before dock/undock Signed-off-by: Alberto Tudela <[email protected]> * Linting Signed-off-by: Alberto Tudela <[email protected]> * Add testing Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]>
quasi-robotics · Oct 23, 2024 · 99dccb6 · 99dccb6
1 parent 26bdf11
commit 99dccb6
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 4 deletions.
diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -235,6 +235,12 @@ void DockingServer::dockRobot()
       dock = generateGoalDock(goal);
     }
 
+    // Check if the robot is docked or charging before proceeding
+    if (dock->plugin->isDocked() || dock->plugin->isCharging()) {
+      RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock");
+      return;
+    }
+
     // Send robot to its staging pose
     publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
     const auto initial_staging_pose = dock->getStagingPose();
@@ -569,6 +575,12 @@ void DockingServer::undockRobot()
       get_logger(),
       "Attempting to undock robot from charger of type %s.", dock->getName().c_str());
 
+    // Check if the robot is docked before proceeding
+    if (!dock->isDocked()) {
+      RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock");
+      return;
+    }
+
     // Get "dock pose" by finding the robot pose
     geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);
 

diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
@@ -86,9 +86,8 @@ TEST(DockingServerTests, testErrorExceptions)
   node->on_configure(rclcpp_lifecycle::State());
   node->on_activate(rclcpp_lifecycle::State());
 
-  node->declare_parameter(
-    "exception_to_throw",
-    rclcpp::ParameterValue(""));
+  node->declare_parameter("exception_to_throw", rclcpp::ParameterValue(""));
+  node->declare_parameter("dock_action_called", rclcpp::ParameterValue(false));
 
   // Error codes docking
   std::vector<std::string> error_ids{
@@ -142,6 +141,9 @@ TEST(DockingServerTests, testErrorExceptions)
     "DockingException", "exception"};
   std::vector<int> error_codes_undocking{999, 902, 905, 999, 999};
 
+  // Set dock_action_called to true to simulate robot being docked in dock plugin
+  node->set_parameter(rclcpp::Parameter("dock_action_called", true));
+
   // Call action, check error code
   for (unsigned int i = 0; i != error_ids_undocking.size(); i++) {
     node->set_parameter(

diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp
@@ -80,7 +80,9 @@ class TestFailureDock : public opennav_docking_core::ChargingDock
 
   virtual bool isDocked()
   {
-    return false;
+    bool dock_action_called;
+    node_->get_parameter("dock_action_called", dock_action_called);
+    return dock_action_called;
   }
 
   virtual bool isCharging()