ROS 学习笔记（五）—— 动作 Action 详解

参考：
ROS探索总结（三十二）——action
actionlib_tutorials/Tutorials
github code

1. 动作 Action 通信机制

前面介绍了 service 通信机制，服务机制常用于同步的请求/响应交互方式，客户端向服务端发出请求，在等待响应期间会强行阻塞程序，因而完全无法获知服务端的处理进度，更不能取消或变更请求。这给我们带来了很大的不便，尤其是需要较长时间才能完成的复杂任务。为解决这个问题，ros 引入了一种基于 ros 消息的高级协议——动作。

action 也是一种类似于 service 的服务端/客户端问答通信机制，不一样的地方是action还带有一个反馈机制，可以周期性的反馈任务的实施进度，而且可以在任务实施过程中，中止运行。
client和server之间通过actionlib定义的“action protocol”进行通讯。这种通讯协议是基于ROS的消息机制实现的，为用户提供了client和server的接口，接口如下图所示：

client向server端发布任务目标以及在必要的时候取消任务，server会向client发布当前的状态、实时的反馈和最终的任务结果。

• goal：任务目标
• cancel：请求取消任务
• status：通知client当前的状态
• feedback：周期反馈任务运行的监控数据
• result：向client发送任务的执行结果，这个topic只会发布一次。

动作机制是异步的，运行服务端和客户端采用无阻塞的编程方式。在机器人应用中，执行一个时长不定，目标引导的新任务是很常见的，如 goto_position，clean_the_house。在任何情况下，当需要执行一个任务时，action 可能都是最佳选择。虽然动作需要写更多的代码，但却比服务更强大，扩展性更好。因此，每当你想使用服务时，都应当考虑一下，是否可以替换为动作。

2. 自定义 action

动作定义文件中包含了动作的目标，结果，和反馈的消息格式。通常放在 package 的 action 文件夹下，文件扩展名为.action。
示例：定义一个定时器动作：Timer.action。这个定时器会进行倒计时，并在倒计时停止时发出结果信号告诉我们总的计数时长，倒计时过程中会定期反馈剩余时间。

# This is an action definition file, which has three parts: the goal, the
# result, and the feedback.

# 1. the goal, to be sent by the client
# The amount of time we want to wait
duration time_to_wait
---
# 2: the result, to be sent by the server upon completion
# How much time we waited
duration time_elapsed
# How many updates we provided along the way
uint32 updates_sent
---
# 3: the feedback, to be sent periodically by the server during execution.
# The amount of time that has elapsed from the start
duration time_elapsed
# The amount of time remaining until we're done
duration time_remaining

在CMakeLists.txt文件中添加如下的编译规则：

find_package(catkin REQUIRED actionlib_msgs)
add_action_files(DIRECTORY action FILES Timer.action)
generate_messages(DEPENDENCIES actionlib_msgs std_msgs)
catkin_package(CATKIN_DEPENDS actionlib_msgs)

还需要在功能包的package.xml中添加：

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend> 

编译完成后会在 devel 文件夹产生一系列的.msg文件:
TimerAction.msg
TimerActionGoal.msg
TimerActionResult.msg
TimerActionFeedback.msg
TimerGoal.msg
TimerResult.msg
TimerFeedback.msg

3. 动作server和client的简单示例

3.1 使用 SimpleActionServer 编写一个简单的动作服务器

动作与话题和服务一样，都使用回调机制，即回调函数会在收到消息时被唤醒和调用。直接使用 actionlib 包的SimpleActionServer类可以简化编程，只需定义收到目标时的回调函数。回调函数会根据目标来操作定时器，并在操作结束后返回一个结果。
服务端实现了服务器执行任务的反馈信息，中断抢占功能。具体实现较为简单，反馈信息通过发布反馈的消息实现，中断抢占通过注册中断毁掉函数实现。
simple_action_server.py

#! /usr/bin/env python
import rospy
import time
import actionlib
from basics.msg import TimerAction, TimerGoal, TimerResult

# 定义回调函数，在服务端收到一个新目标时被执行。
def do_timer(goal): # goal 是一个 TimerGoal 类型的对象，其成员为 Timer.action的 goal 部分的内容(time_to_wait)
    start_time = time.time()
    time.sleep(goal.time_to_wait.to_sec())  # 将 time_to_wait 对象从ros的 duration 类型转换为秒

    result = TimerResult() # 构造结果消息，对应类型为 TimerResult，成员即 Timer.action的 result 部分的内容(time_elapsed,updates_sent)
    result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
    result.updates_sent = 0
    
    server.set_succeeded(result)

rospy.init_node('timer_action_server')  # 节点命名，初始化

# 创建 SimpleActionServer，
# 'timer'为动作服务器的名称，这个名称会成为其所有子话题的命名空间，TimerAction为动作服务器的类型，
# do_timer 为目标的回调函数，False 参数表示关闭动作服务器的自动启动功能。
server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False) 
server.start() # 启动动作服务器
rospy.spin()  # 进入 spin（）循环，等待目标到来。

注：动作服务器的自动启动功能应当始终处于禁用状态。否则会造成竞争问题，从而导致奇怪的错误发生，这是 ros 的 bug。

启动动作服务器：rosrun pkg_name simple_action_server.py

