ROS入门（五）：ROS通信编程（动作编程）

参考：《ROS机器人开发实践》
注：在 catkin_ws 工作空间下的功能包 learning_communication 下继续实现通信编程

一、动作通讯模型

二、动作编程实现

1、服务编程流程：

1、创建服务器;
2、创建客户端;
3、添加编译选项;
4、运行可执行文件;

2、自定义动作文件action

a、在learning_communication下创建action目录，在action目录下创建DoDishes.action文件

#Define the goal
uint32 dishwasher_id #Specify which dishwasher we want to use
---
#Define the result
uint32 total_dishes_cleaned
---
#Define a feedback message
float32 percent_complete

b、在package.xml中添加功能包依赖

  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend>

c、在CMakeLists.txt中添加编译选项

find_package(catkin REQUIRED COMPONENTS
...
  actionlib_msgs
  actionlib
)

add_action_files(DIRECTORY action FILES DoDishes.action)

generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

d、编译
在 catkin_ws 工作空间下生成可执行文件：

catkin_make

3、创建动作服务器

a、在如下目录下创建DoDishes_server.cpp文件：

b、DoDishes_server.cpp文件代码如下：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionServer<learning_communication::DoDishesAction> Server;

// 收到action的goal后调用该回调函数
void execute(const learning_communication::DoDishesGoalConstPtr& goal, Server* as)
{
   ros::Rate r(1);
   learning_communication::DoDishesFeedback 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 n;

   // 定义一个服务器
   Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
   
   // 服务器开始运行
   server.start();

   ros::spin();

   return 0;
}

c、实现逻辑
（1）初始化ROS节点；
（2）创建动作服务器实例；
（3）启动服务器，等待动作请求；
（4）在回调函数中完成动作服务功能的处理，并反馈进度信息；
（5）动作完成，发送结束信息；

4、创建动作客户端

a、在如下目录下创建DoDishes_client.cpp文件：

b、DoDishes_client.cpp文件代码如下：

#include <actionlib/client/simple_action_client.h>
#include "learning_communication/DoDishesAction.h"

typedef actionlib::SimpleActionClient<learning_communication::DoDishesAction> Client;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
       const learning_communication::DoDishesResultConstPtr& result)
{
   ROS_INFO("Yay! The dishes are now clean");
   ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
   ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const learning_communication::DoDishesFeedbackConstPtr& 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
   learning_communication::DoDishesGoal goal;
   goal.dishwasher_id = 1;

   // 发送action的goal给服务器端，并且设置回调函数
   client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

   ros::spin();

   return 0;
}

c、实现逻辑
（1）初始化ROS节点；
（2）创建动作客户端实例；
（3）连接动作服务器；
（4）发送动作目标；
（5）根据不同类型的服务端反馈处理回调函数

5、添加编译选项

编译方法

在 CMakeLists.txt 文件中进行编译代码：

（1）设置需要编译的代码和生成的可执行文件；
（2）设置链接库；
（3）设置依赖；

具体操作

打开learning_communication功能包中的 CMakeLists.txt 文件，添加如下代码：

add_executable(DoDishes_client src/DoDishes_client.cpp)
target_link_libraries(DoDishes_client ${catkin_LIBRARIES})
add_dependencies(DoDishes_client ${${PROJECT_NAME}_EXPORTED_TARGETS})

add_executable(DoDishes_server src/DoDishes_server.cpp)
target_link_libraries(DoDishes_server ${catkin_LIBRARIES})
add_dependencies(DoDishes_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

生成

在 catkin_ws 工作空间下生成可执行文件：

catkin_make

6、运行可执行文件

注意每次打开新终端添加环境变量：（需要手动添加的话）

source ~/test/catkin_ws/devel/setup.bash

a、请ROS Master
新打开终端：

roscore

b、启动client
新打开终端：

rosrun learning_communication DoDishes_client

c、启动server
新打开终端：

rosrun learning_communication DoDishes_server