Action Servers

Action Servers#

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

Added in version Jazzy: Added this section.

Action servers will rely indirectly on our capabilities of working well with messages and services. Although actions are unique in the way they work, they will repeat many of the structures we used so far. The action server, for instance, will be very much like a service server.

While being conscious of our objectives, for any action server, it is important to:

  1. Receive the goal and process it in a meaningful way.

  2. Publish feedback as it becomes available. Without feedback, an Action might always be replaced more effectively by a Service.

  3. Set the final state of the goal and send a Result.

Diagram#

This is the sequence diagram from the point of view of the action server. The major difference with a service server is the need to process and send feedback until the action is completed or aborted.

%%{init: { "theme" : "dark" }}%% sequenceDiagram participant Action Client as Action Client participant Action Server as Action Server autonumber Action Client ->>+ Action Server: Receive Goal loop While action has not ended Action Server -->> Action Client: Publish Feedback end Action Server -->>- Action Client: Send Result

Files#

The highlighted files below will be modified or created in this section.

File structure

python_package_that_uses_the_actions/
|-- package.xml
|-- python_package_that_uses_the_actions
|   |-- __init__.py
|   |-- move_straight_in_2d_action_client_node.py
|   `-- move_straight_in_2d_action_server_node.py
|-- resource
|   `-- python_package_that_uses_the_actions
|-- setup.cfg
|-- setup.py
`-- test
    |-- test_copyright.py
    |-- test_flake8.py
    `-- test_pep257.py

Action Server#

In this section, we will do the following.

  1. Create the node with an ActionServer.

  2. Update the setup.py so that ros2 run finds the node.

Let us create the action server as follows.

move_straight_in_2d_action_server_node.py

  1import time
  2from math import sqrt
  3
  4import rclpy
  5from rclpy.action import ActionServer
  6from rclpy.action.server import ServerGoalHandle
  7from rclpy.node import Node
  8
  9from geometry_msgs.msg import Point
 10from package_with_interfaces.action import MoveStraightIn2D
 11
 12
 13class MoveStraightIn2DActionServerNode(Node):
 14    """A ROS2 Node with an Action Server for MoveStraightIn2D."""
 15
 16    def __init__(self):
 17        super().__init__('move_straight_in_2d_action_server')
 18        self.current_position = Point()
 19        self.MAX_ITERATIONS: int  = 100
 20        self.sampling_time: float = 0.01
 21
 22        self.action_server = ActionServer(
 23            self,
 24            MoveStraightIn2D,
 25            '/move_straight_in_2d',
 26            self.execute_callback)
 27
 28
 29
 30    def get_distance(self, desired_position: Point) -> float:
 31        """
 32        Calculates the error norm (e.g. Euclidean distance) between the current position and the desired position.
 33        Notice that we have chosen to ignore the z-axis as this is a 2D motion.
 34        """
 35
 36        x = self.current_position.x
 37        y = self.current_position.y
 38        xd = desired_position.x
 39        yd = desired_position.y
 40
 41        return sqrt((x - xd) ** 2 + (y - yd) ** 2)
 42
 43    def move_the_object_with_velocity(self, desired_position: Point, desired_speed: float = 1.0) -> None:
 44        """
 45        Moves the object with the desired speed for one iteration. Must be called until objective is reached or
 46        the controller times out.
 47        """
 48        distance = self.get_distance(desired_position)
 49        # Prevent us from dividing by zero or a small number we currently do not care about
 50        if distance < 0.01:
 51            return
 52
 53        x_direction = (self.current_position.x - desired_position.x) / distance
 54        y_direction = (self.current_position.y - desired_position.y) / distance
 55
 56        # Apply new position based on the desired velocity and direction
 57        self.current_position.x -= x_direction * desired_speed * self.sampling_time
 58        self.current_position.y -= y_direction * desired_speed * self.sampling_time
 59
 60
 61    def execute_callback(self, goal: ServerGoalHandle) -> MoveStraightIn2D.Result:
 62        """
 63        To be attached to the Action as its callback. Receives a goal position and tries to move to that position.
 64        It will return the Euclidean distance between the current position and the desired position as the feedback.
 65        If the goal is reached within a given threshold it will succeed, otherwise it will abort.
 66        """
 67        desired_position = goal.request.desired_position
 68
 69        self.get_logger().info(f'current_position is {self.current_position}.')
 70        self.get_logger().info(f'desired_position set to {desired_position}.')
 71
 72        feedback_msg = MoveStraightIn2D.Feedback()
 73
 74        # Let's limit the maximum number of iterations this can accept
 75        for i in range(self.MAX_ITERATIONS):
 76            distance = self.get_distance(desired_position)
 77            feedback_msg.distance = distance
 78            goal.publish_feedback(feedback_msg)
 79
 80            self.move_the_object_with_velocity(desired_position)
 81
 82            # We define a threshold to see if it managed to reach the goal or not.
 83            if distance < 0.01:
 84                goal.succeed()
 85                break
 86
 87            # A sleep illustrating the time it would take in real life for a robot to move
 88            time.sleep(self.sampling_time)
 89
 90        result = MoveStraightIn2D.Result()
 91        result.final_position = self.current_position
 92        return result
 93
 94
 95def main(args=None):
 96    """
 97    The main function.
 98    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
 99    """
