ROS actionlib

Table of Contents

1 actionlib简介

我们知道ROS中的Services是同步的,适应于一些耗时很少的操作,如查询状态信息,对设备进行配置等。对于耗时的操作(如移动到某个位置),ROS中Actions更加合适(Actions的实现以Topics为基础)。

The actionlib package provides tools to create servers that execute long-running goals that can be preempted. It also provides a client interface in order to send requests to the server.

ROS_action_interface.png

Figure 1: Action接口

ROS_action_server_states_detailed.png

Figure 2: Action服务端的状态转移图

ROS_action_client_state_transitions.png

Figure 3: Action客户端的状态转移图

参考:http://wiki.ros.org/actionlib/DetailedDescription

2 Actions实例

下面例子摘自:http://wiki.ros.org/actionlib/Tutorials

这个实例的场景为:
This tutorial covers using the simple_action_server library to create a Fibonacci action server. This example action server generates a Fibonacci sequence, the goal is the order of the sequence, the feedback is the sequence as it is computed, and the result is the final sequence.

后面将介绍Goal, Feedback, Result这三个概念。

2.1 准备工作:创建工作区和功能包

创建和编译workspace:

$ source /opt/ros/indigo/setup.bash
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ~/catkin_ws/
$ catkin_make

创建功能包learning_actionlib和子目录action:

$ source devel/setup.bash
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_actionlib actionlib message_generation roscpp rospy std_msgs actionlib_msgs
$ mkdir action

2.2 创建和编译action定义文件

Action由一个.action文件来定义,它包含三个部分:Goal, Feedback, Result,各个部分之间用“—”分隔。

2.2.1 Goal, Feedback, Result

Goal
To accomplish tasks using actions, we introduce the notion of a goal that can be sent to an ActionServer by an ActionClient. In the case of moving the base, the goal would be a PoseStamped message that contains information about where the robot should move to in the world. For controlling the tilting laser scanner, the goal would contain the scan parameters (min angle, max angle, speed, etc).
Feedback
Feedback provides server implementers a way to tell an ActionClient about the incremental progress of a goal. For moving the base, this might be the robot's current pose along the path. For controlling the tilting laser scanner, this might be the time left until the scan completes.
Result
A result is sent from the ActionServer to the ActionClient upon completion of the goal. This is different than feedback, since it is sent exactly once. This is extremely useful when the purpose of the action is to provide some sort of information. For move base, the result isn't very important, but it might contain the final pose of the robot. For controlling the tilting laser scanner, the result might contain a point cloud generated from the requested scan.

2.2.2 Fibonacci.action

创建文件action/Fibonacci.action,内容如下:

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

如果CMakeList.txt中没有下面内容,则增加它:

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs
)

catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)

2.2.3 编译生成msg文件和头文件

进入到工作区根目录,用 catkin_make 进行编译。如:

$ cd ~/catkin_ws/
$ catkin_make

编译成功后,会由.action文件生成对应的.msg文件,再由.msg文件生成对应的头文件。如:

$ ls devel/share/learning_actionlib/msg/
FibonacciActionFeedback.msg  FibonacciActionResult.msg  FibonacciResult.msg
FibonacciActionGoal.msg      FibonacciFeedback.msg
FibonacciAction.msg          FibonacciGoal.msg
$ ls devel/include/learning_actionlib/
FibonacciActionFeedback.h  FibonacciActionResult.h  FibonacciResult.h
FibonacciActionGoal.h      FibonacciFeedback.h
FibonacciAction.h          FibonacciGoal.h

2.3 准备server节点的代码

创建文件learning_actionlib/src/fibonacci_server.cpp,其内容如下:

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>   /* 由actionlib提供的头文件 */
#include <learning_actionlib/FibonacciAction.h>      /* 由.action文件编译生成的头文件 */

class FibonacciAction
{
protected:

  ros::NodeHandle nh_;
  // NodeHandle instance must be created before this line. Otherwise strange error may occur.
  actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_;
  std::string action_name_;
  // create messages that are used to published feedback/result
  learning_actionlib::FibonacciFeedback feedback_;
  learning_actionlib::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 learning_actionlib::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(ros::this_node::getName());
  ros::spin();

  return 0;
}

2.4 编译并测试server节点

在功能包根目录的CMakefile.txt中,增加下面内容;

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${learning_actionlib_EXPORTED_TARGETS}
)

进入到工作区根目录,用 catkin_make 进行编译。如:

