ROS 动作Action

1 背景知识

1.1 动作是什么

  ROS程序由多个功能组成；一个功能由多个节点完成；各节点的功能独立，节点与节点间通过不同的通信方式，实现数据的传输，完成功能。即总功能由分功能组成，各分功能由节点的子功能及节点间的通信完成。
  动作是一种类似于服务Server的通信方式，相较于服务通信，动作可以通过反馈获取动作任务的进度，还可以发送请求，终止动作任务。

1.2 动作的通信机制

  动作通信的底层是topic，动作将创建5个话题Topic：

     1. goal ：发布目标任务。
     2. cancel：请求取消任务。
     3. status：通知客户端任务状态:Pending，active，recalled，rejected，preempted，aborted，succeeded，lost
     4. feedback：周期反馈任务运行的监控数据。
     5. result：向客户端发送任务执行结果，只会发布一次。

  通过5个topic进行数据的通讯，达到异步处理事件的结果。与服务相似，服务同步，动作异步。

1.3 动作通信的特点

  动作通信是一种异步通信方式，在动作通信过程中，可以查看任务进度，也可以发送终止任务的请求。
  动作通信通过Actionlib库实现。

---

---

---

前提：ROS 创建工作空间和软件包
  当前所处软件包为beginner_tutorials，该软件包的文件系统结构如下图：

2 自定义动作Action

  动作的数据结构：由三组数据构成 ; 每组数据由仅包含三个破折号的一行为界，分为上、中、下 三部分，最上动作目标，中间为结果定义，下为动作反馈。例如：计算斐波那契数列，其动作文件Fibonacci.ation 如下：

#上→动作目标
int32 order
- - -
# 中→结果定义
int32[] sequence
- - -
# 下→反馈定义
int32[] sequence

2.1创建action文件夹（专放消息）， 并编辑.action文件**

$ roscd beginner_tutorials        
$ mkdir action                    
$ cd action
$ touch Fibonacci.action

  编辑Fibonacci.action

int32 order
---
int32[] sequence
---
int32[] sequence

2.2 添加action依赖

  编辑 ‘软件包’ 下的package.xml 文件(修改后)， 添加：

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

  编辑‘软件包’ 下CMakeLists.txt 文件(修改后):

# 需添加部分1：
find_package(catkin REQUIRED COMPONENTS actionlib_msgs)

# 需修改部分2：     
#            1)删除前面的# 符号  
#            2)替换Action1.action，Action2.action为 Fibonacci.action
 add_action_files(
  FILES
  Fibonacci.action
 )

# 需修改部分3： 增加
generate_messages(
  DEPENDENCIES
  actionlib_msgs std_msgs
)

2.3 编译，使.action能被转换为C++、Python和其他语言的源代码

  要使用自定义动作，需将.action 文件编译为 .h 文件，再引入到程序中

$ cd ~/catkin_ws
$ catkin_make

  编译后，生成相应的头文件和.msg文件。
  头文件位置:./devel/include/beginner_tutorials/

	 1. FibonacciAction.h: #include 下面所有头文件
	 2. FibonacciResult.h
	 3. FibonacciGoal.h
	 4. FibonacciFeedback.h
	 5. FibonacciActionFeedback.h
	 6. FibonacciActionGoal.h
	 7. FibonacciActionResult.h

  .msg文件位置：:./develshare/beginner_tutorials/msg/

	 1. FibonacciAction.msg
	 2. FibonacciFeedback.msg
	 3. FibonacciGoal.msg
	 4. FibonacciResult.msg
	 5. FibonacciActionFeedback.msg
	 6. FibonacciActionGoal.msg
	 7. FibonacciActionResult.msg

  查看是否动作是否创建成功（不同于服务和话题，ros1中没有指令专门查看动作）

 $ rosmsg list
 ....
beginner_tutorials/FibonacciAction
beginner_tutorials/FibonacciActionFeedback
beginner_tutorials/FibonacciActionGoal
beginner_tutorials/FibonacciActionResult
beginner_tutorials/FibonacciFeedback
beginner_tutorials/FibonacciGoal
beginner_tutorials/FibonacciResult
 .....

---

3 动作客户端

1. 初始化节点(命名，唯一)
2. 实例化句柄
3. 实例动作对象
4. 请求动作，绑定三个回调函数（数据处理的主要部分)
   4.1 任务开始的回调函数
   4.2 任务进度过程中的进度反馈函数
   4.3 任务完成结束反馈函数

