ROS1中的动作程序编写

（注意：本站所有博客均是记录本人学习过程中的理解，可能存在错误和不完善之处，请仔细甄别）

ROS中的动作

上一章我们讲了服务，设想一下真实的机器人应用场景——比如让机器人从场地的左边走到右边。首先，我们只在需要走的时候走过去，用话题不太合适；走过去的流程很长，如果要用服务，如果把服务内容设定为机器人走一步，那么就要反复调用很多次；如果设置为走到底，那么我很长一段时间都不能再控制它了——没有好的中止机制。而且最重要的是，我们没有好的反馈机制来掌握机器人的位置信息，或者说任务进度如何。为了解决这种需要长时间执行、需要过程反馈、并且可能需要被中途取消的复杂任务，我们引入了ROS三大通信方式的最后一块拼图——动作（Action）。

动作，服务，订阅的特点和适用场景如下表：

特性	话题 (Topic)	服务 (Service)	动作 (Action)
通信模式	异步广播 (发布后不管)	同步请求/响应 (必须等待回答)	异步长时间任务
方向性	单向数据流	双向 (请求+响应)	多向 (目标, 反馈, 结果)
关系	多对多	一对一 (一次调用)	一对一 (一个目标)
适用场景	连续的数据流 (传感器)	快速、一次性的任务查询/执行	长时间、可抢占、带反馈的任务
比喻	电台广播	打电话问问题	点一份外卖

动作和服务一样，也有两个主要角色：服务端（server）和客户端（client）。调用方式也类似，只是程序的书写形式不同。为了更好的讲解，我们还是引入一个具体的任务：我们需要启动一个节点，它可以让我们的小海龟原地转3圈，并且每转一圈就要报告一下一共转了多少圈。为此我们需要设计动作服务端和动作客户端，并通过具体的代码来讲解一般书写方法。

ROS动作程序编写

类似的，在动作程序编写之前，我们需要先定义服务端的接收数据类型、响应数据类型。如下（TurtleSpin.action）所示：

int32 num_rotations
---
int32 rotations_completed
---
int32 rotations_completed

我们发现这里有三组字段。我们一一解析：最上面的表示接受数据类型，需要一个int32类型的数据，命名为num_rotations；中间的表示响应数据类型，也需要一个int32类型的数据，命名为rotations_completed；最下面的表示反馈数据类型，是int32类型，它要的和我们的响应数据类型是一样的，在这个例子中是合理的。因为我们要的是转了几圈，那么结束的标志就是转了3圈，用rotations_completed就足够反映。

将上述文件保存在与src同级的action目录下后，我们来看服务端的代码turtle_spin_server.py：

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

import rospy
import actionlib
import math
from geometry_msgs.msg import Twist
from beginner_tutorials.msg import TurtleSpinAction, TurtleSpinFeedback, TurtleSpinResult

class TurtleSpinActionServer:
    def __init__(self):
        # 创建一个发布者，用于向turtlesim发送速度指令
        self._turtle_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1)
        
        # 创建动作服务端
        self._as = actionlib.SimpleActionServer('turtle_spin', TurtleSpinAction, execute_cb=self.execute_cb, auto_start=False)
        self._as.start()
        rospy.loginfo("海龟旋转动作服务端已准备就绪。")

    def execute_cb(self, goal):
        rospy.loginfo("收到目标: 旋转 %d 圈" % goal.num_rotations)
        
        # 设置旋转速度 (弧度/秒)
        angular_speed = 1.0  # 约6.28秒转一圈
        
        # 准备反馈和结果的消息对象
        feedback = TurtleSpinFeedback()
        result = TurtleSpinResult()
        
        # --- 旋转任务的主循环 ---
        for i in range(goal.num_rotations):
            rospy.loginfo("开始旋转第 %d 圈..." % (i + 1))
            
            # 检查客户端是否中途取消了任务
            if self._as.is_preempt_requested():
                rospy.loginfo("任务被客户端取消。")
                self._as.set_preempted()
                return

            # --- 控制海龟精确旋转一圈 ---
            t0 = rospy.Time.now().to_sec()
            current_angle = 0
            
            # 创建Twist消息
            twist = Twist()
            twist.angular.z = angular_speed

            # 循环发布速度指令，直到转满2π弧度
            while current_angle < 2 * math.pi:
                self._turtle_pub.publish(twist)
                t1 = rospy.Time.now().to_sec()
                current_angle = angular_speed * (t1 - t0)
                rospy.sleep(0.01)
            
            # 转完一圈后，停止海龟
            self._turtle_pub.publish(Twist())

            # --- 发布反馈 ---
            feedback.rotations_completed = i + 1
            self._as.publish_feedback(feedback)
            rospy.loginfo("已完成 %d 圈。" % feedback.rotations_completed)

        # 任务成功完成
        result.rotations_completed = goal.num_rotations
        rospy.loginfo("任务成功完成！总共旋转了 %d 圈。" % result.rotations_completed)
        self._as.set_succeeded(result)

