ROS2 学习（五）接口，动作

接口

通信双方统一规定好接口。比如图像 img，控制运动的线速度和角速度……

我们也不用了解具体实现，基本就是了解接口会去用就行。

$ ros2 interface list # 展示所有 interfaces
$ ros2 interface show ... # 显示具体一个 interface
$ ros2 package show ... # 查看一个 package 里所有的 interface

添加接口

不是说我们写一个接口文件就算添加好了，我们也要通过读取文件。

在 CMakeList.txt 中可以看到：

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/ObjectPosition.msg"
  "srv/AddTwoInts.srv"
  "srv/GetObjectPosition.srv"
  "action/MoveCircle.action"
 )

指从相应的相对目录文件中找接口。

在文件中使用对应接口时，则需要先引入：

from File_Path import interface_name

这个引入文件的路径位于系统生成的 .py 文件中，编译成功即生成。

后面创建对象的时候就可以使用接口了。

动作

比如想让小乌龟转一圈，我们需要不停发送指令，根据当前状态调整指令，在一定条件时取消指令。使用动作比这样一点点写服务方便得多。

实际使用的时候我们也很少直接用话题和服务，都是用封装好的动作接口。

动作就是可以一边发运动指令，一边收到反馈信息。

实现方法还是基于话题和服务，话题是服务端（比如要控制运动的机器人）周期性反馈当前状态，服务是客户端给服务端发的指令请求。

比如打开小乌龟，打开键盘控制后，我们可以尝试发送如下指令：

$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 3.14}"
$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 0}" --feedback 

在接口中可以看到动作接口的三部分：发起动作的目标，动作完成的结束标志，动作的反馈状态。

添加动作

前面的步骤和添加接口一样，编写对应 .action 文件，在 CMakeList.txt 中添加。

服务端：在程序里注意如何添加 action_server：

import rclpy                                      # ROS2 Python接口库
from rclpy.node   import Node                     # ROS2 节点类
from rclpy.action import ActionServer             # ROS2 动作服务器类
from learning_interface.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接口

反馈函数每30度标记一次发布反馈信息。

客户端：先启动，然后发送动作请求并注册自己的回调函数、收到目标后的回调函数。

from rclpy.action import ActionClient             # ROS2 动作客户端类

from learning_interface.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接口

发送动作请求成功了，服务器端先发一个应答反馈，触发的是 goal_response_callback 中的 ‘Goal accepted ’ ，后续收到反馈信息是输出 (‘Received feedback: {%d}’ .

$ ros2 run learning_action action_move_server 
[INFO] [1692636260.811751032] [action_move_server]: Moving circle...
[INFO] [1692636260.812184742] [action_move_server]: Publishing feedback: 0
[INFO] [1692636261.313871989] [action_move_server]: Publishing feedback: 30
[INFO] [1692636261.817491663] [action_move_server]: Publishing feedback: 60
[INFO] [1692636262.319652946] [action_move_server]: Publishing feedback: 90
[INFO] [1692636262.822605205] [action_move_server]: Publishing feedback: 120
[INFO] [1692636263.325196016] [action_move_server]: Publishing feedback: 150
[INFO] [1692636263.826669745] [action_move_server]: Publishing feedback: 180
[INFO] [1692636264.328210660] [action_move_server]: Publishing feedback: 210
[INFO] [1692636264.829900461] [action_move_server]: Publishing feedback: 240
[INFO] [1692636265.331746904] [action_move_server]: Publishing feedback: 270
[INFO] [1692636265.833479902] [action_move_server]: Publishing feedback: 300
[INFO] [1692636266.335957841] [action_move_server]: Publishing feedback: 330

$ ros2 run learning_action action_move_client 
[INFO] [1692636260.807239104] [action_move_client]: Goal accepted :)
[INFO] [1692636260.815466218] [action_move_client]: Received feedback: {0}
[INFO] [1692636261.315441690] [action_move_client]: Received feedback: {30}
[INFO] [1692636261.820438299] [action_move_client]: Received feedback: {60}
[INFO] [1692636262.320808661] [action_move_client]: Received feedback: {90}
[INFO] [1692636262.827049792] [action_move_client]: Received feedback: {120}
[INFO] [1692636263.326864092] [action_move_client]: Received feedback: {150}
[INFO] [1692636263.827891348] [action_move_client]: Received feedback: {180}
[INFO] [1692636264.332103011] [action_move_client]: Received feedback: {210}
[INFO] [1692636264.831075874] [action_move_client]: Received feedback: {240}
[INFO] [1692636265.332972986] [action_move_client]: Received feedback: {270}
[INFO] [1692636265.834606029] [action_move_client]: Received feedback: {300}
[INFO] [1692636266.337943178] [action_move_client]: Received feedback: {330}
[INFO] [1692636266.848281638] [action_move_client]: Result: {1}