动作客户端 → 动作服务端发送两种数据：goal和cancel

  goal数据：请求时，使用变量承载，完成数据的打包。
  cancel数据：用于中止通信，不需要变量承载，只要调相应的接口函数

3.1 编辑fibonacci_client.cpp

  在 软件包 的src文件中，创建文件fibonacci_client.cpp

$ touch fibonacci_client.cpp

  编辑fibonacci_client.cpp文件：

#include "ros/ros.h"
#include <actionlib/client/simple_action_client.h>//actionlib提供Action服务器
#include "beginner_tutorials/FibonacciAction.h" //封装通信数据


//任务完成结束的回调函数 ，只会被执行一次
void doneCb(const actionlib::SimpleClientGoalState &state, const beginner_tutorials::FibonacciResultConstPtr &result)
{
    ROS_INFO("任务完成");
    ros::shutdown();
    //结束spin()的轮询工作
}

// 任务开始的回调函数
void activeCb()
{
    ROS_INFO("开始任务");
}

//任务进度过程中的进度反馈函数 
void feedbackCb(const beginner_tutorials::FibonacciFeedbackConstPtr &feedback)
{
    ROS_INFO(" 完成的进度 : %d ", feedback->count_percent);
}


int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    //ros不支持中文，设置ros支持中文
    ros::init(argc, argv, "fibonacci_client");
    //初始化 ROS 节点 ，节点名需唯一
    ros::NodeHandle nh; 
    //实例化句柄
    
    actionlib::SimpleActionClient<beginner_tutorials::FibonacciAction> fibonacciActionClient("Fibonacc", true);
    //实例化动作对象（动作名，是否自动循环线程）
    //不自动自动循环线程，需要手动创建线程 boost::thread spin_thread(&spinThread); 

    ROS_INFO("等待服务器开始运行");
    fibonacciActionClient.waitForServer();//无线等待服务端上线
    ROS_INFO("服务器开始运行");
    
    beginner_tutorials::FibonacciGoal goal;
    goal.order= 5;
    // 打包数据：创建一个动作的goal
    
    fibonacciActionClient.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
     // 发送action的goal给服务器端，并且设置任务完成，任务开始，任务反馈的回调函数

    //设为同步，等待返回。
    bool finished_before_timeout = fibonacciActionClient.waitForResult(ros::Duration(30.0));//
    if (!finished_before_timeout)
    {
        ROS_INFO("任务未在时限内完成");
    }
    return 0;
}

  客户端的构造函数原型：

SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)  
 : cur_simple_state_(SimpleGoalState::PENDING)  
 {  
   		initSimpleClient(n, name, spin_thread);  
 } 
//ros::NodeHandle & n: 函数句柄。 不指定时，系统会给设定默认的节点句柄。
//const std::string & name ：指定client的topic

//bool spin_thread = true ：表征着构造函数中是否需要使用额外线程 ,要与服务端相对
//true == spin_thread 时，采用系统线程，客户端 auto_start = false;
//false ==  spin_thread 时，需调用手动建线程，客户端 auto_start = true.

  客户端的sendGoal()原型：

template<class ActionSpec>  
void SimpleActionClient<ActionSpec>::sendGoal(const Goal & goal,  
  SimpleDoneCallback done_cb,  //任务结束回调
  SimpleActiveCallback active_cb,  //任务开始回调
  SimpleFeedbackCallback feedback_cb)  //任务反馈回调

//回调函数的原型如下：
//任务结束回调:
typedef boost::function<void (const SimpleClientGoalState & state,  
   const ResultConstPtr & result)> SimpleDoneCallback; 
//任务开始回调
typedef boost::function<void ()> SimpleActiveCallback;
//任务反馈回调
typedef boost::function<void (const FeedbackConstPtr & feedback)> SimpleFeedbackCallback;

3.2 编译，生成可执行文件

  CMakeList.txt文件

# 增加软件包  actionlib
find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)

add_executable(fibonacci_client src/fibonacci_client.cpp)
target_link_libraries( fibonacci_client ${catkin_LIBRARIES})
add_dependencies(fibonacci_client beginner_tutorials_gencpp)

  package.xml

#  添加依赖
<build_depend>actionlib</build_depend>
<exec_depend>actionlib_msgs</exec_depend>

.action文件依赖： actionlib_msgs

使用action的软件包的依赖 ： actionlib_msgs actionlib

  在工作空间下编译, 生成可执行文件。

$ cd catkin_ws
$ catkin_make
$ source ./devel/setup.bash

  可执行文件放置于：devel/lib/beginner_tutorials。

