ROS 动作编程

63 阅读 0 评论 42 点赞

我是靠谱客的博主呆萌猫咪，最近开发中收集的这篇文章主要介绍ROS 动作编程，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

带有连续反馈
Goal发布任务目标。Cancel请求取消任务，status通知客户端当前状态，result向客户端发送任务执行结果，只发布一次
例：洗盘子
客户端（发送指令）发送洗盘子命令给服务端，服务端（执行指令）开始洗盘子并实时反馈当前洗盘子的进度，洗完后发送一个成功命令给客户端。
1、在功能包目录下创建action文件夹，在action文件夹下创建命令指令command.action，内容如下：
mkdir ~/catkin_ws/src/my_package/action
cd ~/catkin_ws/src/my_package/action
~/catkin_ws/src/my_package/action$ cat command.action

#define the goal
uit32 dishwasher_id #specify which dishwasher was want to use
---
#define the result
uint 32 total_dishes_cleaned
---
#define a feedback message
float32 percent_complete

在package.xml添加功能包依赖
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
修改如下：编辑CMakeList.txt文件
find_package(catkin REQUIRED COMPONENTS
actionlib_msgs
actionlib
)
add_action_files(
DIRECTORY action
FILES
command.action
)
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs
#message_generation
)

2、初始化ROS节点，
启动服务器，等待动作请求、在回调函数中完成动作服务功能的处理，并反馈进度信息动作完成，发送结束信息~/catkin_ws/src/my_package/src/command_server.cpp

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "my_package/commandAction.h"
typedef actionlib::SimpleActionServer<my_package::commandAction> Server;
// 收到action的goal后调用该回调函数
void execute(const my_package::commandGoalConstPtr &goal, Server *as)
{
   ros::Rate r(1);
   my_package::commandFeedback feedback;
   ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);
   // 假设洗盘子的进度，并且按照1Hz的频率发布进度feedback
   for(int i = 1; i <= 10; i++)
   {
       feedback.percent_complete = i * 10;
       as->publishFeedback(feedback);
       r.sleep();
   }
   // 当action完成后，向客户端返回结果
   ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
   as->setSucceeded();
}
int main(int argc, char **argv)
{
   ros::init(argc, argv, "do_dishes_server");
   ros::NodeHandle hNode;
   // 定义一个服务器
   Server server(hNode, "do_dishes", boost::bind(&execute, _1, &server), false);
   // 服务器开始运行
   server.start();
   ros::spin();
   return 0;
}
3、创建动作客户端、连接动作服务端、发送动作目标、根据不同类型的服务端反馈处理回调函数，创建动作客户端 ~/catkin_ws/src/my_package/src/command_client.cpp

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "my_package/commandAction.h"
typedef actionlib::SimpleActionClient<my_package::commandAction> Client;
// 当action完成后会调用该回调函数一次
void doneCallback(const actionlib::SimpleClientGoalState &state   ,
const my_package::commandResultConstPtr &result)
{
   ROS_INFO("Yay! The dishes are now clean");
   ros::shutdown();
}
// 当action激活后会调用该回调函数一次
void activeCallback()
{
   ROS_INFO("Goal just went active");
}
// 收到feedback后调用该回调函数
void feedbackCallback(const my_package::commandFeedbackConstPtr &feedback)
{
   ROS_INFO("percent_complete : %f", feedback->percent_complete);
}
int main(int argc, char **argv)
{
   ros::init(argc, argv, "do_dishes_client");
   // 定义一个客户端
   Client client("do_dishes", true);
   // 等待服务器端
   ROS_INFO("Waiting for action server to start.");
   client.waitForServer();
   ROS_INFO("Action server started, sending goal.");
   // 创建一个 action 的 goal
   my_package::commandGoal goal;
   goal.dishwasher_id = 1;
   // 发送action的goal给服务端，并且设置回调函数
   client.sendGoal(goal, &doneCallback, &activeCallback, &feedbackCallback);
   ros::spin();
   return 0;
}
修改 ~/catkin_ws/src/my_package/CMakeLists.txt
add_executable(command_server src/command_server.cpp)
add_executable(command_client src/command_client.cpp)
target_link_libraries(command_server ${catkin_LIBRARIES})
target_link_libraries(command_client ${catkin_LIBRARIES})
add_dependencies(command_server ${PROJECT_NAME}_EXPORTED_TARGETS)
add_dependencies(command_client ${PROJECT_NAME}_EXPORTED_TARGETS)
编译
-machine:~/catkin_ws$ catkin_make
运行
-machine:~$ roscore
-machine:~$ rosrun my_package command_server
-machine:~$ rosrun my_package command_client