if __name__ == '__main__':
    rospy.init_node('turtle_spin_server_node')
    server = TurtleSpinActionServer()
    rospy.spin()

• 导入：我们写动作节点，需要有import actionlib这一步。像服务一样，我们也需要从.action文件生成的类：from beginner_tutorials.msg import TurtleSpinAction, TurtleSpinFeedback, TurtleSpinResult。它们的产生规则分别是文件名+Action，文件名+Feedback，文件名+Result。其含义即英文释义。

• 初始化：关键是actionlib.SimpleActionServer()，这个函数用于初始化动作服务端，各个参数的含义为：
  • ‘turtle_spin’: 动作的名称。客户端将通过这个名称来找到服务端。
  • TurtleSpinAction: 动作的类型，由.action文件生成。
  • execute_cb=self.execute_cb: 指定一个核心的回调函数。当服务端接收到新的目标时，这个函数会被自动调用。
  • auto_start=False: 我们选择手动调用_as.start()来启动服务，这是一个好习惯。
• 主循环：进入主循环前，我们需要先打包好准备返回的值feedback和result:feedback = TurtleSpinFeedback() result = TurtleSpinResult()。基于我们的定义，feedback和result都会有一个int32类型的rotations_completed属性，我们用它存储已经转过的圈数，就可以调用publish_feedback(feedback)函数反馈了。result返回则是调用set_succeeded(result)函数，这表示动作成功执行；如果失败，则应该调用self._as.set_aborted()

接着我们看客户端的代码turtle_spin_client.py：

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

import rospy
import actionlib
from beginner_tutorials.msg import TurtleSpinAction, TurtleSpinGoal

# --- 定义回调函数 ---
def done_cb(state, result):
    rospy.loginfo("任务完成，最终状态: %s" % state)
    rospy.loginfo("总共完成了 %d 圈旋转。" % result.rotations_completed)
    rospy.signal_shutdown("任务结束")

def active_cb():
    rospy.loginfo("任务已被服务端激活处理。")

def feedback_cb(feedback):
    rospy.loginfo("进度更新：已完成 %d 圈。" % feedback.rotations_completed)


def turtle_spin_client():
    # 创建动作客户端
    client = actionlib.SimpleActionClient('turtle_spin', TurtleSpinAction)

    # 等待服务端
    rospy.loginfo("正在等待海龟旋转动作服务端...")
    client.wait_for_server()
    rospy.loginfo("服务端已连接。")

    # 创建并发送目标
    goal = TurtleSpinGoal(num_rotations=3) # 让海龟转3圈
    client.send_goal(goal,
                     done_cb=done_cb,
                     active_cb=active_cb,
                     feedback_cb=feedback_cb)

    rospy.loginfo("已发送旋转3圈的目标。")

if __name__ == '__main__':
    rospy.init_node('turtle_spin_client_node')
    turtle_spin_client()
    rospy.spin()

• 导入：from beginner_tutorials.msg import TurtleSpinAction, TurtleSpinGoal，TurtleSpinGoal这个类是用来包装传递给动作服务端和需求信息的，由我们的定义会具有num_rotations这一int32类型的属性，在这里指小海龟要转的圈数
• 回调函数：我们需要编写以下三个回调函数：
  • done_cb(state, result)：接收到任务完成的返回值时被调用，state参数会告诉我们任务的最终状态（如 SUCCEEDED, PREEMPTED, ABORTED）。
  • active_cb()：需求信息从客户端传出时调用，一般只在这里放一些日志信息
  • feedback_cb(feedback)：接收到反馈信息时调用，如果需要调整目标可以在这里部署代码
• 主函数：使用actionlib.SimpleActionClient()函数初始化动作客户端，类似于服务，使用wait_for_server()函数等待服务端就绪。比较重要的是我们向服务端发送信息的格式，使用send_goal()函数，该函数的参数及说明如下：
  • goal: 一个填充好数据的…Goal对象，即我们的任务指令。
  • done_cb: 挂载任务完成时的回调函数。
  • active_cb: 挂载任务激活时的回调函数。
  • feedback_cb: 挂载收到反馈时的回调函数。

客户端和服务端都需要使用rospy.spin()函数使节点保持运行状态。

编译和运行

我们已经编写好了所有的源代码，但要让ROS系统认识并运行它们，还需要完成最后也是最关键的三个步骤：配置、编译和运行。

第一步：修改编译配置文件

你需要告诉ROS的构建系统（Catkin），我们新增了一个.action文件以及它的依赖项。这个步骤只需要在你添加了新的.action文件后做一次。

