赞
踩
从本文开始我们用三篇文章学习机器人SLAM建图与自主导航,本文先单独介绍下ROS的Action通信机制,后面自主导航的样例将用到ros action。在之前的ROS高效入门系列文章里,我们详细学习了ros的topic和service通信机制,而ros的action是第三种通信机制。
本文参考资料如下:
(1)《ROS机器人开发实践》胡春旭 第12章第一节
(2)chatGPT
本系列博客汇总:ROS高效进阶系列。
(1)ros topic是异步通信,是广播通信,通信上下游没有应答机制,类似udp协议。而ros service是一种带有应答(请求和回复)的通信机制,采用C/S模式,是同步通信,是一对一的,类似tcp协议。
(2)ros action类似service,也有应答机制,也是C/S模式,不同之处在于ros action带有连续反馈。server端可以不停的向client端发布任务进展,直到任务结束时发布一次任务处理结论,而client可以在任务执行中间发送任务终止请求。很明显,ros action适合大型任务的通信沟通,通常需要持续一段时间。
(3)ros action依赖ros的actionlib库,基于ros msg进行封装的一种更高级的通信方式。其中,client与server端有如下五个接口:
goal:client使用goal接口向server发布任务
cancel:任务中途,client使用cancel接口向server发布任务中止请求
status:任务结束时,server向client反馈任务状态,如SUCCEEDED,ABORTED
result:任务结束时,server向client反馈任务结果,比如刷了多少个盘子
feedback:任务过程中,server向client反馈任务处理进展,比如完成了百分之多少
(1)我们创建的样例逻辑是:客户端向服务端指定一个洗碗机刷盘子,服务端拿到洗碗机号,去刷盘子,定期反馈进度,最后反馈盘子个数。任务过程中,客户端可能会向服务端发送任务中止请求,服务端收到中止请求,返回任务状态和已经刷的盘子个数。
创建 action_example 样例及相关文件:
cd ~/catkin_ws/src
catkin_create_pkg action_sample actionlib actionlib_msgs std_msgs rospy roscpp message_generation message_runtime
cd action_sample
mkdir action launch scripts
touch action/DoDishes.action
touch launch/start_cpp.launch launch/start_py.launch
touch scripts/DoDishes_client.py scripts/DoDishes_server.py
touch src/DoDishes_client.cpp src/DoDishes_server.cpp
(2)首先是定义action文件,通常是三段式(goal,result,feedback),如这里的 DoDishes.action:
# Goal,内容是洗碗机号
uint32 dishwasher_id
---
# Result,刷盘子的个数
uint32 total_dishes_cleaned
---
# Feedback,反馈一般是进度
float32 percent_complete
(3)DoDishes_client.cpp
#include <actionlib/client/simple_action_client.h>
#include "action_sample/DoDishesAction.h"
// actionlib::SimpleClientGoalState 是拓扑图中的 status
// action_sample::DoDishesResultConstPtr 是拓扑图中的 result
// 服务端完成任务并反馈后,客户端调用一次doneCb
void doneCb(const actionlib::SimpleClientGoalState& state,
const action_sample::DoDishesResultConstPtr& result) {
ROS_INFO("%d dishes is clean, state is %s", result->total_dishes_cleaned, state.toString().c_str());
ros::shutdown();
}
// 服务端接收任务并启动后,客户端调用一次activeCb
void activeCb() {
ROS_INFO("job is active");
}
// 服务端周期反馈进度,客户端周期调用feedbackCb
void feedbackCb(const action_sample::DoDishesFeedbackConstPtr& feedback) {
ROS_INFO("percent is %f", feedback->percent_complete);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "do_dishes_client");
// action的类型名是DoDishes.action文件的名字加Action,即DoDishesAction
// 客户端与服务端通过action名链接,即"do_dishes"
// true表示客户端启动后,就自动尝试连接服务端,理论上这里是true,就不需要调用下面的waitForServer()
actionlib::SimpleActionClient<action_sample::DoDishesAction> client("do_dishes", true);
ROS_INFO("waiting for action server to start.");
client.waitForServer();
ROS_INFO("action server started, sending goal.");
action_sample::DoDishesGoal goal;
goal.dishwasher_id = 1;
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
// sleep(4);
// client.cancelGoal(); // 任务取消命令
ros::spin();
return 0;
}
(4)DoDishes_server.cpp
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_sample/DoDishesAction.h"
// action_sample::DoDishesGoalConstPtr是拓扑中的goal
void execute(const action_sample::DoDishesGoalConstPtr& goal, actionlib::SimpleActionServer<action_sample::DoDishesAction>* server) {
ros::Rate rate(1);
// 这是拓扑中的feedback
// 这是拓扑中的result
action_sample::DoDishesFeedback feedback;
action_sample::DoDishesResult result;
ROS_INFO("dishwasher %d is working", goal->dishwasher_id);
int i = 0;
for (i = 0; i < 10; i++) {
feedback.percent_complete = i * 10;
// 调用publishFeedback()周期反馈进度
server->publishFeedback(feedback);
rate.sleep();
// 如果任务被取消,则执行这里,反馈取消状态和中途执行结果
if (server->isPreemptRequested()) {
ROS_INFO("The goal is canceled");
result.total_dishes_cleaned = i*10;
server->setPreempted(result);
return;
}
}
ROS_INFO("dishwasher %d finish working", goal->dishwasher_id);
result.total_dishes_cleaned = i*10;
// 任务成功,执行setSucceeded(),反馈成功状态和结果
server->setSucceeded(result);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "do_dishes_server");
ros::NodeHandle nh;
// "do_dishes"必须与客户端的名称相同
// 接收到任务后,执行execute()回调函数
// false表示需要调用start(),手动启动服务端
// 这里在创建server句柄的同时,又把server句柄绑定给了execute(),用法比较特殊
actionlib::SimpleActionServer<action_sample::DoDishesAction> server(nh, "do_dishes", std::bind(&execute, std::placeholders::_1, &server), false);
server.start();
ros::spin();
return 0;
}
(5)DoDishes_client.py(注释类似DoDishes_client.cpp,请对照着看,不写了)
#! /usr/bin/env python3
import rospy
import actionlib
from action_sample.msg import DoDishesAction, DoDishesGoal
def done_cb(state, result):
rospy.loginfo("%d dishes is clean, state is %s" %(result.total_dishes_cleaned, str(state)))
rospy.signal_shutdown('Normal shutdown')
def active_cb():
rospy.loginfo("job is action")
def feedback_cb(feedback):
rospy.loginfo("percent is %f" %feedback.percent_complete)
def main():
rospy.init_node("do_dishes_client")
client = actionlib.SimpleActionClient("do_dishes", DoDishesAction)
rospy.loginfo("waiting for action server to start")
client.wait_for_server()
rospy.loginfo("action server started, sending goal")
goal = DoDishesGoal()
goal.dishwasher_id = 1
client.send_goal(goal, done_cb=done_cb, active_cb=active_cb, feedback_cb=feedback_cb)
rospy.sleep(4)
client.cancel_goal()
rospy.spin()
if __name__ == "__main__":
main()
(6)DoDishes_server.py
#! /usr/bin/env python3
import rospy
import actionlib
from action_sample.msg import DoDishesAction, DoDishesGoal, DoDishesFeedback, DoDishesResult
// 采用类解决execute_cb()绑定server句柄的问题
class DishWasherServer(object):
def __init__(self, action_name):
self._action_name = action_name
self._server = actionlib.SimpleActionServer(self._action_name, DoDishesAction, execute_cb=self.execute_cb, auto_start=False)
def start_server(self):
self._server.start()
def execute_cb(self, goal):
rate = rospy.Rate(1)
rospy.loginfo("dishwasher %d is working" %goal.dishwasher_id)
feedback = DoDishesFeedback()
result = DoDishesResult()
for i in range(10):
feedback.percent_complete = i * 10
self._server.publish_feedback(feedback)
rate.sleep()
if self._server.is_preempt_requested():
rospy.loginfo("the goal is canceled")
result.total_dishes_cleaned = i * 10
self._server.set_preempted(result)
return
rospy.loginfo("dishwasher %d finish working" %goal.dishwasher_id)
result.total_dishes_cleaned = (i+1) * 10
self._server.set_succeeded(result)
def main():
rospy.init_node("do_dishes_server")
dish_washer_server = DishWasherServer("do_dishes")
dish_washer_server.start_server()
rospy.spin()
if __name__ == "__main__":
main()
(7)CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(action_sample)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
actionlib
actionlib_msgs
)
add_action_files(
DIRECTORY action
FILES DoDishes.action
)
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs
)
catkin_package(
CATKIN_DEPENDS actionlib actionlib_msgs roscpp rospy std_msgs message_runtime
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(DoDishes_client src/DoDishes_client.cpp)
add_executable(DoDishes_server src/DoDishes_server.cpp)
add_dependencies(DoDishes_client ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(DoDishes_server ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(DoDishes_client
${catkin_LIBRARIES}
)
target_link_libraries(DoDishes_server
${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
scripts/DoDishes_client.py scripts/DoDishes_server.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
(8)start_cpp.launch
<launch>
<node
pkg="action_sample"
type="DoDishes_client"
name="do_dishes_client"
required="true"
output="screen"
/>
<node
pkg="action_sample"
type="DoDishes_server"
name="do_dishes_server"
respawn="true"
output="screen"
/>
</launch>
(9)start_py.launch
<launch>
<node
pkg="action_sample"
type="DoDishes_client.py"
name="do_dishes_client"
required="true"
output="screen"
/>
<node
pkg="action_sample"
type="DoDishes_server.py"
name="do_dishes_server"
respawn="true"
output="screen"
/>
</launch>
(10)编译并运行
cd ~/catkin_ws
catkin_make --source src/action_sample
source devel/setup.bash
roslaunch action_sample start_cpp.launch
roslaunch action_sample start_py.launch
start_cpp运行截图:
start_py运行截图:
本文中的例子放在了本人的github上: action_sample
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。