Fixing docking server when already docked at the requeste ddock (#5171)

Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

nav2_docking/opennav_docking/src/docking_server.cpp

@@ -265,6 +265,8 @@ void DockingServer::dockRobot()
     if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) {
       RCLCPP_INFO(
         get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock");
+      result->success = true;
+      docking_action_server_->succeeded_current(result);
       return;
     }