ROS学习笔记：actionlib

在任何大型的基于ROS的系统中，都有这样的情况:有人想向某个节点发送请求，以执行某些任务，并接收对请求的应答。这可以通过ROS服务来实现。但是，在某些情况下，如果服务需要很长时间执行，用户可能希望在执行过程中取消请求，或者得到关于请求进展情况的定期反馈。actionlib包提供了创建服务器的工具，这些服务器执行可被抢占的长期目标。它还提供了一个客户端接口，以便向服务器发送请求。

actionlib接口不但可以调度任务的执行，而且具备中断任务、任务状态跟踪与周期性状态反馈、执行过程中间结果反馈的能力。

ActionClient与 ActionServer的交互设计

actionlib接口定义了ActionClient(任务请求的客户端)与ActionServer(任务调度的服务器端)。

Goal
在移动底盘的情况下，目标将是一个包含关于机器人应该移动到世界何处的信息的、具有固定功能的信息。为了控制倾斜激光扫描仪，目标将包含扫描参数(最小角度，最大角度，速度等)。

Feedback
反馈为服务器实现者提供了一种方式来告诉一个ActionClient关于一个目标的渐进进展。对于移动底盘，这可能是机器人在路径上的当前姿态。为了控制倾斜的激光扫描仪，这可能是在扫描完成之前的时间。

Result
在完成目标后，将结果从ActionServer发送到ActionClient。这与反馈不同，因为它只发送一次。当行动的目的是提供某种信息时，这是非常有用的。对于移动底盘，其结果并不十分重要，但它可能包含机器人的最终姿态。为了控制倾斜的激光扫描仪，结果可能包含从所请求的扫描产生的点云。

写一个简单的服务器和客户端

1 .action文件

创建名为Fibonacci.action文件。该.action文件具有目标定义，随后的结果定义，随后通过反馈定义，与由3连字号（分隔每个部分---）。这些文件放在包的./action目录中，看起来非常类似于服务的.srv文件。

#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence

修改CMakeList.txt文件

find_package(catkin REQUIRED COMPONENTS actionlib std_msgs message_generation) 
add_action_files(DIRECTORY action FILES Fibonacci.action)
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

修改package.xml文件

<exec_depend>message_generation</exec_depend>

编译工作空间

$ cd ../.. # Go back to the top level of your catkin workspace
$ catkin_make
$ ls devel/share/actionlib_tutorials/msg/
FibonacciActionFeedback.msg  FibonacciAction.msg        FibonacciFeedback.msg
FibonacciResult.msg          FibonacciActionGoal.msg    FibonacciActionResult.msg  FibonacciGoal.msg
$ ls devel/include/actionlib_tutorials/
FibonacciActionFeedback.h  FibonacciAction.h        FibonacciFeedback.h  FibonacciResult.h
FibonacciActionGoal.h      FibonacciActionResult.h  FibonacciGoal.h

2.创建服务器

创建一个名为fibonacci_server的C++文件。

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>//实现简单行为使用的行为库
#include <actionlib_tutorials/FibonacciAction.h>//包含了Fibonacci.action文件中生成的行为消息

class FibonacciAction
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  std::string action_name_;
  // 创建用作反馈/结果的消息
  actionlib_tutorials::FibonacciFeedback feedback_;
  actionlib_tutorials::FibonacciResult result_;

public:

  FibonacciAction(std::string name) :
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
  }

  ~FibonacciAction(void)
  {
  }

  void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    // 将信息映射到屏幕上
    ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);

    // 开始运行行为
    for(int i=1; i<=goal->order; i++)
    {
      // 检查客户端是否未请求抢占
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        //将行为状态设置为preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // 发布反馈
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep();
    }

    if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // 将行为状态设置为succeeded
      as_.setSucceeded(result_);
    }
  }


};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "fibonacci");

  FibonacciAction fibonacci("fibonacci");
  ros::spin();

  return 0;
}

3.创建客户端

创建fibonacci_client的C++文件

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <actionlib_tutorials/FibonacciAction.h>

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_fibonacci");

  // 创建行为客户端
  // true会开启客户端创建自己的线程
  actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  // 等待服务器开启
  ac.waitForServer(); //will wait for infinite time

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  actionlib_tutorials::FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal);

  // 等待行为返回
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else
    ROS_INFO("Action did not finish before the time out.");

  return 0;
}

启动服务器和客户端节点

$ roscore
$ rosrun actionlib_tutorials fibonacci_server
$ rosrun actionlib_tutorials fibonacci_client