ROS开发笔记（3）：基于RoboWare Studio 与Python编写动作（action）通讯节点（node）

ROS的动作非常适合时间不确定，目标导向型的操作接口。原理上用话题实现，其本质是相关于规定了一系列话题（目标、结果、反馈、取消等）的组合使用方法的高层协议。

1、定义动作 与 相关话题说明

选中action目录名，右键AddActionFile,这里定义类似定时器的动作，格式与service类似，分为goal、result、feedback三段，同样由‘---’分隔，文件Timer.action内容：

#the goal
duration time_to_wait  # Specify which dishwasher we want to use
---
#the result
duration timer_elapsed
int32 updates_sent
---
# the feedback message 
duration timer_elapsed
duration timer_remaining

为了帮助理解，我这里先把动作有关的测试结果展示出来，添加并启动动作服务端simple_action_server与客户端程序simple_action_client（后文有详细介绍）以后，新出现了timer动作的五个话题：

wsc@wsc-pc:~/ros1_ws$ rostopic list
/rosout
/rosout_agg
/timer/cancel
/timer/feedback
/timer/goal
/timer/result

其中goal、cancer消息 ，由client端发布，server端订阅，而result、feedback消息 ，由server端发布，client端订阅，话题的详细信息如下：

wsc@wsc-pc:~/ros1_ws$ rostopic  info /timer/goal
Type: ros_dev_test/TimerActionGoal

