ROS通信机制之动作(Action)服务的实践_ros action

1、动作概述

讲完了 服务 之后，接下来就是通信的第三种机制，动作。在上节我们知道服务的应用场景是需要在有限时间内完成，而对于一些比较复杂的任务，需要耗时比较长，甚至是不确定时间的时候，就需要用到动作这种通信方式了。
在原理上，动作使用 话题 来实现，而且引入了反馈来提供操作的进度信息，也就是说在执行任务的过程中，可以周期性得到反馈，帮助我们了解运行到哪个步骤了，本质上相当于规定了一系列话题(目标，反馈、结果等)的组合使用方法的高层协议。
动作的定义和使用跟服务差不多，稍微复杂一点，但是却非常强大和灵活。

2、动作的定义

创建一个动作，里面的格式分三部分：目标、结果、反馈，同样使用三个短横线进行隔开，跟前面介绍的服务(请求和响应，也就是输入和输出)类似，只不过多了一个反馈消息项，同样的在定义好动作之后进行编译，.action文件也会被打包为一个消息类型。

2.1、.action定义

依然先来看下动作的定义，后缀名是.action，先来到工作区的test包，新建action目录 

cd ~/catkin_ws/src/test
mkdir action
cd action

我们来定义一个定时器Timer.action，进行倒计时，并在倒计时停止时发出信号，在这个过程中将定期告诉我们剩余的时间，计时结束之后，返回总时长。

gedit Timer.action

duration time_to_wait
---
duration time_elapsed
uint32 updates_sent
---
duration time_elapsed
duration time_remaining

目标goal：客户端发送的等待总时长
结果result：服务端发送的等待时长以及更新次数
反馈feedback：服务端周期性反馈，消逝的时长和剩余时长 

加个执行权限：chmod u+x Timer.action

2.2、修改CMakeLists.txt

动作文件定义好了之后，同样需要将编译的相关依赖项给写出来

cd ~/catkin_ws/src/test
gedit CMakeLists.txt

find_package增加actionlib_msgs

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation actionlib_msgs)

告知catkin需要编译的动作文件

add_action_files(
  DIRECTORY action 
  FILES Timer.action
)

generate_messages显式列出actionlib_msgs消息依赖

generate_messages(DEPENDENCIES actionlib_msgs std_msgs)

catkin_package中添加actionlib_msgs依赖

catkin_package(CATKIN_DEPENDS message_runtime actionlib_msgs) 

2.3、修改package.xml

cd ~/catkin_ws/src/test
gedit package.xml

添加actionlib_msgs

<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib_msgs</exec_depend>

2.4、编译

相关准备做好了之后，就对其进行编译，创建该动作实际使用的代码以及类定义：

cd ~/catkin_ws
catkin_make

其中对于编译也可以指定Python版本：

cd ~/catkin_ws
catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python2

cd ~/catkin_ws
catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3

当然如果在某个版本里面没有安装catkin_pkg将会报错：

ImportError: "from catkin_pkg.package import parse_package" failed: No module named 'catkin_pkg'
Make sure that you have installed "catkin_pkg", it is up to date and on the PYTHONPATH.

比如本人在Python3里面没有安装，重新安装如下：

sudo apt install python3-pip
pip3 install --user catkin_pkg
pip3 install --user empy

查看下，在两个版本下面都安装好了catkin_pkg

sudo find / -name "catkin_pkg"
/usr/lib/python2.7/dist-packages/catkin_pkg
/home/yahboom/.local/lib/python3.6/site-packages/catkin_pkg

编译成功之后，我们来查看下生成了哪些消息文件：

ls ~/catkin_ws/devel/share/test/msg
TimerActionFeedback.msg  TimerAction.msg        TimerFeedback.msg  TimerResult.msg
TimerActionGoal.msg      TimerActionResult.msg  TimerGoal.msg

 这些msg文件里面都是这些动作所定义的类型，我们看下其详细情况，截图如下：

如果指定Python版本去编译，将在对应版本下面，将上面对应的.msg消息文件生成对应的类的定义文件：

cd ~/catkin_ws/devel/lib/python2.7/dist-packages/test/msg
cd ~/catkin_ws/devel/lib/python3/dist-packages/test/msg

ls查看下生成了哪些文件(其中_Complex.py是前面话题章节自定义的消息类型)：

_Complex.py   __init__.py   _TimerActionFeedback.py  _TimerAction.py        _TimerFeedback.py  _TimerResult.py
_Complex.pyc  __init__.pyc  _TimerActionGoal.py      _TimerActionResult.py  _TimerGoal.py

从这些生成的目录与文件来看，再次说明了这个动作的本质是话题，而话题本质又是类型的消息流。

3、动作服务器

动作的相关类都已生成好了，接下来就写动作服务器，动作跟话题和服务一样，都使用回调机制，即回调函数会在收到消息时被唤醒和调用。这里我们就直接使用来自actionlib里面的SimpleActionServer类来简化编写过程。
我们只需要定义接收到目标时的回调函数，而回调函数会根据目标来操作定时器，并在操作结束后返回一个结果，除了目标和结果，到后面我们会将反馈再加上，先来看一个基本的动作服务。

