机器人系统开发ros2-基础学习11-机器人aciton动作功能的定义及开发示例_ros2动作

机器人是一个复杂的智能系统，并不仅仅是键盘遥控运动、识别某个目标这么简单，我们需要实现的是送餐、送货、分拣等满足具体场景需求的机器人。

在这些应用功能的实现中，另外一种ROS通信机制也会被常常用到——那就是动作。从这个名字上就可以很好理解这个概念的含义，这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。

以下是一些使用ROS 2 action的常用应用场景：

• 导航：机器人可以使用action来发送导航指令，并实时接收路径规划的反馈。

• 物流自动驾驶：无人驾驶汽车或物流车可以使用action来接收路径指令并执行。

• 人机交互（任务处理）：机器人可以通过action来接收用户的指令和反馈。

• 复杂运动规划：机器人可以使用action来发送运动指令，并在执行过程中接收运动的反馈。

通信模型

举个例子，比如我们想让机器人转个圈，这肯定不是一下就可以完成的，机器人得一点一点旋转，直到360度才能结束，假设机器人并不在我们眼前，发出指令后，我们根本不知道机器人到底有没有开始转圈，转到哪里了？

OK，现在我们需要的是一个反馈，比如每隔1s，告诉我们当前转到多少度了，10度、20度、30度，一段时间之后，到了360度，再发送一个信息，表示动作执行完成。

这样一个需要执行一段时间的行为，使用动作的通信机制就更为合适，就像装了一个进度条，我们可以随时把控进度，如果运动过程当中，我们还可以随时发送一个取消运动的命令。

客户端/服务器模型

动作和服务类似，使用的也是客户端和服务器模型，客户端发送动作的目标，想让机器人干什么，服务器端执行动作过程， 控制机器人达到运动的目标，同时周期反馈动作执行过程中的状态。

客户端发送一个运动的目标，想让机器人动起来，服务器端收到之后，就开始控制机器人运动，一边运动，一边反馈当前的状态，如果是一个导航动作，这个反馈可能是当前所处的坐标，如果是机械臂抓取，这个反馈可能又是机械臂的实时姿态。当运动执行结束后，服务器再反馈一个动作结束的信息。整个通信过程就此结束。

一对多通信

和服务一样，动作通信中的客户端可以有多个，大家都可以发送运动命令，但是服务器端只能有一个，毕竟只有一个机器人，先执行完成一个动作，才能执行下一个动作。

同步通信

既然有反馈，那动作也是一种同步通信机制，之前我们也介绍过，动作过程中的数据通信接口，使用.action文件进行定义。

由服务和话题合成

大家再仔细看下上边的动图，是不是还会发现一个隐藏的秘密。

动作的三个通信模块，竟然有两个是服务，一个是话题，当客户端发送运动目标时，使用的是服务的请求调用，服务器端也会反馈一个应带，表示收到命令。动作的反馈过程，其实就是一个话题的周期发布，服务器端是发布者，客户端是订阅者。

没错，动作是一种应用层的通信机制，其底层就是基于话题和服务来实现的。

示例：小海龟的动作

ros2 run turtlesim turtlesim_node
1

重新开命令框启动键控节点

ros2 run turtlesim turtle_teleop_key
1

启动turtle_teleop_key 节点后我们会看到如下这些信息，

Use arrow keys to move the turtle. Use G|B|V|C|D|E|R|T keys to rotate
to absolute orientations. ‘F’ to cancel a rotation.
让我们关注第二行，它对应一个动作。 第一条指令对应于“cmd_vel”主题

请注意，字母键在美国 QWERTY 键盘上的按键G|B|V|C|D|E|R|T周围形成一个“框”

注意/turtlesim节点运行的终端。每次按下这些键之一，您都会将目标发送到作为节点一部分的操作服务器/turtlesim。目标是旋转海龟以面向特定方向。一旦海龟完成旋转，就会显示一条传达目标结果的消息：

[INFO] [turtlesim]: Rotation goal completed successfully

该F键将取消执行中的目标。

您可以/turtle1/rotate_absolute使用以下命令进一步检查操作：

ros2 action info /turtle1/rotate_absolute
1

这告诉我们之前在每个节点上运行时学到的东西：节点有一个操作客户端，节点有一个用于操作的操作服务器。ros2 node info/teleop_turtle/turtlesim/turtle1/rotate_absolute

发送操作目标

go ros2 action send_goal <action_name> <action_type> <values>

<values>需要采用 YAML 格式。

密切关注turtlesim 窗口，然后在终端中输入以下命令：

ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: -1.57}"
1

您应该看到海龟在旋转，并在终端中看到以下消息：

所有目标都有一个唯一的 ID，显示在返回消息中。您还可以看到结果，一个名为 的字段delta，它是到起始位置的位移。

要查看此目标的反馈，请添加–feedback命令：ros2 action send_goal

ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: -1.57}" --feedback
1

您的终端将收到反馈，直到目标完成。

ros2 显示动作内容

命令如下

ros2 interface show turtlesim/action/RotateAbsolute
1

将返回：

# The desired heading in radians（目标请求的结构）
float32 theta
---
# The angular displacement in radians to the starting position（结果的结构）
float32 delta
---
# The remaining rotation in radians（反馈的结构）
float32 remaining
1
2
3
4
5
6
7
8

