(Un)docking action never finishes

[BertVanAcker]

I'm currently experimenting with the automated docking and undocking of the robot.
When I want to perform an undocking routine, my ROS2 node seems to freeze and I'm unable to determine whether or not the undocking action is finished or not. The same holds for the docking routine. The undocking movement does start however but after this, the node freezes.

My undocking function looks like this:

Undock action

def undock(self):
    self.undock_action_client.wait_for_server()
    undock_goal_result = self.undock_action_client.send_goal(Undock.Goal())
    if undock_goal_result.result.is_docked:
        print('Undocking failed')
    self.state = State.UNDOCKED

Within the statemachine, the LED changes colors to see the states but it never reaches the State.UNDOCKED.
In the docking routine, i used the async mechanism but also here, the action never stops

def dock(self):
    """Perform Undock action."""
    self.get_logger().info('Docking the robot...')
    self.dock_send_goal()

    while not self.isDockComplete():
        time.sleep(0.1)

    self.state = State.DOCKED

def dock_send_goal(self):
    goal_msg = DockServo.Goal()
    self.dock_action_client.wait_for_server()
    goal_future = self.dock_action_client.send_goal_async(goal_msg)

    rclpy.spin_until_future_complete(self, goal_future)

    self.dock_goal_handle = goal_future.result()

    if not self.dock_goal_handle.accepted:
        self.get_logger().info('Docking of the robot FAILED - Dock goal rejected')
        return

    self.dock_result_future = self.dock_goal_handle.get_result_async()

def isDockComplete(self):
    """
    Get status of Dock action.
    :return: ``True`` if docked, ``False`` otherwise.
    """
    if self.dock_result_future is None or not self.dock_result_future:
        return True

    rclpy.spin_until_future_complete(self, self.dock_result_future, timeout_sec=0.1)

    if self.dock_result_future.result():
        self.dock_status = self.dock_result_future.result().status
        if self.dock_status != GoalStatus.STATUS_SUCCEEDED:
            self.info(f'Goal with failed with status code: {self.status}')
            return True
    else:
        return False
   
    self.get_logger().info('Docking of the robot SUCCEEDED')
    return True 

Sometimes, if I'm waiting long enough, a docking routine can be performed after a undocking routine but the action never provides a result.
Would it be possible to show have a small example how the docking and undocking routines actually need to be performed and how to analyze the status of the action correctly?
Thanks in advance.

[kwujciak]

It is definitely possible to have an action dock/undock file that takes advantage of callbacks, but if you're looking for a more simple way, take a look at this script.

If you want to keep the same structure you're working with, try throwing in some print statements to find where it's getting stuck. We used classes and a main function at the end to organize it. This way you can spin the node and then shut it down when you're done - possibly avoiding confusion within itself. Additionally, it may be helpful to break your isDockComplete function into two: goal response callback and get result callback.

[Rebecca-iRobot]

I defer to @kwujciak 's reply above as a primary answer. I also have encountered dock/undock actions "hanging" or never finishing. It seems to improve when I switch from cycloneDDS to fastRTPS or vice-versa. Worth a try!