cd ~/catkin_ws/src/test/src
gedit action_server1.py

#!/usr/bin/env python
import rospy
import time
import actionlib
from test.msg import TimerGoal,TimerResult,TimerAction
 
def do_timer(goal):
    start_time = time.time()
    time.sleep(goal.time_to_wait.to_sec())
 
    result = TimerResult()
    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')
server = actionlib.SimpleActionServer('timer',TimerAction,do_timer,False)
server.start()
rospy.spin()

添加执行权限：chmod u+x action_server1.py

首先导入相关类以及Timer.action自动生成的类，然后定义回调函数，其中参数goal本质是一个TimerGoal类型的对象，goal中的time_to_wait对象返回的是duration类型，所以需要转成to_sec()秒，反过来需要得到持续时间，这里的time_elapsed是Duration类型，使用from_sec()转换。最后就是将结果result作为参数，调用set_succeeded()，告诉SimpleActionServer已成功执行了目标。
我们来看下SimpleActionServer构造函数：

__init__(self, name, ActionSpec, execute_cb=None, auto_start=True)

name：动作服务器的名称
ActionSpec：动作类型
execute_cb：回调函数在单独的线程中调用，接收到一个新的目标，允许用户阻塞回调，添加一个执行回调也会使目标回调失效。
auto_start：告诉ActionServer是否在它出现时立即开始发布，这里应该总是设置为False以出现竞争条件的问题，而导致一些错误的发生，然后显式调用start()来启动动作服务器。那么为什么没有默认为Fasle，这里可能是因为被发现问题时，已经有很多代码都依赖这个默认行为，导致修改变得非常麻烦了。

4、启动检查

动作服务器编写完成之后，我们来检查下是否正常工作。

首先启动：roscore
运行动作服务器：rosrun test action_server1.py
查看话题列表：rostopic list

/rosout
/rosout_agg
/timer/cancel
/timer/feedback
/timer/goal
/timer/result
/timer/status

正确显示出了timer名字空间下的话题，一共有5个，我们来查看一个话题

rostopic info /timer/goal
'''
Type: test/TimerActionGoal
Publishers: None
Subscribers: 
 * /timer_action_server (http://YAB:42723/)
'''

类型是TimerActionGoal，我们来查看下其信息

rosmsg show TimerActionGoal
'''
[test/TimerActionGoal]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
test/TimerGoal goal
  duration time_to_wait
'''

在这里也验证了前面说的goal是TimerGoal的类型，里面包含了time_to_wait字段，其余部分是服务器和客户端用来追踪动作执行状态的，只不过在目标传入服务器端的回调函数之前，这些信息都将被去除，最后剩下的就是这个TimerGoal消息。我们查看下TimerGoal：

rosmsg show TimerGoal
'''
[test/TimerGoal]:
duration time_to_wait
'''

从这些打印的结果来看，如果你使用的是actionlib包中的一些程序库，就不需要去访问这些名字里面含有Action的类型，单纯的Goal、Result、Feedback就完全够用了。

5、动作客户端

服务端我们定义好了，接下来我们定义客户端，向服务器发送一个目标，并且等待结果的到来。

cd ~/catkin_ws/src/test/src
gedit action_client1.py

#!/usr/bin/env python
import rospy
import actionlib
from test.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)
client.send_goal(goal)
client.wait_for_result()
print(client.get_result().time_elapsed.to_sec())

添加可执行权限：chmod u+x action_client1.py

客户端的代码，在导入相关模块之后，创建的SimpleActionClient里面的两个参数跟服务端一样。等待动作服务器启动跟前面的服务rospy.wait_for_service类似，这里是rospy.wait_for_server。接下来就是创建目标，构造一个TimerGoal对象，填入我们想要定时器等待的时间，这里设置为5秒，然后就发送给服务器。最后就是等待服务器的处理结果了，如果一切正常，就打印出获取的结果get_result的持续时间。

运行客户端

rosrun test action_client1.py
#5.005053997

客户端发送5秒的时间给目标，服务端做一个sleep模拟，这个阻塞时间比请求时间要稍微长点，所以会大于5秒。

6、复杂的动作服务器

对于动作有了基本了解之后，我们就重点关注它的特点，也就是动作跟服务的区别，主要是动作的异步特性，接下来修改上述代码，实现终止目标、处理打断请求和实时反馈等功能，突出动作的优势。

cd ~/catkin_ws/src/test/src
gedit action_server2.py

#!/usr/bin/env python
# -*- coding: utf-8
 
import rospy
import time
import actionlib
from test.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback
 
def do_timer(goal):
    start_time = time.time()
    update_count = 0
    
    if goal.time_to_wait.to_sec() > 60:
        result = TimerResult()
        result.time_elapsed = rospy.Duration.from_sec(time.time()-start_time)
        result.updates_sent = update_count
        server.set_aborted(result,"超时终止")
        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,"中断")
            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)
 
    result = TimerResult()
    result.time_elapsed = rospy.Duration.from_sec(time.time()-start_time)
    result.updates_sent = update_count
    server.set_succeeded(result,"成功完成")
 
