ROS2专栏（三） | 理解ROS2的动作

1. 创建一个动作

目标：在ROS 2软件包中定义一个动作。

1.1 新建包

设置一个 workspace 并创建一个名为 action_tutorials_interfaces 的包：

mkdir -p ros2_ws/src #you can reuse existing workspace with this naming convention
cd ros2_ws/src
ros2 pkg create action_tutorials_interfaces

1.2 任务

定义一个动作

动作在 .action 文件中定义，格式如下：

# Request
---
# Result
---
# Feedback

一个动作定义由三个消息定义组成，用 — 分隔。

一个请求消息是从动作客户端发送到动作服务器，用于启动一个新的目标。

一个结果消息是从动作服务器发送到动作客户端，表示一个目标已完成。

Feedback messages are periodically sent from an action server to an action client with updates about a goal.

一个动作的实例通常被称为一个目标。
假设我们想要定义一个用于计算斐波那契数列的新动作 “Fibonacci”。
在我们的ROS 2包action_tutorials_interfaces中创建一个名为action的目录：

cd action_tutorials_interfaces
mkdir action

在action目录中创建一个名为Fibonacci.action的文件，并包含以下内容：

int32 order
---
int32[] sequence
---
int32[] partial_sequence

目标请求是我们想要计算的斐波那契序列的order，结果是最终的sequence，反馈是到目前为止计算出的partial_sequence。

构建一个action

在我们的代码中使用新的Fibonacci action类型之前，我们必须将定义传递给rosidl代码生成流程。

这可以通过在action_tutorials_interfaces的CMakeLists.txt文件中在ament_package()之前添加以下行来实现：

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Fibonacci.action"
)

理解：在ROS 2 中，为了能够使用 rosidl_generate_interfaces 来自动生成消息、服务和动作的源代码，需要在 CMakeLists.txt 文件中添加 find_package(rosidl_default_generators REQUIRED) 这一行来告诉 CMake在构建过程中要找到并使用 rosidl_default_generators 工具。

rosidl_default_generators 是一个用于生成 ROS 2 消息、服务和动作的工具包。通过在CMakeLists.txt 文件中添加 find_package(rosidl_default_generators REQUIRED) 这一行，你可以确保在构建时正确地找到并使用该工具包来生成动作所需的源代码。

然后，通过 rosidl_generate_interfaces() 函数来指定需要生成的消息、服务和动作文件。在这个例子中，你指定了Fibonacci.action 动作文件，以便生成与该动作相关的源代码。

我们还需要将所需的依赖项添加到我们的 package.xml 文件中：

<buildtool_depend>rosidl_default_generators</buildtool_depend>

<depend>action_msgs</depend>

<member_of_group>rosidl_interface_packages</member_of_group>

注意，我们需要依赖于 action_msgs，因为动作定义包括附加元数据（例如目标 ID）。

现在，我们应该能够构建包含 Fibonacci 动作定义的软件包：

# Change to the root of the workspace
cd ~/ros2_ws
# Build
colcon build

我们完成了！

按照惯例，动作类型将以其包名称和单词 action 作为前缀。因此，当我们想引用我们的新动作时，它将具有完整的名称 action_tutorials_interfaces/action/Fibonacci。

我们可以使用命令行工具检查我们的动作是否成功构建：

# Source our workspace
# On Windows: call install/setup.bat
. install/setup.bash
# Check that our action definition exists
ros2 interface show action_tutorials_interfaces/action/Fibonacci

在这里插入图片描述

2. 编写一个动作服务器和客户端（Python）

编写一个动作服务器和客户端（C++）

目标：在Python中实现一个动作服务器和客户端。

2.1 背景

在ROS 2中，动作是一种异步通信形式。动作客户端向动作服务器发送目标请求。动作服务器向动作客户端发送目标反馈和结果。

你需要使用action_tutorials_interfaces包和前面教程中定义的Fibonacci.action接口

2.2 任务

编写动作服务器

在您的主目录ros2_ws/src中新建一个文件，我们将其命名为fibonacci_action_server.py，然后添加以下代码：

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__('fibonacci_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')
        result = Fibonacci.Result()
        return result


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

    fibonacci_action_server = FibonacciActionServer()

    rclpy.spin(fibonacci_action_server)


if __name__ == '__main__':
    main()

第8行定义了一个名为FibonacciActionServer的类，它是Node的子类。通过调用Node构造函数来初始化该类，并将节点命名为fibonacci_action_server：

        super().__init__('fibonacci_action_server')

在构造函数中，我们还实例化了一个新的动作服务器：

        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback)

一个操作服务器需要四个参数：

要添加操作客户端的ROS 2节点对象：self。
动作消息类型：Fibonacci（在第5行导入）。
动作服务器的名称：fibonacci。
用于执行已接受的目标的回调函数：self.execute_callback。该回调函数必须返回该操作类型的结果消息。