100    try:
101        rclpy.init(args=args)
102
103        node = MoveStraightIn2DActionServerNode()
104
105        rclpy.spin(node)
106    except KeyboardInterrupt:
107        pass
108    except Exception as e:
109        print(e)
110
111
112if __name__ == '__main__':
113    main()

As always, our class must inherit from rclpy.node.Node, so that it can be used as argument of rclpy.spin().

An action server is an instance of rclpy.action.ActionServer. As input to the initializer of ActionServer, we need an action, a string that names this action server, and a suitable callback.

class MoveStraightIn2DActionServerNode(Node):
    """A ROS2 Node with an Action Server for MoveStraightIn2D."""

    def __init__(self):
        super().__init__('move_straight_in_2d_action_server')
        self.current_position = Point()
        self.MAX_ITERATIONS: int  = 100
        self.sampling_time: float = 0.01

        self.action_server = ActionServer(
            self,
            MoveStraightIn2D,
            '/move_straight_in_2d',
            self.execute_callback)

The callback is likely to be the most complex aspect of an action server. The callback must receive an instance of ServerGoalHandle. This will be managed automatically by ROS2. It must return an instance of the class MyAction.Result instance for any action MyAction you are serving. This must be done manually by the programmer of the server. Note that the MyAction.Result class is created for you via colcon build.

The request can be obtained using goal.request of type MyAction.Request. Given that our action has the field desired_position we can access it this way.

A feedback message must be sent as suitable. One can be created with the MyAction.Feedback class, which is created for you via colcon build. After creating a suitable message, it can be sent with goal.publish_feedback.

If the action is successful, it is important to set goal.succeed. There are instances in which you would like to abort the goal early. This does not apply to this example because it will fail naturally after it times out. If you do not set a goal as succeeded, it will automatically be aborted at the end of the callback.

Lastly, the method must return an instance of MyAction.Result. In our case, we have the field final_position in the action file description, therefore we populate it as needed.

    def execute_callback(self, goal: ServerGoalHandle) -> MoveStraightIn2D.Result:
        """
        To be attached to the Action as its callback. Receives a goal position and tries to move to that position.
        It will return the Euclidean distance between the current position and the desired position as the feedback.
        If the goal is reached within a given threshold it will succeed, otherwise it will abort.
        """
        desired_position = goal.request.desired_position

        self.get_logger().info(f'current_position is {self.current_position}.')
        self.get_logger().info(f'desired_position set to {desired_position}.')

        feedback_msg = MoveStraightIn2D.Feedback()

        # Let's limit the maximum number of iterations this can accept
        for i in range(self.MAX_ITERATIONS):
            distance = self.get_distance(desired_position)
            feedback_msg.distance = distance
            goal.publish_feedback(feedback_msg)

            self.move_the_object_with_velocity(desired_position)

            # We define a threshold to see if it managed to reach the goal or not.
            if distance < 0.01:
                goal.succeed()
                break

            # A sleep illustrating the time it would take in real life for a robot to move
            time.sleep(self.sampling_time)

        result = MoveStraightIn2D.Result()
        result.final_position = self.current_position
        return result

The other methods are to support the important aspects of the action server. The get_distance method will compute the Euclidean distance between current_position and desired_position. Remember The distance equation. This is represented by the following piece of code.

    def get_distance(self, desired_position: Point) -> float:
        """
        Calculates the error norm (e.g. Euclidean distance) between the current position and the desired position.
        Notice that we have chosen to ignore the z-axis as this is a 2D motion.
        """

        x = self.current_position.x
        y = self.current_position.y
        xd = desired_position.x
        yd = desired_position.y

        return sqrt((x - xd) ** 2 + (y - yd) ** 2)

