6.6 Calling actions

From Polyscope X 10.7 ROS2 actions can be acessed through handles just like other ROS2 functionality in URScript. A handle is created using the ros_action_client_factory builtin, and each handle is conceptually linked one concurrent goal. If you need to handle concurrent goals, create multiple handles using the builtin. Copies of a handle functions just like the handle itself for all practical purposes.

A ROS2 action consists of server accepting a goal of a user defined type. When the goal is sent to the server, the server can accept or reject it. From acceptance the goal will have one of the following states associated:

  • unknown = 0: The goal is in a unknown state. E.g. the server knows nothing about it.

  • accepted = 1: The goal is accepted, but execution has not yet started.

  • executing = 2: The goal is executing.

  • canceling = 3: The goal was cancelled, and the served is processing the cancellation.

  • succeeded = 4: The goal has succeeded.

  • canceled = 5: The goal was cancelled.

  • aborted = 6: The goal was aborted by the server. What methods are useable on the action client handle, and how they behave, depends on the state of the goal.

Command

A handle can be created using the builtin ros_action_client_factory.

action_handle = ros_action_client_factory(name, type)

Note

Quality of Service is not supported.

Parameters

  1. name: The name of the action. Prepend with / and (optionally namespace + /) for actions not on the same node/namespace.

  2. type: The type of the action. Dynamic libraries corresponding to the type must be present on the robot, e.g liburinterfaces__rosidl_typesupport_introspection_c.so etc. for urinterfaces/FollowTrajectory.

Methods on handles

  1. wait_for_server(): Wait for a server compatible with the action handle to become available. If given a number, it will timeout after that amount of seconds, if not, it will wait indefinitely for the server.

  • found = wait_for_server(1.5) will assign true to found if and only if the action server appears within at most 1.5 seconds.

  1. send_goal(goal): Send a goal to the action server. The goal must be a struct compatible with the goal type defined for that action. Compatibility requires the same named members with compatible types. Incompatibility results in a runtime exception.

  • accepted = send_goal(goal) will assign true to accepted if and only if the goal was accepted by the action server.

  1. wait_for_done(): Wait for the last goal sent from this handle to be either ‘succeeded’, ‘cancelled’ or ‘aborted’. If given a number as an argument, timeout after that many seconds, if not, it will wait for the states indefinitely.

  • done = wait_for_done(2.5) assigns true to done if and only if the last goal sent using this handle entered one of the three mentioned states within 2.5 seconds.

  1. get_result(): Get the result of the action. It will block until the result is available.

  2. cancel(): Cancel the last goal sent using this handle.

  • cancelled = cancel() will return true if and only if that goal was cancelled.

  1. get_status(): Get the status of the last sent goal. If no goal has been sent, the status is assumed to be ‘unknown/0’. All status values are returned as integers from 0 through 6 as per the list of states.

  2. feedback(): Get the feedback from the action as a struct with two members:

    • success - A boolean that is true if and only if the feedback was successfully taken.

    • feedback. The feedback is a struct corresponding to the type defined by the action.

    • If/when the goal is not in the ‘accepted’, ‘executing’ states, feedback will return immediately. If no more feedback is available, the result will be struct(success=False, feedback=...) where feedback is zero initialized. While the goal state is ‘accepted’ or ‘executing’ feedback will block until the next feedback is available.

  3. close(): Close the action handle. All other threads using the handle will have their execution interrupted. For wait_for_server, feedback, wait_for_done, cancel, get_status this will result in the intuitive return value, e.g. feedback will have success = False. Other methods such as get_result() or send_goal() will cause a runtime exception if interrupted.

Example usage

# Create an action client handle
action_handle = ros_action_client_factory(
        name="/testAction", 
        type="urinterfaces/FollowTrajectory"
    )

# Wait for the server. found will be true if the server appears within 5 seconds
found = action_handle.wait_for_server(5)
textmsg(str_cat("Server found ",found))

# A thread for executing feedback and status in the background
global spin = True
thread feedback() :
    while(spin):
        sleep(0.5)
        status = action_handle.get_status()
        textmsg(str_cat("Status: ",status))
        feedback = action_handle.feedback()
        textmsg(str_cat("Feedback: ",feedback))
    end
end
fb = run feedback() # Start the thread 

# Create a goal matching the actions goal type
goal = struct(trajectory=struct(motions=[]))
accepted = action_handle.send_goal(goal)
textmsg(str_cat("Request goal reply: ",accepted))

# sleep(1) # wait for some output before cancelling
# Canceling can be done like this:
# cancelled = action_handle.cancel()
# textmsg(str_cat("Canceled: ",cancelled))

# If you want to wait for succeeded/cancelled/aborted
# was_done = action_handle.wait_for_done(1.5)

# Get the result of the action (will block)
result = action_handle.get_result()
textmsg(str_cat("Result: ",result))

spin = False # Set a bool to stop the thread
join fb # Wait for thread to join

action_handle.close() # Close the action handle

Example python action server

To make an action server to test against, something like the following python program can be used.

import rclpy
import time
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import CancelResponse
from rclpy.action import GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup

from urinterfaces.action import FollowTrajectory


class FollowTrajectoryActionServer(Node):

    def __init__(self):
        super().__init__('test_action_server')
        self._action_server = ActionServer(
            self,
            FollowTrajectory,
            '/testAction',
            self.execute_callback,
            callback_group=ReentrantCallbackGroup(),
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback,
        )

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal with id {0}'.format(goal_handle.goal_id.uuid))

        feedback = FollowTrajectory.Feedback()
        steps = 10
        for i in range(steps):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info("Goal canceled")
                result = FollowTrajectory.Result()
                result.error_code = 1
                result.error_string = "Cancelled"
                return result

            feedback.current_primitive_index = i

            print("Feedback! " + str(feedback))
            goal_handle.publish_feedback(feedback)
            time.sleep(0.1)

        # Create result
        result = FollowTrajectory.Result()
        result.error_code = 0
        result.error_string = "No error"
        goal_handle.succeed()
        return result

    def goal_callback(self, goal_request):
        # Accepts or rejects a client request to begin an action
        self.goal = goal_request
        self.get_logger().info('Received goal request :)')
        self.get_logger().info('Trajectory ' + str(self.goal.trajectory))
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        # Accepts or rejects a client request to cancel an action
        self.get_logger().info('Received cancel request :(')
        self.goal = goal_handle
        return CancelResponse.ACCEPT


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

    test_action_server = FollowTrajectoryActionServer()
    executor = MultiThreadedExecutor()
    executor.add_node(test_action_server)

    try:
        test_action_server.get_logger().info('Beginning client, shut down with CTRL-C')
        executor.spin()
    except KeyboardInterrupt:
        test_action_server.get_logger().info('Keyboard interrupt, shutting down.\n')
    test_action_server.destroy_node()


if __name__ == '__main__':
    main()

Limitations

Quality of service if currently not supported for actions.

Please notice, that the actions client can completely release the resources only when the program stops, or close is called on the object.