ROS2教程 08 动作Action

一、ROS2 Action运行示意图

二、ros2 action

目前国内还没有统一的叫法，action在牛津词典中意义接近的释义是：
1.the process of doing sth in order to make sth happen or to deal with a situation
可以看出action一词的关键释义词是process，也就是过程，是具有延续性的词汇，管中窥豹，Action可以译作行动。

ros1中不存在Action这种通讯方式，操作Action适用于长时的，需要过程反馈的工作。
Action是有时间跨度的任务，在这个时间跨度中有不断的反馈。
Action的特点是可以中途停止操作，比如通过Action控制轮子转动，可以在过程中随时停止轮子转动，即中途中断正在进行的操作Action，不仅可以中断它，也可以通过新的goal来覆盖它，在未完成旧goal时开始执行新goal，比如轮子从5m/s
渐渐减速，但还未完全停止时，请求它以1m/s的goal速度转动，它的速度将会趋向1m/s而不是停止。

可以将Action视作Topic与Service构成的新类型，可以简化认为它有两个Service与一个Topic，Goal Service与Result Service，在客户端Client向服务端Server发出Goal请求Request后，得到这条请求的反馈Goal Response，然后客户端Client向服务端Server请求Request结果Result，随后服务端Server持续稳定地发送反馈话题FeedbackTopic，而不是像Service那样只反馈一次，客户端Client订阅这个话题，从而了解操作Action的进展情况，当处理结束后，服务端向客户端反馈执行结果Result Response

总之，行动是可以长时间运行的任务，提供定期的反馈Feedback，并可以随时取消，在任务完成后，会有执行结果的返回Result。
比如在机器人导航时，向机器人发送目标位置，机器人在前往目标位置的途中定时发布反馈话题Feedback Topic，在到达最终目的地后发送结果Result，在机器人运动的过程中，可以随时中止或覆盖goal request。

① ros2 action list

带上-t将会同时输出Action的结构类型Type

ros2 action list -t

OUTPUT：
/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]

查看这个结构类型的具体内容

ros2 interface show turtlesim/action/RotateAbsolute

可以看到数据分成三个部分，每个部分之间用三短横线分割开了，从上往下分别是目标请求部分goal request，结果部分result，反馈部分feedback

OUTPUT：
# 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

② ros2 action info

查看action的具体信息

ros2 action info /turtle1/rotate_absolute

可以看到与这个Action连接的客户端与服务端，之所以用“连接”一词，是客户端或服务端通过Action连接其他部分的关系都不是简单的订阅或者请求与反馈，它是一种复合的关系

OUTPUT:
Action: /turtle1/rotate_absolute
Action clients: 1
    /teleop_turtle
Action servers: 1
    /turtlesim

③ ros2 action send_goal

在命令行中发送Action goal

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

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

每个Goal都有独立的id，下方的result为返回的执行结果

OUTPUT:
Waiting for an action server to become available...
Sending goal:
     theta: 1.57

Goal accepted with ID: 0453c7b6af94418a94f1b1fd6a26b195

Result:
    delta: -1.5679999589920044

Goal finished with status: SUCCEEDED

当需要显示过程的feedback时，加入–feedback尾缀

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

此时则会显示一系列的过程feedback

OUTPUT:
Sending goal:
   theta: -1.57

Goal accepted with ID: e6092c831f994afda92f0086f220da27

Feedback:
  remaining: -3.1268222332000732

Feedback:
  remaining: -3.1108222007751465

…

Result:
  delta: 3.1200008392333984

Goal finished with status: SUCCEEDED

三、行动Action

在之前的章节已经对行动做了说明
行动的信息接口形式和msg和srv类似，但它分为三部分：
1.动作客户端Client发出的目标请求Goal Request
2.动作服务端Server完成请求后的执行结果Result
3.定期从动作服务端Server发送给动作客户端Client的关于目标执行情况的反馈Feedback

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

