ros机器人之动作(二)

前面我们实现了动作的定义，接下来实现动作的功能

实现一个基本的动作服务器

准备好所需的动作定义后就可以开始编写代码了。动作和话题一样，都是使用回调机制，即回调函数会在收到消息时被唤醒和调用。

例:simple_action_server.py

1 #!/usr/bin/env python                                                  
 2 import rospy
 3 
 4 import time                              #导入时间time标准库
 5 import actionlib　　　　　　　　　　　　#导入actionlib包来提供将要使用的SimpleActionServer
 6 from basic.msg import TimerAction,TimerGoal,TimerResult       #导入一些从Timer.action中自动生成的消息类
 7 
 8 def do_timer(goal):　　　　　　　　　　　　　#定义一个函数，对收到的目标进行了处理，传入函数do_timer()的参数goal是TimerGoal类型
 9     start_time=time.time()
10     time.sleep(goal.time_to_wait.to_sec())　　　　
11     result=TimerResult()　　　　　　　　　　　　#构造结果消息，对应的类型为ＴimerResult
12     result.time_elapsed=rospy.Duration.from_sec(time.time()-start_time)
13     result.updates_sent=0
14     server.set_succeeded(result)         #以结果作为参数调用set_succeeded()
15 
16 rospy.init_node('time_action_server')
17 server=actionlib.SimpleActionServer('timer',TimerAction,do_timer,False)#构造函数(第一个参数为动作服务器的名称，第二个参数为动作服务器的类型，第三个参数目标的回调函数，最后通过传递Ｆalse参数来关闭动作服务器的自动启动
18 server.start()
19 rospy.spin() 

完成动作服务器的编写，需要检查其工作是否正常，启动roscore ,然后运行动作服务器

qqtsj@qqtsj-Nitro-AN515-51:~/catkin_ws$ rosrun basic simple_action_server.py 

查看相应的话题

qqtsj@qqtsj-Nitro-AN515-51:~/catkin_ws/src/basic/actionnode$ rostopic list/rosout
/rosout_agg
/timer/cancel
/timer/feedback
/timer/goal
/timer/result
/timer/status

动作的使用

为了便利起见，我们直接使用actionlib包中的SimpleActionClient作为客服端

例:simple_action_client.py

 1 #!/usr/bin/env python 
 2 
 3 import rospy                                                           
 4 import actionlib
 5 from basic.msg import TimerAction ,TimerGoal,TimerResult
 6 
 7 rospy.init_node('timer_action_client')
 8 client=actionlib.SimpleActionClient('timer',TimerAction)    #创建一个SimpleActionClient,构造函数的第一个参数为动作客户端的名称，名称必须与我们之前创建的服务器相匹配，第二个参数为动作的类型，也要与服务器相匹配。
 9 client.wait_for_server()　　　　#等待服务器启动
10 goal=TimerGoal()　　　　　　　　　＃创建目标，构建一个TimerGoal对象，并填入我们希望定时器等待的时间(5.0)
11 goal.time_to_wait=rospy.Duration.from_sec(5.0)　　　
12 client.send_goal(goal)
13 client.wait_for_result()
14 print('Time elspsed:%f'% (client.get_result().time_elapsed.to_sec()))
15 #最后就是等待服务器的结果，如果一切正常的话，我们应该会在此处阻塞５s，结果到来后，就可以用来get_result来获得它并打印服务器汇报的time_elapsed信息。

同样，也需要对客服端进行检查，确保roscore和动作服务器均启动，然后运行客户端：

qqtsj@qqtsj-Nitro-AN515-51:~/catkin_ws/src/basic/actionnode$ rosrun basic simple_action_client.py 
Time elspsed:5.006946

在启动客户端和打印结果信息之间，应该出现约５s的延迟。而结果中的time_elapsed则会比５秒稍微长一些，因为time_sleep()的阻塞时间往往比请求时间长。

实现一个更复杂的的动作服务器

动作和服务看起来非常的相似，只是在配置上多了一些步骤，动作和服务的主要区别在于动作的异步特性，复杂的动作可以实现终止目标，处理打断请求和实时反馈功能。

例：fany_action_Server.py

 1 #!/usr/bin/env python                                                  
 2 import time
 3 import rospy
 4 import actionlib
 5 from basic.msg import TimerAction ,TimerGoal,TimerResult,TimerFeedback   #增加量对TimerFeedback消息类型的导入
 6 
 7 def do_timer(goal):
 8     start_time=time.time()
 9     update_count=0　　　　　　　#增加一个变量，用于统计总共发布了多少反馈消息
