ROS actionlib学习（二）

　　在ROS actionlib学习（一）中的例子展示了actionlib最基本的用法，下面我们看一个稍微实际一点的例子，用actionlib计算斐波那契数列，并发布反馈（feedback）和结果（result）。斐波那契数列指的是这样一个数列：

1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233，377，610，987，1597，2584，4181，6765，10946，17711，28657，46368........

　　这个数列从第3项开始，每一项都等于前两项之和。

　　首先在action文件中定义goal、result、feedback，其中goal是斐波那契数列的阶数，result为最终生成的数列，feedback为当前一步的中间结果（也是一个数列）。

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

　　然后按照教程SimpleActionServer(ExecuteCallbackMethod)中所述的步骤，修改CMakeLists.txt以及package.xml文件，编译成功后会生成相应的消息文件以及头文件。

$ 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

　　下面编写服务端程序，用于处理客户端发送的请求。

#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();  // Explicitly start the action server
  }
 
  ~FibonacciAction(void)
  {
  }
 
  void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r();
    bool success = true;
 
    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back();
    feedback_.sequence.push_back();
 
    // 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[], feedback_.sequence[]);
 
    // start executing the action
    for(int i=; 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(); // signals that the action has been preempted by user request
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-]);
      // 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 ;
}

　　客户端程序用于发送计算请求，服务器收到请求后会生成20阶的斐波那契数列。

#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");
// the action client is constructed with the server name and the auto spin option set to true.
  actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true); //The action client is templated on the action definition, specifying what message types to communicate to the action server with
 
  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start (Since the action server may not be up and running, the action client will wait for the action server to start before continuing)
  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 = ;
  ac.sendGoal(goal); // the goal value is set and sent to the action serve
 
  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); // The timeout on the wait is set to 30 seconds, this means after 30 seconds the function will return with false if the goal has not finished.
 
  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 ;
}

　　修改CMakeLists.txt后使用catkin_make编译上面两个程序。运行roscore开启ROS主节点，然后在两个新终端中分别输入下面命令分别运行server和client：

rosrun actionlib_tutorials fibonacci_server  
 
rosrun actionlib_tutorials fibonacci_client

　　可以通过 rostopic echo /fibonacci/feedback 和 rostopic echo /fibonacci/result 命令查看斐波那契数列计算的反馈和结果：

　　最终的计算结果result如下：

---
header:
  seq: 1
  stamp: 1250813759950015000
  frame_id:
status:
  goal_id:
    stamp: 1250813739949752000
    id: 1250813739949752000
  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)

　　在一个新终端中输入下面命令，运行另一个客户端节点（重命名为client2）。如果在原fibonacci_client节点的请求还没完成时就运行client2，那么服务将会被client2抢占。

rosrun actionlib_tutorials fibonacci_client __name:=client2

　　效果如下图所示，如果前一个client发送的请求还没计算完成，新的goal就到达，server会重新开始计算数列：

　　SimpleActionServer在ActionServer类上实现了single goal policy，就是在某一时刻只能有一个goal是处于active状态，并且新的goal可以抢占先前的goal：

• Only one goal can have an active status at a time
• New goals preempt previous goals based on the stamp in their GoalID field (later goals preempt earlier ones)
• An explicit preempt goal preempts all goals with timestamps that are less than or equal to the stamp associated with the preempt
• Accepting a new goal implies successful preemption of any old goal and the status of the old goal will be changed automatically to reflect this

参考：

actionlib-Tutorials

ROS actionlib学习（一）