1. 修改 package.xml 文件

打开功能包（beginner_tutorials）下的package.xml文件，确保文件中包含以下四行依赖。它们告诉ROS，我们的包在编译时(build)和运行时(exec)都需要actionlib（动作库）和actionlib_msgs（动作消息）。

<!-- 在你的 <buildtool_depend> 下面添加 -->
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>

2. 修改 CMakeLists.txt 文件

这是最容易出错的地方，需要修改三个地方：

• 第一处： 在 find_package 函数的 COMPONENTS 列表中，加入 actionlib 和 actionlib_msgs。

  find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    std_msgs
    actionlib          # 添加这一行
    actionlib_msgs     # 添加这一行
  )
  

• 第二处： 找到 add_action_files 函数（如果被注释了，请取消注释），并加入你的 .action 文件名。

  add_action_files(
    FILES
    TurtleSpin.action  # 添加你的action文件名
  )
  

• 第三处： 找到 generate_messages 函数（如果被注释了，也请取消注释），并添加actionlib_msgs依赖。

  generate_messages(
    DEPENDENCIES
    actionlib_msgs   # 添加这一行
    std_msgs
  )
  

第二步：编译工作区

现在，配置文件已经修改完毕，我们可以命令Catkin来编译整个工作区了。

1. 回到工作区根目录

cd ~/catkin_ws

2. 运行 catkin_make

catkin_make

第三步：启动与运行

现在万事俱备，我们可以启动所有节点了。这个项目需要 4 个独立的终端。请一个个地打开它们，并按顺序执行命令。

---

▶️ 终端 1：启动 ROS Master

这是ROS的大脑，必须第一个启动。

roscore

让这个终端一直开着。

---

▶️ 终端 2：启动小海龟仿真器

这是我们的“演员”。

# 不需要 source，因为 turtlesim 是ROS自带的
rosrun turtlesim turtlesim_node

你会看到一个带有小海龟的蓝色窗口弹出。

---

▶️ 终端 3：启动我们的动作服务端

这是控制海龟旋转的“执行者”。

# 黄金法则第一步：Source环境文件！
# 告诉这个终端在哪能找到我们刚刚编译好的代码。
source ~/catkin_ws/devel/setup.bash

# 黄金法则第二步：运行节点。
rosrun beginner_tutorials turtle_spin_server.py

你会看到日志显示 “海龟旋转动作服务端已准备就绪。”。它现在正在等待指令。

---

▶️ 终端 4：启动我们的动作客户端

这是下达命令的“指挥官”。

# 同样，每个新终端都必须先 Source！
source ~/catkin_ws/devel/setup.bash

# 运行客户端节点
rosrun beginner_tutorials turtle_spin_client.py

完成以上步骤后，我们就可以在屏幕上看到我们的小海龟按照我们布置的任务旋转了。我也录制了一个效果图：

一些补充

Action的本质其实是话题操作的封装。actionlib这个库并没有创建一种全新的底层通信方式，而是通过巧妙地组合使用多个话题（用于目标、取消、状态、反馈、结果），并在此之上构建了一个强大的状态机，最终为我们提供了这个易于使用的、专门用于长时间任务的接口。

动作程序的模板如下：

• 服务端模板：

  #!/usr/bin/env python3
  import rospy
  import actionlib
  from your_package.msg import YourAction, YourFeedback, YourResult
    
  class YourActionServer:
      def __init__(self, name):
          self._as = actionlib.SimpleActionServer(name, YourAction, execute_cb=self.execute_cb, auto_start=False)
          self._as.start()
    
      def execute_cb(self, goal):
          # ... 你的耗时任务逻辑在这里 ...
            
          # 检查是否被取消
          if self._as.is_preempt_requested():
              self._as.set_preempted()
              return
    
          # 发布反馈
          feedback = YourFeedback(...)
          self._as.publish_feedback(feedback)
            
          # 设置最终结果
          result = YourResult(...)
          self._as.set_succeeded(result)
    
  if __name__ == '__main__':
      rospy.init_node('your_action_server_node')
      server = YourActionServer(rospy.get_name())
      rospy.spin()
  

• 客户端模板:

  #!/usr/bin/env python3
  import rospy
  import actionlib
  from your_package.msg import YourAction, YourGoal
    
  def done_cb(state, result):
      # ...
  def active_cb():
      # ...
  def feedback_cb(feedback):
      # ...
    
  def your_client():
      client = actionlib.SimpleActionClient('your_action_server_node', YourAction)
      client.wait_for_server()
        
      goal = YourGoal(...)
      client.send_goal(goal, done_cb=done_cb, active_cb=active_cb, feedback_cb=feedback_cb)
    
  if __name__ == '__main__':
      rospy.init_node('your_action_client_node')
      your_client()
      rospy.spin()