The action server will update the current_position based on a simple constant speed motion of the point. Remember The controller equation. This is represented by the following piece of code.

    def move_the_object_with_velocity(self, desired_position: Point, desired_speed: float = 1.0) -> None:
        """
        Moves the object with the desired speed for one iteration. Must be called until objective is reached or
        the controller times out.
        """
        distance = self.get_distance(desired_position)
        # Prevent us from dividing by zero or a small number we currently do not care about
        if distance < 0.01:
            return

        x_direction = (self.current_position.x - desired_position.x) / distance
        y_direction = (self.current_position.y - desired_position.y) / distance

        # Apply new position based on the desired velocity and direction
        self.current_position.x -= x_direction * desired_speed * self.sampling_time
        self.current_position.y -= y_direction * desired_speed * self.sampling_time

Note that we use an arbitrary minimum distance as stop criterion. Do not expect the distance to ever be zero, given that we are using floating point representation for the numbers. If you do not add a stopping range it is likely that your node will be stuck forever.

Adjusting the setup.py#

This file will include the directives for the server and client. The one for the server is highlighted below.

from setuptools import find_packages, setup

package_name = 'python_package_that_uses_the_actions'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='murilo.marinho@manchester.ac.uk',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'move_straight_in_2d_action_server_node = python_package_that_uses_the_actions.move_straight_in_2d_action_server_node:main',
            'move_straight_in_2d_action_client_node = python_package_that_uses_the_actions.move_straight_in_2d_action_client_node:main'
        ],
    },
)

Build and source#

Before we proceed, let us build and source once.

cd ~/ros2_tutorial_workspace
colcon build
source install/setup.bash

Note

For additional explanation and troubleshooting tips, see Always source after you build.

Warning

colcon will not work properly if your terminal has an active venv.

Testing the Action Server#

We will be working with two terminal windows. The first will run the action server. The second, ros2 action send_goal.

ros2 run python_package_that_uses_the_actions move_straight_in_2d_action_server_node

Please note the --feedback flag, used to allow us to see the feedback that comes from the server. Aside from that, the formation of the command is similar to topics and services.

ros2 action send_goal --feedback \
/move_straight_in_2d \
package_with_interfaces/action/MoveStraightIn2D \
'{
desired_position:{
    x: 1.0,
    y: 0.0,
    z: 0.0}
}'

This is what will be shown in each terminal

[INFO] [1759841973.596577423] [move_straight_in_2d_action_server]: current_position is geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0).
[INFO] [1759841973.596757548] [move_straight_in_2d_action_server]: desired_position set to geometry_msgs.msg.Point(x=1.0, y=0.0, z=0.0).

Waiting for an action server to become available...
Sending goal:
     desired_position:
  x: 1.0
  y: 0.0
  z: 0.0

Goal accepted with ID: 09c38042aaf846f49a87523dddf50900

Feedback:
    distance: 1.0

Feedback:
    distance: 0.9900000095367432

Feedback:
    distance: 0.9800000190734863

Feedback:
    distance: 0.9700000286102295

Feedback:
    distance: 0.9599999785423279

Feedback:
    distance: 0.949999988079071

Feedback:
    distance: 0.9399999976158142

Feedback:
    distance: 0.9300000071525574

Feedback:
    distance: 0.9200000166893005

Feedback:
    distance: 0.9100000262260437

Feedback:
    distance: 0.8999999761581421

Feedback:
    distance: 0.8899999856948853

Feedback:
    distance: 0.8799999952316284

Feedback:
    distance: 0.8700000047683716

Feedback:
    distance: 0.8600000143051147

Feedback:
    distance: 0.8500000238418579

Feedback:
    distance: 0.8399999737739563

Feedback:
    distance: 0.8299999833106995

Feedback:
    distance: 0.8199999928474426

Feedback:
    distance: 0.8100000023841858

Feedback:
    distance: 0.800000011920929

Feedback:
    distance: 0.7900000214576721

Feedback:
    distance: 0.7799999713897705

Feedback:
    distance: 0.7699999809265137

Feedback:
    distance: 0.7599999904632568

Feedback:
    distance: 0.75