$ cd ~/catkin_ws/
$ catkin_make

编译正常结束后,会生成对应的可执行程序"/home/cig01/catkin_ws/devel/lib/learning_actionlib/fibonacci_server"。

运行roscore和server节点。如:

$ roscore &
$ . ./devel/setup.bash
$ rosrun learning_actionlib fibonacci_server

打开另外一个终端,用 rostopic list -v 检查对应的Topics是不是正常地启动了。如:

$ rostopic list -v

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

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

rqt_graph 可以看到相应的Nodes和Topics。如:

ROS_action_server_rqt_graph.jpg

Figure 4: rqt_graph查看action server

说明:上图中Topic "/statistics"和Node "/rqt_gui_py_node_3730"是由 rqt_graph 启动的。

测试结束后,在启动server节点的终端中按Ctrl+C暂时结束server节点,下面将演示创建一个client节点。

2.5 准备client节点的代码

创建文件learning_actionlib/src/fibonacci_client.cpp,其内容如下:

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

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

  // create the action client
  // true causes the client to spin its own thread
  actionlib::SimpleActionClient<learning_actionlib::FibonacciAction> ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time

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

  //wait for the action to return
  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.");

  //exit
  return 0;
}

2.5.1 编译client节点

在功能包根目录的CMakefile.txt中,增加下面内容;

add_executable(fibonacci_client src/fibonacci_client.cpp)

target_link_libraries(
  fibonacci_client
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_client
  ${learning_actionlib_EXPORTED_TARGETS}
)

进入到工作区根目录,用 catkin_make 进行编译。如:

$ cd ~/catkin_ws/
$ catkin_make

编译正常结束后,会生成对应的可执行程序"/home/cig01/catkin_ws/devel/lib/learning_actionlib/fibonacci_client"。

2.6 测试server和client

启动server节点,在另一终端中启动client节点。如:

$ . ./devel/setup.bash
$ rosrun learning_actionlib fibonacci_server
[ INFO] [1430885667.685616856]: /fibonacci: Executing, creating fibonacci sequence of order 20 with seeds 0, 1
[ INFO] [1430885687.685983763]: /fibonacci: Succeeded
$ . ./devel/setup.bash
$ rosrun learning_actionlib fibonacci_client
[ INFO] [1430885667.475625124]: Waiting for action server to start.
[ INFO] [1430885667.684697617]: Action server started, sending goal.
[ INFO] [14303885687.686609898]: Action finished: SUCCEEDED

当client节点还没有结束时,运行 rqt_graph ,查看Nodes和Topics。结果类似于下面:

ROS_action_rqt_graph.jpg

Figure 5: rqt_graph查看action server/client

如果在启动client节点前先运行 rostopic echo /fibonacci/result ,则在client运行结束后,会得到下面输出:

$ . ./devel/setup.bash
$ rostopic echo /fibonacci/result
header: 
  seq: 2
  stamp: 
    secs: 1430887629
    nsecs: 956256438
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1430887609
      nsecs: 955739472
    id: /test_fibonacci-1-1430887609.955739472
  status: 3
  text: ''
result: 
  sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946]
---

如果在启动client节点前先运行 rostopic echo /fibonacci/feedback ,则在client运行结束后,会得到类似于下面输出:

$ . ./devel/setup.bash
$ rostopic echo /fibonacci/feedback
header: 
  seq: 60
  stamp: 
    secs: 1430888155
    nsecs: 804780373
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1430888155
      nsecs: 804482578
    id: /test_fibonacci-1-1430888155.804482578
  status: 1
  text: This goal has been accepted by the simple action server
feedback: 
  sequence: [0, 1, 1]
---
header: 
  seq: 61
  stamp: 
    secs: 1430888156
    nsecs: 804956818
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1430888155
      nsecs: 804482578
    id: /test_fibonacci-1-1430888155.804482578
  status: 1
  text: This goal has been accepted by the simple action server
feedback: 
  sequence: [0, 1, 1, 2]
---
......
header: 
  seq: 79
  stamp: 
    secs: 1430888174
    nsecs: 805142443
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1430888155
      nsecs: 804482578
    id: /test_fibonacci-1-1430888155.804482578
  status: 1
  text: This goal has been accepted by the simple action server
feedback: 
  sequence: [0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946]
---

Author: cig01

Created: <2015-05-08 Fri 00:00>

Last updated: <2016-05-29 Sun 13:51>

Creator: Emacs 25.1.1 (Org mode 9.0.7)