ROS之动作(计时器进阶)

---恢复内容开始---

上次的代码只有简单的发送目标和接受结果，这次将实现终止目标、处理打断请求和实时反馈的功能

fancy_action_server.py

#!/usr/bin/env python                                                             

import rospy

import time
import actionlib
from basic.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.0: 　　　　　　　　　　　　　　　　　　　　　　　　　　　　#增加的错误检查，当时长大于60则则处理错误
        result = TimerResult()　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#创建一个TimerResula的类型
        result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)　　　　　　　#将运行的时间从秒转换为ROS的Duration类型
        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():　　　　　　　　　　　　　　　　　　　　　　　　　　　　#检查是否打断，（即在客户端执行前一个动作时，又发送了新的目标）函数会返回True
            result = TimerResult()　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#此时需要填充一个result,同时返回提示信息
            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()　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#创建TimerFeedback类型
        feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)　　#赋值为处理经过的时间，并从秒转换为ROS的Duration类型
        feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed　　　　　　#赋值还需要等待的时间，用目标时间减去已经运行的时间
        server.publish_feedback(feedback)　　　　　　　　　　　　　　　　　　　　　　　　　　　　#发布反馈给客户端
        update_count += 1　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#增加update_count来表示反馈的次数

        time.sleep(1.0)　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#进行短暂的休眠，这里使用的是固定休眠时长，这样回导致世界休眠时间过长超过目标时间，

    result = TimerResult()　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#循环结束后创建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('timer_action_server')　　　　　　　　　　　　　　　　　　　　　　　　　　#初始化节点
server = actionlib.SimpleActionServer('timer' , TimerAction , do_timer,False)　　　　#
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):　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　#创建回调函数，处理反馈信息，这里只是简单将反馈信息打印
        print('[Feedback] Time elapsed: %f ' % (feedback.time_elapsed.to_sec()))　　　　　　#目标已经执行的时间　　
        print('[Feedback] TIme remaining: %f' % (feedback.time_remaining.to_sec()))　　　　#目标还需要执行的时间

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)　　　　　　　　　　　　　　　　　　　　　　　　#发送目标

client.send_goal(goal ,feedback_cb = feedback_cb)　　　　　　　　　　　　　　　　　　　　　　　#将回调函数作为feedback_cb关键词作为参数，传入send_goal中完成回调的注册

#time.sleep(3.0)
#client.cancel_goal()


client.wait_for_result()

print('[Result] State: %d' % (client.get_state()))
print('[Result] Status: %s' % (client.get_goal_status_text()))
print('[Result] Time elapsed: %f' % (client.get_result().time_elapsed.to_sec()))
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