ROS与C++入门教程-actionlib-编写简单的action服务器(Execute Callback)

ROS与C++入门教程-actionlib-编写简单的action服务器(Execute Callback)

说明：

介绍如何编写简单带Execute Callback的action服务器
介绍如何使用simple_action_server库来实现action服务器
介绍实现生成Fibonacci数列，goal是数列序，feedback是计算中数列，result是计算完成的数列。

参考：

http://wiki.ros.org/actionlib
http://wiki.ros.org/actionlib/DetailedDescription
https://github.com/ros/common_tutorials/tree/indigo-devel/actionlib_tutorials

创建包

cd ~/catkin_ws/src
catkin_create_pkg actionlib_tutorials roscpp actionlib message_generation std_msgs actionlib_msgs

创建Action消息

action有三种消息：goal, result, and feedback
他们通过.action文件自动生成的。想了解更多查阅actionlib wiki
创建Fibonacci.action：参考代码

cd ~/catkin_ws/src/actionlib_tutorials/src
mkdir action
touch Fibonacci.action
vim Fibonacci.action

代码如下：

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

编译：

更改CMakeLists.txt文件
增加actionlib_msgs支持，需要有如下行，没有则增加：

find_package(catkin REQUIRED COMPONENTS actionlib_msgs)

增加add_action_files：

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

调用generate_messages

generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs  # Or other packages containing msgs
)

增加actionlib_msgs到catkin_package

catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)

编译并查看结果：

$ cd ~/catkin_ws
$ 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文件来手工生成上面的文件。

编写action服务器端

创建actionlib_tutorials/src/fibonacci_server.cpp，可参考代码：

cd ~/catkin_ws/actionlib_tutorials/src/
touch fibonacci_server.cpp
vim 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 是action库，用来执行简单的actions
代码：

#include <actionlib_tutorials/FibonacciAction.h>

解释：这个头文件是由Fibonacci.action自动生成。
代码：

protected:

  ros::NodeHandle nh_;
  // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; 
  std::string action_name_;
  // create messages that are used to published feedback/result
  actionlib_tutorials::FibonacciFeedback feedback_;
  actionlib_tutorials::FibonacciResult result_;

解释：创建多个私有变量，节点实例化变量，actionlib实例化变量，保存反馈的feedback_变量，保存结果的result_变量
代码：

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

解释：
- 构造函数，并初始化函数，创建action服务器。需要提供action名称，节点实例变量，可选的executeCB变量。
代码：

void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)

解释：
- executeCB函数引用在构造函数中创建，回调函数传递boost的共享指针类型ConstPtr的goal作为参数
代码：

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

解释：
- action服务器重要功能是可以接受客户端段取消目前目标的执行。
- 当客户端请求当前取消的目标优先处理，action服务器就会清除相关内容，并调用setPreempted()函数。
- 也可以设置优先处理检查的频率，action服务器就自动根据这个频率实现检查并做对应处理。
代码：

// 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里，并发布这个feedback，然后进入新一轮的循环。
代码：

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

解释：
- 当计算完成，action服务器就会通知客户端已经完成，并发送最后的结果。
代码：

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

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

  return 0;
}

解释：
- main函数实现开始action，并启动线程处理，运行和等待去接受goal值传入。

编译

更改 CMakeLists.txt，增加：

cd ~/catkin_ws/actionlib_tutorials/
vim 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}
)

完整版本如下：

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

编译：

cd ~/catkin_ws
catkin_make

运行

新终端，运行roscore

$ roscore

新终端，执行action服务器

$ 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

效果：

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号