该方法是一个回调函数，它会在接收到动作目标时被调用。在这个示例中，它只是打印一条日志，并创建一个空的 Fibonacci 动作结果，然后返回。

我们还在类中定义了一个execute_callback方法：

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')
        result = Fibonacci.Result()
        return result

这是一旦接受目标就会被调用来执行目标的方法。

让我们尝试运行我们的动作服务器：

python3 fibonacci_action_server.py

在另一个终端中，我们可以使用命令行界面发送一个目标：

ros2 action send_goal fibonacci action_tutorials_interfaces/action/Fibonacci "{order: 5}"

这个命令的意思是向名为 fibonacci 的动作服务器发送一个动作目标，该动作目标包含一个名为 order 的参数，其值为 5。

具体来说，命令的各部分含义如下：

ros2 action send_goal：这是一个 ROS 2 命令，用于向动作服务器发送动作目标。
fibonacci：动作服务器的名称，这里是指向名为 fibonacci 的动作服务器发送动作目标。
action_tutorials_interfaces/action/Fibonacci：动作的消息类型，指定了动作的类型为
Fibonacci。 “{order: 5}”：动作目标的参数，其中 order 是动作的参数名称，5 是 order 参数的值。

在正在运行动作服务器的终端中，您应该看到一个记录的消息“正在执行目标…”，然后是一个警告，说明目标状态未设置。默认情况下，如果在执行回调中未设置目标处理状态，则假定为“中止”状态。

