ros机器人之动作(二)

1 #!/usr/bin/env python                                                  
 2import rospy
 3 
 4import time                              #导入时间time标准库
 5import actionlib　　　　　　　　　　　　#导入actionlib包来提供将要使用的SimpleActionServer
 6 frombasic.msg import TimerAction,TimerGoal,TimerResult       #导入一些从Timer.action中自动生成的消息类
 7 
 8def do_timer(goal):　　　　　　　　　　　　　#定义一个函数，对收到的目标进行了处理，传入函数do_timer()的参数goal是TimerGoal类型
 9     start_time=time.time()
10time.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
14server.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() 

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

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

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

 1 #!/usr/bin/env python                                                  
 2import time
 3import rospy
 4import actionlib
 5 frombasic.msg import TimerAction ,TimerGoal,TimerResult,TimerFeedback   #增加量对TimerFeedback消息类型的导入
 6 
 7def 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         ifserver.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
31server.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)
41server.start()
42rospy.spin()
43 

 1 #!/usr/bin/env python
 2import rospy
 3import time
 4import actionlib
 5 frombasic.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback
 6 
 7def 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)
13client.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  
24client.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))
~                                                                         

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

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-longwait
[Result] Time elapse: 0.000139[Result] Updates sent: 0