---

4 动作服务端

1. 初始化 ROS 节点
2. 实例化句柄
3. 实例化动作服务端对象，动作，动作的回调函数
4. 处理的动作请求（数据处理的主要部分）

动作服务端 →动作客户端发送三种数据：result，feedback， status
    result数据：服务完成时发送，需要变量承载，完成数据的打包。
    feedback数据：反馈时发送，需要变量承载，完成数据的打包。
    status数据数据：任务开始时，不需要变量承载。

4.1 编辑fibonacci_server.cpp

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>//actionlib提供Action服务器
#include "beginner_tutorials/FibonacciAction.h" //封装通信数据

// 收到 goal 后调用的回调函数
void GoalCallBack(const beginner_tutorials::FibonacciGoalConstPtr& goal, actionlib::SimpleActionServer<beginner_tutorials::FibonacciAction> * as)
{
    ros::Rate r(1);
    bool success = true;
    //处理数据
    beginner_tutorials::FibonacciFeedback feedback;
    feedback.sequence.clear();
    feedback.sequence.push_back(0);
    feedback.sequence.push_back(1);

    for(int i=1; i<= goal->order; i++)    //按照一定的频率上报feedback
    {
        if (as->isPreemptRequested() || !ros::ok())
        {
           ROS_INFO("动作被取消");
           as->setPreempted();
           success = false;
           break;
        }
        feedback.sequence.push_back(feedback.sequence[i] + feedback.sequence[i-1]);
        feedback.count_percent  = i * 20;
        ROS_INFO("任务完成到%d %,数列的值%d ",  feedback.count_percent,feedback.sequence.at(i));
        as->publishFeedback(feedback);
        r.sleep();
    }

    // 当action完成后，向客户端返回结果
    beginner_tutorials::FibonacciResult result;
    if(success)
    {
        ROS_INFO("任务结束");
        result.sequence = feedback.sequence;
        as->setSucceeded();
    }
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "fibonacci_server");
    //初始化节点，节点名称需唯一
    ros::NodeHandle n;
    //实例化句柄

    actionlib::SimpleActionServer<beginner_tutorials::FibonacciAction> server(n, "Fibonacc", boost::bind(&GoalCallBack, _1, &server), false);
    // 实例化服务对象 （句柄，动作名，回调函数，是否自动循环线程）
    // 不自动循环线程，需手动启动
    server.start();
    ros::spin();
    return 0;
}

服务端的构造函数原型：

SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start);  
SimpleActionServer(std::string name, ExecuteCallback execute_callback, bool auto_start); // 使用默认节点句柄  
SimpleActionServer(ros::NodeHandle n, std::string name, bool auto_start); // 省去了回调函数  
SimpleActionServer(std::string name, bool auto_start); // 使用默认节点句柄且省去了回调函数 
//参数解释：
//ExecuteCallback execute_callback 的作用：通过收到来自client的goal请求信号，产生响应向client发送feedback、state、result
//ExecuteCallback execute_callback 的原型：
typedef boost::function<void (const GoalConstPtr &)> ExecuteCallback;
//const GoalConstPtr& :  接收从client传来的goal信息

//bool auto_start： server节点是否自启动 ，是否自启动要与客户端相对
//false == auto_start 时，手动启动，客户端的spin_thread = true
//true == auto_start 时.自动启动,需调用  obj.start(); 客户端的spin_thread = false

4.2 编译，生成可执行文件

  CMakeList.txt文件

# 增加软件包  actionlib
find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)

add_executable(fibonacci_client src/fibonacci_server.cpp)
target_link_libraries( fibonacci_client ${catkin_LIBRARIES})
add_dependencies(fibonacci_client beginner_tutorials_gencpp)

  package.xml

#  添加依赖
<build_depend>actionlib</build_depend>
<exec_depend>actionlib_msgs</exec_depend>

  在工作空间下编译, 生成可执行文件。

$ cd catkin_ws
$ catkin_make
$ source ./devel/setup.bash

  可执行文件放置于：devel/lib/beginner_tutorials。

---

5 验证

打开一终端，启动roscore

$ roscore

打开另一终端，启动动作服务器

rosrun beginner_tutorials fibonacci_server

再打开另一终端，启动动作客户端

rosrun beginner_tutorials fibonacci_client 

结果

打开另一终端，运行rqt_graph，查看当前节点间通信

$ rosrun rqt_graph rqt_graph

当前软件包的文件系统结构：（红框为当前教程新增）