rostopic list  # 查看话题列表
rostopic info /timer/goal  # 查看话题详细信息
rosmsg show TimerActionGoal
rosmsg show TimerGoal 

可以看到 TimerActionGoal 比 TimerGoal 多了许多内容。这些额外信息是服务端和客户端用来追踪动作执行状态的。不过，在目标传入服务器端的回调函数之前，这些信息已经被去除了。

一般来说，使用 actionlib 包的程序库，不需要访问名字中含有 Action 的类型。单纯的 Goal，Result，Feedback 已经够用了。

3.2. 使用 SimpleActionClient 编写一个简单的动作客户端

直接使用 actionlib 包的 SimpleActionClient 类作为客户端。向服务端发送一个目标，并等待结果的到来。

simple_action_client.py

#! /usr/bin/env python
import rospy

import actionlib
from basics.msg import TimerAction, TimerGoal, TimerResult

rospy.init_node('timer_action_client')

client = actionlib.SimpleActionClient('timer', TimerAction)  # 动作服务器的名称和动作的类型，必须与服务端的名称相匹配。
client.wait_for_server()  # 阻塞程序，等待动作服务器启动.

goal = TimerGoal()
goal.time_to_wait = rospy.Duration.from_sec(5.0)
client.send_goal(goal)  # 将 goal 发送给服务端

client.wait_for_result()  # 阻塞，等待服务器结果
print('Time elapsed: %f'%(client.get_result().time_elapsed.to_sec()))

启动动作客户端：rosrun pkg_name simple_action_client.py
等待 5s 会看到print结果信息：Time elapsed: 5.001044

c++程序讲解示例：ROS学习笔记六：action-server/action-client.

4. 复杂的动作 server 和 client 示例

第三节的简单动作示例与服务非常相似。实际上动作与服务的主要区别在于动作的异步特性。下面的示例实现了终止目标，处理打断请求（客户端在前一个动作还在执行的过程中，发送了一个新的目标）和实时反馈等功能。

4.1 action 服务端

fancy_action_server.py

#! /usr/bin/env python
import rospy
import time
import actionlib
from basics.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback  # 由于要提供反馈，增加了 TimerFeedback 消息类型

def do_timer(goal):
    start_time = time.time()
    update_count = 0  # 用于统计总共发布了多少反馈信息

	# 错误检查。对于请求 time_to_wait 大于 60s的情形，显式调用set_aborted()来终止目标执行.这个调用会向 client 发送一个消息，告知其本次目标已经终止。
    if goal.time_to_wait.to_sec() > 60.0:
        result = TimerResult()
        result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
        result.updates_sent = update_count
        server.set_aborted(result, "Timer aborted due to too-long wait")
        return
        
	# 进入循环，并在循环中进行间断的休眠等待。
    # 这样的休眠方式可以在动作的执行过程中进行一些操作，例如检查是否发生打断，提供反馈等。
    while (time.time() - start_time) < goal.time_to_wait.to_sec():
    
        if server.is_preempt_requested():  # 检查是否发生中断
            result = TimerResult()
            result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
            result.updates_sent = update_count
            server.set_preempted(result, "Timer preempted")
            return

        feedback = TimerFeedback()  # 反馈
        feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
        feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed
        server.publish_feedback(feedback)  # 向客户端发生反馈信息
        update_count += 1  # 统计反馈次数
        time.sleep(1.0)  # 其实这种休眠方式并不好，因为这样很容易导致实际休眠时长超出请求时长。

    result = TimerResult()  # 结果
    result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)
    result.updates_sent = update_count
    server.set_succeeded(result, "Timer completed successfully")

rospy.init_node('timer_action_server')
server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False)
server.start()
rospy.spin()

4.2 客户端

fancy_action_client.py

#! /usr/bin/env python
import rospy

import time
import actionlib
from basics.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback

# 回调函数，当收到feedback 消息时会被执行。
def feedback_callback(feedback):
    print('[Feedback] Time elapsed: %f'%(feedback.time_elapsed.to_sec()))
    print('[Feedback] Time remaining: %f'%(feedback.time_remaining.to_sec()))

rospy.init_node('timer_action_client')
client = actionlib.SimpleActionClient('timer', TimerAction)
client.wait_for_server()

goal = TimerGoal()
goal.time_to_wait = rospy.Duration.from_sec(5.0)

# 解除下一行注释，测试服务器主动终止（server-side abort）:
# goal.time_to_wait = rospy.Duration.from_sec(500.0)

# 将回调函数作为feedback_cd关键词的参数，传入 send_geal，完成回调的注册
client.send_goal(goal, feedback_cb=feedback_callback)

# 解除下列两行注释，测试中断（test preemption）:
#time.sleep(3.0)
#client.cancel_goal()  # 取消任务目标

client.wait_for_result()
# 返回结果后，打印信息
print('[Result] State: %d'%(client.get_state()))  # 本次目标执行的结果状态（PREEMPTED=2，SUCCEED=3,ABORTED=4）
print('[Result] Status: %s'%(client.get_goal_status_text()))  # 服务端发送的状态字符串
print('[Result] Time elapsed: %f'%(client.get_result().time_elapsed.to_sec()))
print('[Result] Updates sent: %d'%(client.get_result().updates_sent))