10 
11     if goal.time_to_wait.to_sec()>60.0:
12         result=TimerResult()
13         result.time_elapsed=rospy.Duration.from_sec(time.time()-start_t   ime)
14         result.updates_sent=update_count
15         server.set_aborted(result,"Timer aborted due to too-long wait")
16         return
17     while (time.time()-start_time)<goal.time_to_wait.to_sec():
18 
19 
20         if server.is_preempt_requested():　#检查是否发生终端，如果发生中断(即客户端在前一个动作还在执行时，发送了新的目标),函数会返回Ture，此时就需要补充一个result,同时提供一个表示状态的字符串，然后调用set_preempted
21             reslut=TimerResult()
22             result.time_elapsed=
23                 rospy.Duration.from_sec(time.time()-start.time)
24             result.updates_sent=update_count
25             server.set_preempted(result,"Timer preempted")
26             return
27 
28         feedback=TimerFeedback()
29         feedback.time_elapsed=rospy.Duration.from_sec(time.time()-start   _time)
30         feedback.time_remaining=goal.time_to_wait-feedback.time_elapsed
31         server.publish_feedback(feedback)　　　　　#把反馈发送给客户端
32         update_count+=1　　　　　　　　　　　#增加１表示进行了一次反馈
33 
34         time.sleep(1.0)
35     result=TimerResult()
36     result.time_elapsed=rospy.Duration.from_sec(time.time()-start_time)
37     result.updates_sent=update_count
38     server.set_succeeded(result,"timer completed successfully")
39 rospy.init_node('time_action_server')
40 server=actionlib.SimpleActionServer('timer' ,TimerAction ,do_timer ,Fal   se)
41 server.start()
42 rospy.spin()
43 

使用更复杂的动作

这个客户端以测试服务端功能，对反馈进行处理，打断正在执行的目标，以及引发一个终止。

例:fancy_action_client.py

 1 #!/usr/bin/env python
 2 import rospy
 3 import time
 4 import actionlib
 5 from basic.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback
 6 
 7 def feedback_cb(feedback):               #定义一个回调函数feedback_cb(),当收到反馈消息时会被执行。
 8     print('[Feedback] Time elapsed:%f' %(feedback.time_elapsed.to_sec()   ))
 9     print('[Feedback] Time remaining: %f'%(feedback.time_remaining.to_s   ec()))
10 
11 rospy.init_node('timer_action_client')
12 client=actionlib.SimpleActionClient('timer',TimerAction)
13 client.wait_for_server()
14 goal=TimerGoal()
15 goal.time_to_wait=rospy.Duration.from_sec(5.0)
16 #Uncomment this line to test server-side abort:                        
17 #goal.time_to_wait=rospy.Duration.from_sec(500.0)
18 client.send_goal(goal, feedback_cb=feedback_cb)　　　　#将回调函数作为feedback_cb关键词的参数
19 
20 #Uncomment these lines to test goal preemption:
21 #time.sleep(3.0)
22 #client.cancel_goal()
23  
24 client.wait_for_result()
25 print('[Result] State: %d' %(client.get_state()))
26 print('[Result] State: %s' %(client.get_goal_status_text()))
27 print('[Result] Time elapse: %f'%(client.get_result().time_elapsed.to_s   ec()))
28 print('[Result] Updates sent: %d'%(client.get_result().updates_sent))
~                                                                         

跟以前一样，先启动roscore,然后运行server,在运行客服端client,结果如下

qqtsj@qqtsj-Nitro-AN515-51:~/catkin_ws/src/basic/actionnode$ rosrun basic fancy_action_client.py 
[Feedback] Time elapsed:0.000043
[Feedback] Time remaining: 4.999957
[Feedback] Time elapsed:1.001889
[Feedback] Time remaining: 3.998111
[Feedback] Time elapsed:2.003785
[Feedback] Time remaining: 2.996215
[Feedback] Time elapsed:3.005333
[Feedback] Time remaining: 1.994667
[Feedback] Time elapsed:4.007131
[Feedback] Time remaining: 0.992869
[Result] State: 3
[Result] State: timer completed successfully
[Result] Time elapse: 5.007945
[Result] Updates sent: 5

所有节点都如期运行起来了

现在来引发一个服务端的主动终止，将等待时间从５s改为５００s

再次运行客户端

qqtsj@qqtsj-Nitro-AN515-51:~/catkin_ws/src/basic/actionnode$ rosrun basic fancy_action_client.py 
[Result] State: 4
[Result] State: Timer aborted due to too-long wait
[Result] Time elapse: 0.000139
[Result] Updates sent: 0

服务端立即主动终止了目标的执行

小结

本章探讨了ros的动作机制，它是ros中一个功能强大，使用广泛的通信工具，与服务类似，动作允许你发起一个请求（即目标），同时接受一个响应（即结果），不过，动作提供了更多的控制形式，

服务端可以在执行过程中提供反馈，客户端也可以取消之前发出的目标。

话题，服务，动作机制的对比

类型　　　　　　　　　　最佳使用场景

话题　　　　　　　单工通信，尤其是接收方多个时（如传感器数据流）

服务　　　　　　　简单的请/响应式交互场景，如询问节点的当前状态

动作　　　　　　　大部分请求/响应式交互场景，尤其是执行过程不能立即完成时(如导航前往一个目标点)