【ROS总结】教程Actionlib——使用Execute Callback编写一个简单的行为服务器

前言

  所有的Actionlib教程已经在中文ROS页面服务器上翻译，有兴趣的可以查看总目录： http://wiki.ros.org/cn/actionlib_tutorials/Tutorials

描述

   这个教程涵盖了使用simple_action_server库来创建一个Fibonacci行为服务器。这个行为服务器教程会生成一个Fibonacci序列、目标的顺序序列、自身计算的反馈序列和最后序列的结果。

创建行为消息

开始编写行为之前，非常重要的事是定义目标，结果和反馈消息。行为消息会自动从.action'文件生成，对于更多行为文件信息查看actionlib文档。这个文件定义目标、结果和行为的反馈话题的类型和文本格式。使用你最喜欢的文本编辑器创建actionlib_tutorials/action/Fibonacci.action文件，并且用以下内容在该文件中添加：

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

为了在make进程中自动生成消息文件，需要在CMakeLists.txt中添加一些小的内容。

• 添加actionlib_msgs包作为参数到find_package这个宏中，就像这样（如果你使用catkin_create_package生成的CMakeLists.txt，这一行可能已经添加）：

find_package(catkin REQUIRED COMPONENTS actionlib_msgs)

• 注意CMake需要find_package actionlib_msgs (message_generation不需要明确的列出，因为以及被actionlib_msgs隐式的参考了).

• 使用add_action_files 宏来声明你想要生成的行为：

• add_action_files(
    DIRECTORY action
    FILES Fibonacci.action
  )

• 调用generate_messages宏，不要忘记依赖actionlib_msgs和其他消息包，例如std_msgs：
  

• generate_messages(
    DEPENDENCIES actionlib_msgs std_msgs  # 或其他包含消息的包
  )

  
  
  • catkin_package也指定了CATKIN_DEPEND到actionlib_msgs。message_runtime自动传递依赖。

现在遵循以下，会自动生成你的action文件的msg文件,并且可以查看结果。

$ cd ../.. # 返回到你的catkin工作空间的最顶层
$ 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

为了从这个文件手动生成消息文件，从actionlib_msgs包使用genaction.py脚本。

编写一个简单的服务器

代码

首先，使用你最喜欢的编辑器创建actionlib_tutorials/src/fibonacci_server.cpp，随后用以下内容填充：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>

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_;
  // create messages that are used to published feedback/result
  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);

    // publish info to the console for the user
    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]);

    // start executing the action
    for(int i=1; i<=goal->order; i++)
    {
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // publish the feedback
      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());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }


};


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

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

  return 0;
}

代码解释

现在，让我们分块解释代码。

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>

从实现简单行为中使用的行为库actionlib/server/simple_action_server.h。

#include <actionlib_tutorials/FibonacciAction.h>

这个包含了从以上Fibonacci.action文件中生成的行为消息。这是从 FibonacciAction.msg文件中自动生成的。对于更多关于消息定义的细节，查看 msg界面.

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_;
  // create messages that are used to published feedback/result
  actionlib_tutorials::FibonacciFeedback feedback_;
  actionlib_tutorials::FibonacciResult result_;

这些是行为类中受保护的变量。在创建行为的过程中，构造node handle并传递到行为服务器中。在行为的构造函数中构造行为服务器，并且关于行为服务器将会在以下讲述。创建反馈和结果消息用于在行为中发布。

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

在行为构造函数中，行为服务器会被创建。行为服务器会得到一个节点句柄（node handle）、行为名称和选择一个运行回调函数（executeCB）参数。在这个例子中，创建的行为服务器将回调函数（executeCB）作为参数。

 void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
  {

现在调用的executeCB函数以及在构造函数中创建。回调函数会传递一个指向目标消息的指针。 注意: 这是一个boost共享指针, 在目标消息类型最后附加一个给定的" ConstPtr".

  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);

    // publish info to the console for the user
    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]);

在行为中创建内部参数。在这个例程中，发布ROS_INFO来让用户指定行为正在运行。

   // start executing the action
    for(int i=1; i<=goal->order; i++)
    {
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);

一个行为服务器的一个重要组成部分是允许行为客户端请求取消当前目标执行。当一个客户端请求抢占当前目标是，行为服务器应该取消目标，随后执行重要的清理，然后调用函数setPreempted()，该函数会发出该行为已经被用户请求抢占信号。设置检查抢占请求服务器的等级到服务器系统。

      // publish the feedback
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep();
    }

这里，Fibonacci序列赋值给feedback变量，然后通过行为服务器提供的反馈频道发布出去。随后行为继续循环和发布反馈。

    if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }

一旦行为完成计算Fibonacci序列，会通知行为客户端操作设置成功。

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

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

  return 0;
}

最后main函数，创建行为然后spins节点。行为会运行并等待接收目标。

编译

在你的CMakeLists.txt添加如下行：

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${actionlib_tutorials_EXPORTED_TARGETS}
)

完整的 CMakeLists.txt如下：

cmake_minimum_required(VERSION 2.8.3)
project(actionlib_tutorials)

find_package(catkin REQUIRED COMPONENTS roscpp actionlib actionlib_msgs)
find_package(Boost REQUIRED COMPONENTS system)

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs
)

catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${actionlib_tutorials_EXPORTED_TARGETS}
)

运行行为服务器

在一个新终端中开启roscore

$ roscore

然后运行行为服务器：

$ rosrun actionlib_tutorials fibonacci_server

你可以看到类似如下输出：

[ INFO] 1250790662.410962000: Started node [/fibonacci], pid [29267], bound on [aqy], xmlrpc port [39746], tcpros port [49573], logging to [~/ros/ros/log/fibonacci_29267.log], using [real] time

想要检查你的行为运行是否正常的话，查看发布的话题列表：

$ rostopic list -v

你可以看到类似如下输出：

Published topics:
 * /fibonacci/feedback [actionlib_tutorials/FibonacciActionFeedback] 1 publisher
 * /fibonacci/status [actionlib_msgs/GoalStatusArray] 1 publisher
 * /rosout [rosgraph_msgs/Log] 1 publisher
 * /fibonacci/result [actionlib_tutorials/FibonacciActionResult] 1 publisher
 * /rosout_agg [rosgraph_msgs/Log] 1 publisher

Subscribed topics:
 * /fibonacci/goal [actionlib_tutorials/FibonacciActionGoal] 1 subscriber
 * /fibonacci/cancel [actionlib_msgs/GoalID] 1 subscriber
 * /rosout [rosgraph_msgs/Log] 1 subscriber

另外你可以查看节点：

$ rqt_graph

这个显示你的行为服务器正在发布反馈、状态和期望的通道结果，和目标订阅以及期望的目标取消通道。行为服务器启动并运行正常。

发送一个目标到行为服务器

对于使用你的行为的下一步，你需要使用Ctrl-C关闭行为服务器，然后编写一个简单的行为客户端.