ros机器人之动作(二)

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

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

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

例:simple_action_server.py

 #!/usr/bin/env python
  import rospy

  import time                              #导入时间time标准库
  import actionlib　　　　　　　　　　　　#导入actionlib包来提供将要使用的SimpleActionServer
  from basic.msg import TimerAction,TimerGoal,TimerResult       #导入一些从Timer.action中自动生成的消息类

  def do_timer(goal):　　　　　　　　　　　　　#定义一个函数，对收到的目标进行了处理，传入函数do_timer()的参数goal是TimerGoal类型
      start_time=time.time()
     time.sleep(goal.time_to_wait.to_sec())　　　　
     result=TimerResult()　　　　　　　　　　　　#构造结果消息，对应的类型为ＴimerResult
     result.time_elapsed=rospy.Duration.from_sec(time.time()-start_time)
     result.updates_sent=
     server.set_succeeded(result)         #以结果作为参数调用set_succeeded()

 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-:~/catkin_ws$ rosrun basic simple_action_server.py 

查看相应的话题

qqtsj@qqtsj-Nitro-AN515-:~/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

  #!/usr/bin/env python 

  import rospy
  import actionlib
  from basic.msg import TimerAction ,TimerGoal,TimerResult

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

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

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

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

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

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

例：fany_action_Server.py

  #!/usr/bin/env python
  import time
  import rospy
  import actionlib
  from basic.msg import TimerAction ,TimerGoal,TimerResult,TimerFeedback   #增加量对TimerFeedback消息类型的导入

  def do_timer(goal):
      start_time=time.time()
      update_count=0　　　　　　　#增加一个变量，用于统计总共发布了多少反馈消息

     if goal.time_to_wait.to_sec()>60.0:
         result=TimerResult()
         result.time_elapsed=rospy.Duration.from_sec(time.time()-start_t   ime)
         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():　#检查是否发生终端，如果发生中断(即客户端在前一个动作还在执行时，发送了新的目标),函数会返回Ture，此时就需要补充一个result,同时提供一个表示状态的字符串，然后调用set_preempted
             reslut=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('time_action_server')
 server=actionlib.SimpleActionServer('timer' ,TimerAction ,do_timer ,Fal   se)
 server.start()
 rospy.spin()
 

使用更复杂的动作

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

例:fancy_action_client.py

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

  def feedback_cb(feedback):               #定义一个回调函数feedback_cb(),当收到反馈消息时会被执行。
      print('[Feedback] Time elapsed:%f' %(feedback.time_elapsed.to_sec()   ))
      print('[Feedback] Time remaining: %f'%(feedback.time_remaining.to_s   ec()))

 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)
 #Uncomment this line to test server-side abort:
 #goal.time_to_wait=rospy.Duration.from_sec(500.0)
 client.send_goal(goal, feedback_cb=feedback_cb)　　　　#将回调函数作为feedback_cb关键词的参数

 #Uncomment these lines to test goal preemption:
 #time.sleep(3.0)
 #client.cancel_goal()

 client.wait_for_result()
 print('[Result] State: %d' %(client.get_state()))
 print('[Result] State: %s' %(client.get_goal_status_text()))
 print('[Result] Time elapse: %f'%(client.get_result().time_elapsed.to_s   ec()))
 print('[Result] Updates sent: %d'%(client.get_result().updates_sent))
~                                                                         

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

qqtsj@qqtsj-Nitro-AN515-:~/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:
[Result] State: timer completed successfully
[Result] Time elapse: 5.007945
[Result] Updates sent: 

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

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

再次运行客户端

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

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

小结

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

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

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

类型　　　　　　　　　　最佳使用场景

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

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

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