rospy.init_node("timer_action_server2")
server = actionlib.SimpleActionServer('timer',TimerAction,do_timer,False)
server.start()
rospy.spin()

加个执行权限：chmod u+x action_server2.py

由于需要提供反馈，所以导入TimerFeedback消息类型，对于请求time_to_wait超过60秒的，显式调用set_aborted来终止当前的目标请求，这个调用会给客户端发送一个消息，告知本次目标已被终止。如果没超时就进入到一个循环，在循环中进行间断的休眠等待，在这个过程中检查是否有发生中断is_preempt_requested等情况，并提供反馈。当休眠时长超过请求目标时长，就退出循环，告诉客户端目标已被成功执行完了。 

7、复杂的动作客户端

服务端做了调整之后，我们客户端对其进行测试，比如对反馈进行处理，中断正在执行的目标，以及引发终止等操作：

cd ~/catkin_ws/src/test/src
gedit action_client2.py

#!/usr/bin/env python
# -*- coding: utf-8
 
import rospy
import time
import actionlib
from test.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback
 
def feedback_cb(feedback):
    print("持续时间:%f"%(feedback.time_elapsed.to_sec()))
    print("剩余时间:%f"%(feedback.time_remaining.to_sec()))
 
rospy.init_node('timer_action_client2')
client = actionlib.SimpleActionClient('timer',TimerAction)
client.wait_for_server()
 
goal = TimerGoal()
goal.time_to_wait = rospy.Duration.from_sec(5)
#goal.time_to_wait = rospy.Duration.from_sec(100)
client.send_goal(goal,feedback_cb=feedback_cb)
 
time.sleep(3)
client.cancel_goal()
 
client.wait_for_result()
print("状态码:%d"%(client.get_state()))
print("状态描述:%s"%(client.get_goal_status_text()))
print("持续时间:%f"%(client.get_result().time_elapsed.to_sec()))
print("反馈次数:%d"%(client.get_result().updates_sent))

添加执行权限：chmod u+x action_client2.py

查看反馈的话题的信息

rostopic info /timer/feedback
 
'''
Type: test/TimerActionFeedback
Publishers: 
 * /timer_action_server (http://YAB:33125/)
Subscribers: None
'''

 其消息类型是TimerActionFeedback，我们查看下详情

rosmsg show TimerActionFeedback
'''
[test/TimerActionFeedback]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalStatus status
  uint8 PENDING=0
  uint8 ACTIVE=1
  uint8 PREEMPTED=2
  uint8 SUCCEEDED=3
  uint8 ABORTED=4
  uint8 REJECTED=5
  uint8 PREEMPTING=6
  uint8 RECALLING=7
  uint8 RECALLED=8
  uint8 LOST=9
  actionlib_msgs/GoalID goal_id
    time stamp
    string id
  uint8 status
  string text
test/TimerFeedback feedback
  duration time_elapsed
  duration time_remaining
'''

除了定义的两个持续时间与剩余时间之外，我们还看到了当前的可能状态有10种，主要关注其中三种：PREEMPTED=2、SUCCEEDED=3、ABORTED=4

了解了客户端的一些定义之后，我们来运行测试下(前提是先开启roscore和action_server2.py) 

 

7.1、正常测试

运行客户端：rosrun test action_client2.py

持续时间:0.000023
剩余时间:4.999977
持续时间:1.001364
剩余时间:3.998636
持续时间:2.002950
剩余时间:2.997050
持续时间:3.004577
剩余时间:1.995423
持续时间:4.005102
剩余时间:0.994898
状态码:3
状态描述:成功完成
持续时间:5.007248
反馈次数:5

7.2、终止测试

将时间调到超过60秒，比如100秒

goal.time_to_wait = rospy.Duration.from_sec(100)

运行客户端：rosrun test action_client2.py

状态码:4
状态描述:超时终止
持续时间:0.000014
反馈次数:0

7.3、中断测试 

将下面代码中的注释去掉：

time.sleep(3)
client.cancel_goal()

运行客户端：rosrun test action_client2.py

持续时间:0.000042
剩余时间:4.999958
持续时间:1.002338
剩余时间:3.997662
持续时间:2.005545
剩余时间:2.994455
状态码:2
状态描述:中断
持续时间:3.007982
反馈次数:3

8、小结

节点通信的三种机制就介绍完了，其中本节说的动作机制是ROS中一个功能强大，使用广泛的通信工具。那么在哪些场景选择哪种通信，这里做一个对比：

这里主要是服务和动作有点暧昧，那么这么去理解，在任何时候，当你执行一个任务，想要使用服务的时候，都值得考虑是否使用动作。虽然动作需要写更多的代码，但是比服务强大很多，扩展性也好很多。 

另外代码中出现了中文，所以需要加上：# -*- coding: utf-8

在本节，我们新建了两个节点，都开启之后，我们来查看下节点列表

rosnode list
'''
/rosout
/timer_action_server
/timer_action_server2
'''

 可以看到出现了代码中定义的两个节点。