该消息第一部分上方—是目标请求的结构（数据类型和名称）。下一部分是结果的结构。最后一部分是反馈的结构。

案例2： 实现一个小车画圆的动作

通信模型

1. 接口aciton 定义

例程使用的动作并不是ROS中的标准定义，我们通过MoveCircle.action进行自定义：

learning_interface/action/MoveCircle.action

bool enable     # 定义动作的目标，表示动作开始的指令
---
bool finish     # 定义动作的结果，表示是否成功执行
---
int32 state     # 定义动作的反馈，表示当前执行到的位置
1
2
3
4
5

修改配置文件，新增 “action/MoveCircle.action”

重新编译learning_interface 功能包

colcon build --packages-select learning_interface
1

新增learning_action 功能包

ros2 pkg create --build-type ament_python learning_action
1

新增服务器server代码

learning_action/action_move_server.py

import time

import rclpy                                      # ROS2 Python接口库
from rclpy.node   import Node                     # ROS2 节点类
from rclpy.action import ActionServer             # ROS2 动作服务器类
from learning_inteface.action import MoveCircle  # 自定义的圆周运动接口


class MoveCircleActionServer(Node):
    def __init__(self, name):
        super().__init__(name)                   # ROS2节点父类初始化
        self._action_server = ActionServer(      # 创建动作服务器（接口类型、动作名、回调函数）
            self,
            MoveCircle,
            'move_circle',
            self.execute_callback)

    def execute_callback(self, goal_handle):            # 执行收到动作目标之后的处理函数
        self.get_logger().info('Moving circle...')
        feedback_msg = MoveCircle.Feedback()            # 创建一个动作反馈信息的消息

        for i in range(0, 360, 30):                     # 从0到360度，执行圆周运动，并周期反馈信息
            feedback_msg.state = i                      # 创建反馈信息，表示当前执行到的角度
            self.get_logger().info('Publishing feedback: %d' % feedback_msg.state)
            goal_handle.publish_feedback(feedback_msg)  # 发布反馈信息
            time.sleep(0.5)

        goal_handle.succeed()                           # 动作执行成功
        result = MoveCircle.Result()                    # 创建结果消息
        result.finish = True                            
        return result                                   # 反馈最终动作执行的结果

def main(args=None):                                    # ROS2节点主入口main函数
    rclpy.init(args=args)                               # ROS2 Python接口初始化
    node = MoveCircleActionServer("action_move_server") # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                    # 循环等待ROS2退出
    node.destroy_node()                                 # 销毁节点对象
    rclpy.shutdown()                                    # 关闭ROS2 Python接口
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

新增客户端代码
learning_action/action_move_client.py

import rclpy                                      # ROS2 Python接口库
from rclpy.node   import Node                     # ROS2 节点类
from rclpy.action import ActionClient             # ROS2 动作客户端类

from learning_inteface.action import MoveCircle  # 自定义的圆周运动接口

class MoveCircleActionClient(Node):
    def __init__(self, name):
        super().__init__(name)                   # ROS2节点父类初始化
        self._action_client = ActionClient(      # 创建动作客户端（接口类型、动作名）
            self, MoveCircle, 'move_circle') 

    def send_goal(self, enable):                 # 创建一个发送动作目标的函数
        goal_msg = MoveCircle.Goal()             # 创建一个动作目标的消息
        goal_msg.enable = enable                 # 设置动作目标为使能，希望机器人开始运动

        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: {%d}' % result.finish)                # 日志输出执行结果

    def feedback_callback(self, feedback_msg):                                # 创建处理周期反馈消息的回调函数
        feedback = feedback_msg.feedback                                      # 读取反馈的数据
        self.get_logger().info('Received feedback: {%d}' % feedback.state) 

def main(args=None):                                       # ROS2节点主入口main函数
    rclpy.init(args=args)                                  # ROS2 Python接口初始化
    node = MoveCircleActionClient("action_move_client")    # 创建ROS2节点对象并进行初始化
    node.send_goal(True)                                   # 发送动作目标
    rclpy.spin(node)                                       # 循环等待ROS2退出
    node.destroy_node()                                    # 销毁节点对象
    rclpy.shutdown()                                       # 关闭ROS2 Python接口
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

设置setup.py

         'action_move_client = learning_action.action_move_client:main',
         'action_move_server = learning_action.action_move_server:main',
1
2

编译

 colcon build --packages-select learning_action
1

设置环境变量

source install/local_setup.bash
1

测试验证：

ros2 action list                  # 查看服务列表
1

启动action 服务节点

ros2 run learning_action action_move_server
1

启动action 客户端节点

ros2 run learning_action action_move_client
1

运行结果如下：

可以看到有一个action list 有一个新增的 /move_circle

同时也可以看到服务端和客户端有一个数据交互反馈的日志输出

查看下/move_circle 的服务列表情况

ros2 action info /move_circle
1

运行结果如下：

动作命令的常用操作如下：

$ ros2 action list                  # 查看动作服务列表
$ ros2 action info <action_name>    # 查看服务数据类型
$ ros2 action send_goal <action_name> <action_type> <action_data>   # 发送服务请求
1
2
3

机器人系统可能会使用动作指令进行导航。行动目标可以告诉机器人前往某个位置。当机器人导航到该位置时，它可以沿途发送更新（即反馈），然后在到达目的地后发送最终结果消息。