ROS2: Template to program an Action Client

zurück zum Hauptartikel

#!/usr/bin/env python # this specifies the interpreter of this script

###
### Basic example of how to implement an action client
###

# import the ROS2 Python client libraries for nodes and actions
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

# import the interfaces used
from mypackage.action import InterfaceName


class MyActionClient(Node):

    def __init__(self):
        super().__init__('my_action_client')
        # create action client with 
        # 1) self: the node that contains the ActionClient
        # 2) the type of the Action (related to the interface, imported on top of this script
        # 3) the name of the Action
        # !! 1)-3) must be the same in ActionClient and ActionServer
        self._action_client = ActionClient(self, InterfaceName, 'MyActionClientName')

    def send_goal(self, seconds):
        # Start by creating a Goal() object of the InterfaceName action type
        goal_msg = InterfaceName.Goal()

        # set a variable
        goal_msg.some_variable_time = seconds

        # wait for the Action Server to be up and running
        self._action_client.wait_for_server()

        # send the goal to the Server using the send_goal_async method
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback)

        # assign a callback method to be triggered when the future is completed
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    #  this method will be triggered when the goal has been processed
    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    # assign a callback method to be triggered when the result is ready
    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Result: {0}'.format(result.success))
        # shut down the node
        rclpy.shutdown()

    # setting up the feedback
    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
           'Received feedback: {0}'.format(feedback.elapsed_time))


def main(args=None):
    rclpy.init(args=args)

    # creating an instance
    action_client = MyActionClient()

    # calling its send_goal() method, passing it five seconds as a parameter.
    action_client.send_goal(5.0)

    # spin the node so that the callback functions are properly executed.
    rclpy.spin(action_client)


if __name__ == '__main__':
    main()