ROS2学习笔记，案例实现记录——13.动作action

前言

在学习了ROS 2官方教程之后，我决定以自导自演的方式，给自己出题来实践所学知识，以加深对ROS 2的各个概念和使用方式的理解。我将把我的学习过程记录在博客中，并将代码放在我的GitHub仓库中https://github.com/YuAndWang/ROS2_Case（当然博客中也会贴上代码），后面有新的案例将会放到对应的章节之后，供大家参考和学习。

欢迎━(｀∀´)ノ亻! 大家一起出题，一起探讨！希望我的实践过程对你理解ROS 2有所帮助。如果你有任何问题或建议，欢迎提出！谢谢~~~

13.动作action

13.1案例1：moveforward_action

该案例我想实现在服务器端生成一个随机数，模拟机器人前进的距离，模拟机器人前进，并在客户端告知服务器可以开始动作后，开始行走，定时返回当前走的距离，最后返回执行完成的状态。

创建自定义动作文件

这里需要创建消息，我在mine_interface功能包下建立了action文件夹，在文件夹中创建了一个名为MoveForward.action的消息文件，写入如下内容：

bool enable
---
bool finish
---
int32 current_distance

这个动作文件定义了以下内容：

1. 目标部分（Goal）：由enable字段组成，表示是否启用动作执行。
2. 反馈部分（Feedback）：由current_distance字段组成，表示当前的行进距离。
3. 结果部分（Result）：由finish字段组成，表示动作执行是否完成。

接着需要更新CMakeLists.txt文件，将新的srv文件引用到CMakeLists.txt文件中：

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

建立功能包

打开一个新的终端， cd 到我的工作空间目录。并且应该在 src 目录中创建包，而不是在工作空间的根目录中创建包。因此，cd 到 my_ws/src，并运行包创建命令：

cd my_ws/src
ros2 pkg create --build-type ament_python moveforward_action_py

客户端 moveforward_action_cli.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 王仰旭
@说明: 在服务器端生成一个随机数，模拟机器人前进的距离，
    模拟机器人前进，并在客户端告知服务器可以开始动作后，
    开始行走，定时返回当前走的距离，最后返回执行完成的状态。——客户端
"""

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

class MoveForwardActionClient(Node):
    def __init__(self):
        super().__init__('action_move_client')
        self._action_client = ActionClient(
            node=self,
            action_type=MoveForward,
            action_name='move_forward'
        )
    
    def send_goal(self, state):
        goal_msg = MoveForward.Goal()
        goal_msg.enable = state
        if not goal_msg.enable:                     # 如果状态为false，则不执行
            return
        
        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('Receiver feedback: {%d}' % feedback.current_distance)

def main(args=None):
    rclpy.init(args=args)
    client_node = MoveForwardActionClient()
    client_node.send_goal(True)
    rclpy.spin(client_node)
    client_node.destroy_node()
    rclpy.shutdown()

服务端 moveforward_action_srv.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 王仰旭
@说明: 在服务器端生成一个随机数，模拟机器人前进的距离，
    模拟机器人前进，并在客户端告知服务器可以开始动作后，
    开始行走，定时返回当前走的距离，最后返回执行完成的状态。——服务端
"""

import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from mine_interface.action import MoveForward
import random

class MoveForwardActionServer(Node):
    def __init__(self):
        super().__init__('action_move_server')
        self._action_server = ActionServer(
            node=self,
            action_type=MoveForward,
            action_name='move_forward',
            execute_callback=self.execute_callback)
    
    def execute_callback(self, goal_handle):            # 收到动作目标后开始处理
        self.get_logger().info("Moving Forward...")
        result_msg = MoveForward.Result()
        feedback_msg = MoveForward.Feedback()

        random_goal = random.randint(5, 20)             # 取随机数作为距离目标
        self.get_logger().info('Random_goal is %d' % random_goal)
        current_distance = 0
        
        while rclpy.ok() and current_distance < random_goal:
            current_distance += 1
            feedback_msg.current_distance = current_distance
            self.get_logger().info('Publishing feedback: %d' % feedback_msg.current_distance)
            goal_handle.publish_feedback(feedback_msg)  # 发布反馈消息
            time.sleep(0.5)
        
        goal_handle.succeed()                           # 动作执行成功
        self.get_logger().info('Succeed!')
        
        result_msg.finish = True
        return result_msg

def main(args=None):
    rclpy.init(args=args)
    server_node = MoveForwardActionServer()
    rclpy.spin(server_node)
    server_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

配置package.xml文件

<package format="3">
  <name>moveforward_action_pyname>
  <version>0.0.0version>
  <description>move forward action using rclpydescription>
  <maintainer email="[email protected]">wangmaintainer>
  <license>BSDlicense>

  <test_depend>ament_copyrighttest_depend>
  <test_depend>ament_flake8test_depend>
  <test_depend>ament_pep257test_depend>
  <test_depend>python3-pytesttest_depend>

  <depend>mine_interfacedepend>
  
  <export>
    <build_type>ament_pythonbuild_type>
  export>
package>

配置setup.py文件

记得这里需要在setup.py文件中添加入口点（entry points），加入新文件的入口点。

from setuptools import setup

package_name = 'moveforward_action_py'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='wang',
    maintainer_email='[email protected]',
    description='move forward action using rclpy',
    license='BSD',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'moveforward_action_srv = moveforward_action_py.moveforward_action_srv:main',
            'moveforward_action_cli = moveforward_action_py.moveforward_action_cli:main',
        ],
    },
)

配置setup.cfg文件

[develop]
script_dir=$base/lib/moveforward_action_py
[install]
install_scripts=$base/lib/moveforward_action_py

编译

cd my_ws/
colcon build

运行

打开新终端，刷新环境

source ~/my_ws/install/setup.bash

ros2 run moveforward_action_py moveforward_action_srv
ros2 run moveforward_action_py moveforward_action_cli

效果gif