ROS之动作(计时器进阶)
---恢复内容开始---
上次的代码只有简单的发送目标和接受结果，这次将实现终止目标、处理打断请求和实时反馈的功能
fancy_action_server.py
1 #!/usr/bin/env python 2 3 import rospy 4 5 import time 6 import actionlib 7 from basic.msg import TimerAction , TimerGoal , TimerResult , TimerFeedback #这里导入生成的包 8 9 def do_timer(goal): #创建处理函数 10 start_time = time.time() #记录当前的时间 11 update_count = 0 #增加这份额变量统计一共发了多少次反馈 12 13 if goal.time_to_wait.to_sec() > 60.0: #增加的错误检查，当时长大于60则则处理错误 14 result = TimerResult() #创建一个TimerResula的类型 15 result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) #将运行的时间从秒转换为ROS的Duration类型 16 result.updates_sent = update_count #将反馈的次数返回 17 server.set_aborted(result , "Timer aborted due to too-long wait") #给予提示信息 18 return 19 20 while(time.time() - start_time) < goal.time_to_wait.to_sec(): #通过了错误检查后，当完成条件未达到时，则进入循环执行一下程序，，，可以在动作执行过程中进行一些操作如检查是否打断，提供反馈 21 if server.is_preempt_requested(): #检查是否打断，（即在客户端执行前一个动作时，又发送了新的目标）函数会返回True 22 result = TimerResult() #此时需要填充一个result,同时返回提示信息 23 result.time_elapsed = 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() #创建TimerFeedback类型 29 feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) #赋值为处理经过的时间，并从秒转换为ROS的Duration类型 30 feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed #赋值还需要等待的时间，用目标时间减去已经运行的时间 31 server.publish_feedback(feedback) #发布反馈给客户端 32 update_count += 1 #增加update_count来表示反馈的次数 33 34 time.sleep(1.0) #进行短暂的休眠，这里使用的是固定休眠时长，这样回导致世界休眠时间过长超过目标时间， 35 36 result = TimerResult() #循环结束后创建TimerResult类型将结果返回，并返回成功的信息 37 result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) # 38 result.updates_sent = update_count # 39 server.set_succeeded(result , "Timer completed successfully") # 40 41 rospy.init_node('timer_action_server') #初始化节点 42 server = actionlib.SimpleActionServer('timer' , TimerAction , do_timer,False) # 43 server.start() # 44 rospy.spin() #
创建新的客户端，包括对反馈的处理，打断正在执行的目标，以及引发一个终止
fancy_action_client.py
1 #!/usr/bin/env python 2 3 import rospy 4 5 import time 6 import actionlib 7 from basic.msg import TimerAction , TimerGoal , TimerResult , TimerFeedback 8 9 def feedback_cb(feedback): #创建回调函数，处理反馈信息，这里只是简单将反馈信息打印 10 print('[Feedback] Time elapsed: %f ' % (feedback.time_elapsed.to_sec())) #目标已经执行的时间 11 print('[Feedback] TIme remaining: %f' % (feedback.time_remaining.to_sec())) #目标还需要执行的时间 12 13 rospy.init_node('timer_action_client') 14 client = actionlib.SimpleActionClient('timer',TimerAction) #声明节点 15 client.wait_for_server() #等待服务器的启动 16 goal = TimerGoal() 17 goal.time_to_wait = rospy.Duration.from_sec(5.0) #发送目标 18 19 client.send_goal(goal ,feedback_cb = feedback_cb) #将回调函数作为feedback_cb关键词作为参数，传入send_goal中完成回调的注册 20 21 #time.sleep(3.0) 22 #client.cancel_goal() 23 24 25 client.wait_for_result() 26 27 print('[Result] State: %d' % (client.get_state())) 28 print('[Result] Status: %s' % (client.get_goal_status_text())) 29 print('[Result] Time elapsed: %f' % (client.get_result().time_elapsed.to_sec())) 30 print('[Result] Updates sent: %d' % (client.get_result().updates_sent))
get_status()函数的返回对应的执行结果类型为actionlib_msgs/GoalStatus 可能的状态有10种，这里只用到了三种：PREEMPTED = 2,SUCCEEDED = 3 , ABORTED = 4,
[actionlib_msgs/GoalStatus]: 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
运行上述代码测试功能
miao@openlib:~$ rosrun basic fancy_action_client.py [Feedback] Time elapsed: 0.000044 [Feedback] TIme remaining: 4.999956 [Feedback] Time elapsed: 1.001584 [Feedback] TIme remaining: 3.998416 [Feedback] Time elapsed: 2.003138 [Feedback] TIme remaining: 2.996862 [Feedback] Time elapsed: 3.004638 [Feedback] TIme remaining: 1.995362 [Feedback] Time elapsed: 4.006026 [Feedback] TIme remaining: 0.993974 [Result] State: 3 [Result] Status: Timer completed successfully [Result] Time elapsed: 5.007281 [Result] Updates sent: 5
将21,22的注释去掉测试。
miao@openlib:~$ rosrun basic fancy_action_client.py [Feedback] Time elapsed: 0.000043 [Feedback] TIme remaining: 4.999957 [Feedback] Time elapsed: 1.001521 [Feedback] TIme remaining: 3.998479 [Feedback] Time elapsed: 2.003029 [Feedback] TIme remaining: 2.996971 [Result] State: 2 [Result] Status: Timer preempted [Result] Time elapsed: 3.003640 [Result] Updates sent: 3
然后测试 将发送时间设为100s
miao@openlib:~$ rosrun basic fancy_action_client.py [Result] State: 4 [Result] Status: Timer aborted due to too-long wait [Result] Time elapsed: 0.000024 [Result] Updates sent: 0