Feedback:
    distance: 0.7400000095367432

Feedback:
    distance: 0.7300000190734863

Feedback:
    distance: 0.7200000286102295

Feedback:
    distance: 0.7099999785423279

Feedback:
    distance: 0.699999988079071

Feedback:
    distance: 0.6899999976158142

Feedback:
    distance: 0.6800000071525574

Feedback:
    distance: 0.6700000166893005

Feedback:
    distance: 0.6600000262260437

Feedback:
    distance: 0.6499999761581421

Feedback:
    distance: 0.6399999856948853

Feedback:
    distance: 0.6299999952316284

Feedback:
    distance: 0.6200000047683716

Feedback:
    distance: 0.6100000143051147

Feedback:
    distance: 0.6000000238418579

Feedback:
    distance: 0.5899999737739563

Feedback:
    distance: 0.5799999833106995

Feedback:
    distance: 0.5699999928474426

Feedback:
    distance: 0.5600000023841858

Feedback:
    distance: 0.550000011920929

Feedback:
    distance: 0.5400000214576721

Feedback:
    distance: 0.5299999713897705

Feedback:
    distance: 0.5199999809265137

Feedback:
    distance: 0.5099999904632568

Feedback:
    distance: 0.5

Feedback:
    distance: 0.49000000953674316

Feedback:
    distance: 0.47999998927116394

Feedback:
    distance: 0.4699999988079071

Feedback:
    distance: 0.46000000834465027

Feedback:
    distance: 0.44999998807907104

Feedback:
    distance: 0.4399999976158142

Feedback:
    distance: 0.4300000071525574

Feedback:
    distance: 0.41999998688697815

Feedback:
    distance: 0.4099999964237213

Feedback:
    distance: 0.4000000059604645

Feedback:
    distance: 0.38999998569488525

Feedback:
    distance: 0.3799999952316284

Feedback:
    distance: 0.3700000047683716

Feedback:
    distance: 0.36000001430511475

Feedback:
    distance: 0.3499999940395355

Feedback:
    distance: 0.3400000035762787

Feedback:
    distance: 0.33000001311302185

Feedback:
    distance: 0.3199999928474426

Feedback:
    distance: 0.3100000023841858

Feedback:
    distance: 0.30000001192092896

Feedback:
    distance: 0.28999999165534973

Feedback:
    distance: 0.2800000011920929

Feedback:
    distance: 0.27000001072883606

Feedback:
    distance: 0.25999999046325684

Feedback:
    distance: 0.25

Feedback:
    distance: 0.23999999463558197

Feedback:
    distance: 0.23000000417232513

Feedback:
    distance: 0.2199999988079071

Feedback:
    distance: 0.20999999344348907

Feedback:
    distance: 0.20000000298023224

Feedback:
    distance: 0.1899999976158142

Feedback:
    distance: 0.18000000715255737

Feedback:
    distance: 0.17000000178813934

Feedback:
    distance: 0.1599999964237213

Feedback:
    distance: 0.15000000596046448

Feedback:
    distance: 0.14000000059604645

Feedback:
    distance: 0.12999999523162842

Feedback:
    distance: 0.11999999731779099

Feedback:
    distance: 0.10999999940395355

Feedback:
    distance: 0.10000000149011612

Feedback:
    distance: 0.09000000357627869

Feedback:
    distance: 0.07999999821186066

Feedback:
    distance: 0.07000000029802322

Feedback:
    distance: 0.05999999865889549

Feedback:
    distance: 0.05000000074505806

Feedback:
    distance: 0.03999999910593033

Feedback:
    distance: 0.029999999329447746

Feedback:
    distance: 0.019999999552965164

Feedback:
    distance: 0.009999999776482582

Result:
    final_position:
  x: 0.9900000000000007
  y: 0.0
  z: 0.0

Goal finished with status: SUCCEEDED

You can try send_goal with various positions. As long as the action server node is not closed, it will keep the state. As soon as the action server is closed, it will lose any internal state.

If the action server is unable to reach the desired goal within the time allowed in the action server, the goal will finish with status ABORTED. For instance, the last lines for a goal that is too far from the current position would be as follows. Even then, the node will not crash, and will accept further commands that might succeed or fail.

[...]

Feedback:
    distance: 98.0199966430664

Result:
    final_position:
  x: 1.9900000000000015
  y: 0.0
  z: 0.0

Goal finished with status: ABORTED