Publishers:
 * /simple_action_client (http://wsc-pc:35747/)

Subscribers:
 * /simple_action_server (http://wsc-pc:41467/)

wsc@wsc-pc:~/ros1_ws$ rostopic  info /timer/result
Type: ros_dev_test/TimerActionResult

Publishers:
 * /simple_action_server (http://wsc-pc:41467/)

Subscribers:
 * /simple_action_client (http://wsc-pc:34939/)

wsc@wsc-pc:~/ros1_ws$ rostopic  info /timer/feedback
Type: ros_dev_test/TimerActionFeedback

Publishers:
 * /simple_action_server (http://wsc-pc:41467/)

Subscribers:
 * /simple_action_client (http://wsc-pc:42565/)

wsc@wsc-pc:~/ros1_ws$ rostopic  info /timer/cancel
Type: actionlib_msgs/GoalID

Publishers:
 * /simple_action_client (http://wsc-pc:40621/)

Subscribers:
 * /simple_action_server (http://wsc-pc:41467/)

以/timer/goal为例，其消息类型为TimerActionGoal，我们来看看其详细信息：

wsc@wsc-pc:~/ros1_ws$ rosmsg  show  TimerActionGoal
[ros_dev_test/TimerActionGoal]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
ros_dev_test/TimerGoal goal
  duration time_to_wait

可以看出其是一个组合消息，其中与我们自己定义相关的是ros_dev_test/TimerGoal，其详细信息如下：

wsc@wsc-pc:~/ros1_ws$ rosmsg  show  TimerGoal
[ros_dev_test/TimerGoal]:
duration time_to_wait

 

可以看出这个是和action文件的goal段对应可以来的，另外还有：

wsc@wsc-pc:~/ros1_ws$ rosmsg  show  TimerResult
[ros_dev_test/TimerResult]:
duration timer_elapsed
int32 updates_sent


wsc@wsc-pc:~/ros1_ws$ rosmsg  show  TimerFeedback
[ros_dev_test/TimerFeedback]:
duration timer_elapsed
duration timer_remaining

以上的 TimerAction、TimerActionGoal、TimerActionFeedback、TimerActionResult、TimerGoal、TimerFeedback、TimerResult在加入action动作文件后由ROS编译系统自动生成，同样，添加action文件需要对CMakeLists.txt、package.xml 进行的修改RoboWare Studio也替我们自动完成了，CMakeLists.txt主要修改了以下几处：

2、实现动作server端

代码及注释如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time

#actionlib包含了了SimpleActionServer
import actionlib

# 导入action文件自动生成的消息类
from ros_dev_test.msg import TimerAction,TimerGoal,TimerResult

#goal处理函数并返回处理结果
def do_timer(goal):
    start_time=time.time()
    time.sleep(goal.time_to_wait.to_sec())
    result=TimerResult()
    result.timer_elapsed=rospy.Duration.from_sec(time.time()-start_time)
    result.updates_sent=0
    action_server.set_succeeded(result)


rospy.init_node('simple_action_server')
action_server=actionlib.SimpleActionServer('timer',TimerAction,do_timer,False)
action_server.start()
rospy.spin()

注意运行前先要通过chmod u+x  命令添加运行权限。

3、client端使用动作

使用服务的client端代码及注释如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time

#actionlib包含了了SimpleActionServer
import actionlib

# 导入action文件自动生成的消息类
from ros_dev_test.msg import TimerAction,TimerGoal,TimerResult


rospy.init_node('simple_action_client')
action_client=actionlib.SimpleActionClient('timer',TimerAction)

#等待服务启动
action_client.wait_for_server()

#发送goal消息
goal=TimerGoal()
goal.time_to_wait=rospy.Duration.from_sec(5.0)
action_client.send_goal(goal)

action_client.wait_for_result()
print('Time elapsed: %f' %(action_client.get_result().timer_elapsed.to_sec()))

注意运行前先要通过chmod u+x  命令添加运行权限。

3、完整一点的server端与client端代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time

#actionlib包含了了SimpleActionServer
import actionlib

# 导入action文件自动生成的消息类
from ros_dev_test.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback

#goal处理函数并返回处理结果
def do_timer(goal):
    start_time=time.time()
    update_count=0
    #异常处理
    if goal.time_to_wait.to_sec()>60.0:
        result=TimerResult()
        result.timer_elapsed=rospy.Duration.from_sec(time.time()-start_time)
        result.updates_sent=update_count
        #动作异常退出
        action_server.set_aborted(result,'Timer aborted due to too-long wait')
        return
    
    while((time.time()-start_time)<goal.time_to_wait.to_sec()):
        
        #判断是否发生中断，比如收到新的目标或者客户端取消目标等
        if action_server.is_preempt_requested():
            result=TimerResult()
            result.timer_elapsed=rospy.Duration.from_sec(time.time()-start_time)
            result.updates_sent=update_count
            action_server.set_preempted(result,'Timer preempted')
            return

        feedback=TimerFeedback()
        feedback.timer_elapsed=rospy.Duration.from_sec(time.time()-start_time)
        feedback.timer_remaining=goal.time_to_wait-feedback.timer_elapsed
        action_server.publish_feedback(feedback)
        update_count+=1
        time.sleep(1)

    result=TimerResult()
    result.timer_elapsed=rospy.Duration.from_sec(time.time()-start_time)
    result.updates_sent=update_count
    action_server.set_succeeded(result,'Timer completed successfully')    



rospy.init_node('simple_action_server')
action_server=actionlib.SimpleActionServer('timer',TimerAction,do_timer,False)
action_server.start()
print 'start server'
rospy.spin()

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time

#actionlib包含了了SimpleActionServer
import actionlib

# 导入action文件自动生成的消息类
from ros_dev_test.msg import TimerAction,TimerGoal,TimerResult,TimerFeedback

def feedback_callback(feedback):
    print('[feedback] timer_elapsed %f'% (feedback.timer_elapsed.to_sec()))
    print('[feedback] timer_remaining %f'% (feedback.timer_remaining.to_sec()))
    
rospy.init_node('simple_action_client')
action_client=actionlib.SimpleActionClient('timer',TimerAction)

#等待服务启动
action_client.wait_for_server()

#发送goal消息
goal=TimerGoal()

goal.time_to_wait=rospy.Duration.from_sec(5.0)
action_client.send_goal(goal,feedback_cb=feedback_callback)

#测试时间超长异常
# goal.time_to_wait=rospy.Duration.from_sec(70.0)
# action_client.send_goal(goal)

#测试中途取消终止目标
# time.sleep(3.0)
# action_client.cancel_goal()


action_client.wait_for_result()
print('[Result] state: %d' %(action_client.get_state()))
print('[Result] status: %s' %(action_client.get_goal_status_text()))

print('[Result] Updates_sent: %f' %(action_client.get_result().updates_sent))
print('[Result] Time elapsed: %f' %(action_client.get_result().timer_elapsed.to_sec()))