Sir Quiesel
Sir Quiesel

Reputation: 1

ROS2 service callback function does not get called after service finished

I am working with webots and when the service Turn has finished (response appears in console) the futures callback function should be called but nothing happens.

robot_mover is the Node, I am calling the MoveRobotActions functions from another class which is the nodes class.

class MoveRobotActions():
    # False until the rotation has finished
    rotated: bool = False

    def turn_to_side(robot_mover):
        MoveRobotActions.rotated = False
        MoveRobotActions.rotate(robot_mover, robot_mover.turn_angle)
        robot_mover.get_logger().info(str(MoveRobotActions.rotated))
        while not MoveRobotActions.rotated:
            None
        robot_mover.get_logger().info(str(MoveRobotActions.rotated))
        robot_mover.check_distance()

    def rotate(robot_mover, angle_to_turn: float):
        client = robot_mover.create_client(Turn, "turn")

        # Wait until the service has started
        while not client.wait_for_service(1.0):
            robot_mover.get_logger().warn("Waiting for Turn service...")

        request = Turn.Request()
        if robot_mover.turn_to_right:
            request.angle = -angle_to_turn
            if (robot_mover.current_rotation < 3):
                robot_mover.current_rotation += 1
            else:
                robot_mover.current_rotation = 0
        else:
            request.angle = angle_to_turn
            if (robot_mover.current_rotation > 0):
                robot_mover.current_rotation -= 1
            else:
                robot_mover.current_rotation = 3

        robot_mover.get_logger().info("Calling Request...")

        robot_mover.future = client.call_async(request)

        robot_mover.get_logger().info("Request called. Adding Callback...")

        robot_mover.future.add_done_callback(MoveRobotActions.rotate_response)

        robot_mover.get_logger().info("Callback has been added. Start spinning...")

        rclpy.spin_until_future_complete(robot_mover, robot_mover.future)

        robot_mover.get_logger().info("Spinning finished.")
    
    def rotate_response(self, future: rclpy.Future):
        self.get_logger().info("Callback has been called.")
        MoveRobotActions.rotated = True

I want my robot to turn when the the function rotate gets called. In that function I have created a client to webots Turn service. Later in the function I use client.call_async and the robot turns. Then I add a done_callback to the future object and call rclpy.spin_until_future_complete. When I run the code I expect the robot to turn and when he has finished the rotate_response (the callback function that I passed) should be executed. When the turning has finished there is a response in vacusim_robot_driver console but the callback function will not be called.

The vacusim_robot_driver console The console of my node

Upvotes: 0

Views: 175

Answers (0)

Related Questions