ROS学习（三）ROS基础

ROS基础

四、ROS通信编程——动作编程

例：实现机器人洗盘子的动作，并实时反馈洗盘子的动作，盘子洗完后回复給客户端成功的命令。

1、自定义动作消息action文件

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

#定义目标信息
uint32 dishwasher_id #Specify which dishwasher we want to use
---
#定义结果信息
uint32 total_dishes_cleaned
---
#定义周期反馈的消息  洗盘子进度
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、编译
在yyj_ws 工作空间层级下生成可执行文件：

catkin_make

2、创建动作服务器

在learning_communicatio的src下创建DoDishes_server.cpp文件
编程逻辑：
（1）初始化ROS节点
（2）创建动作服务器实例
（3）启动服务器，等待动作请求
（4）在回调函数中完成动作服务功能的处理，并反馈进度信息
（5）动作完成，发送结束信息

#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;
}

3、创建动作客户端

在learning_communicatio的src下创建DoDishes_client.cpp文件
编程逻辑：
（1）初始化ROS节点
（2）创建动作客户端实例
（3）连接动作服务器
（4）发送动作目标
（5）根据不同类型的服务端反馈处理回调函数

#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;
}

4、添加编译选项

在 CMakeLists.txt 文件中设置编译代码
a、流程：
（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})

b、编译：
在yyj_ws 工作空间层级下生成可执行文件：

catkin_make

5、运行可执行文件

a、启动ROS Master
终端1：

roscore

b、启动client
终端2：

rosrun learning_communication DoDishes_client

c、启动server
终端3：

rosrun learning_communication DoDishes_server

如没把环境变量直接写进.bashrc，则每次打开新终端都需添加环境变量

source ~/yyj_ws/devel/setup.bash