ROS actionlib

创建和编译 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

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

创建文件 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;
}

在功能包根目录的 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。如：

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

测试结束后，在启动 server 节点的终端中按 Ctrl+C 暂时结束 server 节点，下面将演示创建一个 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;
}

启动 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。结果类似于下面：

如果在启动 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]
---