我们可以使用方法` succeed() 来表示目标已成功：

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')

        goal_handle.succeed()

        result = Fibonacci.Result()
        return result

现在，如果重新启动动作服务器并发送另一个目标，您应该看到目标以SUCCEEDED状态完成。

现在让我们实际计算并返回请求的斐波那契数列：

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')


        sequence = [0, 1]



        for i in range(1, goal_handle.request.order):

            sequence.append(sequence[i] + sequence[i-1])


        goal_handle.succeed()

        result = Fibonacci.Result()

        result.sequence = sequence

        return result

计算序列后，我们在返回之前将其分配给结果消息字段。

然后重新启动动作服务器并发送另一个目标。您应该看到目标以正确的结果序列完成。

发布反馈

动作的一个好处是在目标执行期间向动作客户端提供反馈。我们可以通过调用目标处理器的 publish_feedback() 方法，使我们的动作服务器为动作客户端发布反馈。

我们将替换 sequence 变量，并使用反馈消息来存储序列。在 for 循环中每次更新反馈消息后，我们都会发布反馈消息并休眠以产生戏剧效果：

import time


import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__('fibonacci_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback)

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')


        feedback_msg = Fibonacci.Feedback()		# 创建一个名为 feedback_msg 的 Fibonacci.Feedback 对象

        feedback_msg.partial_sequence = [0, 1]		# 初始化 partial_sequence 数组为 [0, 1]


        for i in range(1, goal_handle.request.order):

            feedback_msg.partial_sequence.append(

                feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i-1])

            self.get_logger().info('Feedback: {0}'.format(feedback_msg.partial_sequence))

            goal_handle.publish_feedback(feedback_msg)

            time.sleep(1)

		# 在这个循环中，计算 Fibonacci 数列的下一个值，并将它添加到 partial_sequence 数组中。然后，将更新后的 	
		# partial_sequence 发布为反馈，以便客户端知道当前的计算进度，并且在每次循环之后，程序会暂停 1 秒钟，以模拟计算的过程

        goal_handle.succeed()		# 调用 goal_handle.succeed() 来通知客户端目标已经成功执行

        result = Fibonacci.Result()		# 创建一个名为 result 的 Fibonacci.Result 对象

        result.sequence = feedback_msg.partial_sequence		# 将最终的 Fibonacci 数列赋值给 result.sequence

        return result


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

    fibonacci_action_server = FibonacciActionServer()

    rclpy.spin(fibonacci_action_server)


if __name__ == '__main__':
    main()

编写一个动作客户端

我们还将将动作客户端限定在单个文件范围内。打开一个新文件，我们称之为fibonacci_action_client.py，然后添加以下样板代码：

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionClient(Node):

    def __init__(self):
        super().__init__('fibonacci_action_client')
        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

    def send_goal(self, order):		# send_goal 方法用于向服务器发送目标。它接受一个参数 order，表示要计算 Fibonacci 数列的长度
        goal_msg = Fibonacci.Goal()		# 首先，创建一个 Fibonacci.Goal 对象，并将 order 赋值给 goal_msg.order
        goal_msg.order = order

        self._action_client.wait_for_server()	# 调用 _action_client.wait_for_server() 方法等待服务器准备就绪

        return self._action_client.send_goal_async(goal_msg)		# 异步地发送目标


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

    action_client = FibonacciActionClient()

    future = action_client.send_goal(10)

    rclpy.spin_until_future_complete(action_client, future)


if __name__ == '__main__':
    main()

我们定义了一个名为FibonacciActionClient的类，它是Node的子类。通过调用Node构造函数来初始化该类，将我们的节点命名为fibonacci_action_client：

        super().__init__('fibonacci_action_client')

在类的构造函数中，我们还使用之前教程中的自定义动作定义来创建一个动作客户端：

        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

我们通过传递三个参数来创建一个ActionClient：

要添加动作客户端的 ROS 2 节点：self

动作的类型：Fibonacci

动作的名称：fibonacci

我们的动作客户端将能够与具有相同名称和类型的动作服务器进行通信。

我们还在FibonacciActionClient类中定义了一个方法send_goal：

    def send_goal(self, order):
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order

        self._action_client.wait_for_server()

        return self._action_client.send_goal_async(goal_msg)

该方法等待动作服务器可用，然后向服务器发送一个目标。它返回一个未来对象，我们可以稍后等待该对象。

在类定义之后，我们定义了一个函数main()，它初始化ROS 2并创建FibonacciActionClient节点的一个实例。然后它发送一个目标并等待该目标完成。

最后，在我们的Python程序入口点调用main()。

让我们通过首先运行之前构建的动作服务器来测试我们的动作客户端：

python3 fibonacci_action_server.py

在另一个终端中运行动作客户端：

python3 fibonacci_action_client.py

当动作服务器成功执行目标时，您应该会看到由其打印的消息：
在这里插入图片描述

获取结果

那么我们可以发送一个目标，但我们如何知道何时完成呢？我们可以通过以下几个步骤获取结果信息。首先，我们需要为发送的目标获取一个目标句柄。然后，我们可以使用目标句柄来请求结果。

以下是这个示例的完整代码：

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionClient(Node):

    def __init__(self):
        super().__init__('fibonacci_action_client')
        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

    def send_goal(self, order):
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(goal_msg)		# 调用 send_goal_async 方法发送目标给服务器

        self._send_goal_future.add_done_callback(self.goal_response_callback)		# 注册一个回调函数 goal_response_callback，以处理服务器对目标的响应

    def goal_response_callback(self, future):		# 发送目标后的回调函数。当服务器接受目标时，它打印一条日志表示目标被接受，并调用 get_result_async 方法以异步方式获取服务器的结果。
        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)

    def get_result_callback(self, future):		# 获取结果后的回调函数。当收到结果时，它打印结果，并调用 rclpy.shutdown() 关闭 ROS 2 节点。
        result = future.result().result
        self.get_logger().info('Result: {0}'.format(result.sequence))
        rclpy.shutdown()


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

    action_client = FibonacciActionClient()

    action_client.send_goal(10)

    rclpy.spin(action_client)


if __name__ == '__main__':
    main()

ActionClient.send_goal_async() 方法返回一个对目标句柄的 future。首先，我们为 future 完成时注册一个回调函数：

        self._send_goal_future.add_done_callback(self.goal_response_callback)

请注意，当一个动作服务器接受或拒绝目标请求时，future 将会完成。让我们更详细地查看 goal_response_callback。我们可以检查目标是否被拒绝，并在此处提前返回，因为我们知道将不会有结果：

    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 :)')

现在我们已经获得了一个目标句柄，我们可以使用它来使用 get_result_async() 方法请求结果。与发送目标类似，我们将得到一个 future，该 future 将在结果准备好时完成。让我们注册一个与目标响应时类似的回调函数：

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

在回调函数中，我们记录结果序列并优雅地关闭 ROS 2 以完成退出：

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Result: {0}'.format(result.sequence))
        rclpy.shutdown()

在一个独立的终端中运行动作服务器后，可以尝试运行我们的斐波那契动作客户端！

python3 fibonacci_action_client.py

你应该会看到记录的消息，显示目标已被接受和最终结果。

获取反馈

我们的动作客户端可以发送目标。很好！但是如果我们能够从动作服务器获取一些关于发送的目标的反馈信息就更好了。

以下是这个示例的完整代码：

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionClient(Node):

    def __init__(self):
        super().__init__('fibonacci_action_client')
        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

    def send_goal(self, order):
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    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)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info('Result: {0}'.format(result.sequence))
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence))


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

    action_client = FibonacciActionClient()

    action_client.send_goal(10)

    rclpy.spin(action_client)


if __name__ == '__main__':
    main()

这是用于反馈消息的回调函数：

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence))

在回调函数中，我们获取消息的反馈部分并将partial_sequence字段打印到屏幕上。

我们需要在动作客户端中注册回调函数。当我们发送一个目标时，可以通过将回调函数附加到动作客户端来实现：

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

一切准备就绪。如果我们运行我们的动作客户端，你应该会在屏幕上看到打印出的反馈信息。

参考：http://fishros.org/doc/ros2/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html#