四、行动信息接口功能包

1.建立动作接口的功能包

ros2 pkg create action_tutorials_interfaces

2.建立动作的信息接口文件

存放在接口功能包目录，名为action的文件夹下

gedit Fibonacci.action

以斐波那契为例，order为指定目标，sequence为执行结果，partial_sequence是计算过程中的反馈信息

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

3.修改CMakeLists.txt

find_package(rosidl_default_generators REQUIRED)

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

4.修改package.xml

<buildtool_depend>rosidl_default_generators</buildtool_depend>

<depend>action_msgs</depend>

<member_of_group>rosidl_interface_packages</member_of_group>

此处有依赖为action_msgs，因为动作的定义需要包含额外的元数据，也就是每次goal的ID

五、行动Action案例

1.建立新功能包py_action

ros2 pkg create --build-type ament_python py_action --dependencies rclpy

2.建立动作服务端ActionServer

在包中源码位置新建fibonacci_action_server.py

import time # 时间库
import rclpy
from rclpy.action import ActionServer # action所需要的
from rclpy.node import Node

from action_tutorials_interfaces.action import Fibonacci


class FibonacciActionServer(Node):

    def __init__(self):
        super().__init__('fibonacci_action_server')
        # 创建一个ActionServer self，消息类型接口名称，动作名，回调函数
        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.partial_sequence = [0, 1]
        sequence = [0, 1]
        for i in range(1, goal_handle.request.order): # 从1到目标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) # publish_feedback用于向客户端发布feedback话题
            time.sleep(1) # 休眠
        goal_handle.succeed() # 在执行回调的过程中设定目标句柄的状态，否则这个goal的执行状态默认为中止了
        result = Fibonacci.Result()
        result.sequence = feedback_msg.partial_sequence
        return result


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

    fibonacci_action_server = FibonacciActionServer()

    rclpy.spin(fibonacci_action_server)


if __name__ == '__main__':
    main()

3.建立动作客户端ActionClient

在包中源码位置新建

gedit fibonacci_action_client.py

import rclpy
from rclpy.action import ActionClient # 引入ActionServer类
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') # self将客户端本身添加到ROS2 ，行动数据类型，行动名

    def send_goal(self, order):
        goal_msg = Fibonacci.Goal() # 初始化 Goal类
        goal_msg.order = order # 填入goal内容
        self._action_client.wait_for_server() # 等待服务器存在
        self._send_goal_future = self._action_client.send_goal_async(goal_msg) # 异步发送一个goal到服务器，并返回一个future值
        self._send_goal_future.add_done_callback(self.goal_response_callback) # 为future注册回调函数，当future完成时即知道goal的执行结果
        
    def goal_response_callback(self, future): # goal执行完毕response回调函数
        goal_handle = future.result() # 创建句柄
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(') # 若无句柄收到，说明goal被拒绝了
            return # 直接跳出这个回调函数，不执行后续内容
        self.get_logger().info('Goal accepted :)') # 若正常收到时，提示收到了
        
        self._get_result_future = goal_handle.get_result_async() # 该方法将future返回给句柄
        self._get_result_future.add_done_callback(self.get_result_callback) # 当有result时调动回调函数get_result_callback

    def get_result_callback(self, future):
        result = future.result().result # 从future处获取result值
        self.get_logger().info('Result: {0}'.format(result.sequence)) # 打印result值
        rclpy.shutdown() # 关闭ros2 干净退出
        
    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) # rclpy初始化

    action_client = FibonacciActionClient() # 初始化客户端

    action_client.send_goal(10) # 发送goal 内容为10

    rclpy.spin(action_client) # 持续循环，直到future完成


if __name__ == '__main__':
    main()

4. 修改package.xml

  <depend>rclpy</depend>
  <depend>action_tutorials_interfaces</depend>

5.修改setup.py

'action_server = py_action.fibonacci_action_server:main',
'action_client = py_action.fibonacci_action_client:main',