下面這個例子將展示用actionlib來計算隨機變量的均值和標準差。首先在action文件中定義goal、result和feedback的數據類型，其中goal為樣本容量，result為均值和標準差，feedback為樣本編號、當前樣本數據、均值和標準差。

#goal definition
int32 samples
---
#result definition
float32 mean
float32 std_dev
---
#feedback
int32 sample
float32 data
float32 mean
float32 std_dev

　　按照之前例程中的步驟修改CMakeLists.txt後進行編譯，生成相關的消息文件和頭文件。

Writing a Simple Server

　　參考 C++ SimpleActionServer 文檔，SimpleActionServer類的構造函數有多種重載形式：

SimpleActionServer (std::string name, ExecuteCallback execute_callback, bool auto_start)
 
SimpleActionServer (std::string name, bool auto_start)

SimpleActionServer (ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool
 
 auto_start)

SimpleActionServer (ros::NodeHandle n, std::string name, bool auto_start)

　　在Writing a Simple Action Server using the Execute Callback的例子中，SimpleActionServer就使用了上面第3種構造函數，參數為節點句柄、action名稱以及ExecuteCallback回調函數。在本例中沒有使用這種來創建action server，而是在action server創建後通過註冊 goal callback 和 preempt callback這兩個回調函數來處理事件。

void  registerGoalCallback (boost::function< void()> cb)     // Allows users to register a callback to be invoked when a new goal is available
 
void  registerPreemptCallback (boost::function< void()> cb)  // Allows users to register a callback to be invoked when a new preempt request is available

　　服務端代碼averaging_server.cpp如下：

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h>  // actionlib/server/simple_action_server.h is the action library used from implementing simple actions
#include <actionlib_tutorials/AveragingAction.h>   // This includes action message generated from the Averaging.action file

class AveragingAction
{
public:
    
  AveragingAction(std::string name) : 
    as_(nh_, name, false),
    action_name_(name)
  {
    //register the goal and feeback callbacks
    as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));

    //subscribe to the data topic of interest
    sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
    as_.start();
  }

  ~AveragingAction(void)
  {
  }

  void goalCB()
  {
    // reset helper variables
    data_count_ = 0;
    sum_ = 0;
    sum_sq_ = 0;
    // accept the new goal
    goal_ = as_.acceptNewGoal()->samples;
　　// acceptNewGoal: Accepts a new goal when one is available. The status of this goal is set to active upon acceptance, and the status of any previously active goal is set to preempted
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void analysisCB(const std_msgs::Float32::ConstPtr& msg)
  {
    // make sure that the action hasn‘t been canceled
    if (!as_.isActive())
      return;
    
    data_count_++;
    feedback_.sample = data_count_;
    feedback_.data = msg->data;
    //compute the std_dev and mean of the data 
    sum_ += msg->data;
    feedback_.mean = sum_ / data_count_;
    sum_sq_ += pow(msg->data, 2);
    feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
    as_.publishFeedback(feedback_);

    if(data_count_ > goal_) 
    {
      result_.mean = feedback_.mean;
      result_.std_dev = feedback_.std_dev;

      if(result_.mean < 5.0)
      {
        ROS_INFO("%s: Aborted", action_name_.c_str());
        //set the action state to aborted
        as_.setAborted(result_);
      }
      else 
      {
        ROS_INFO("%s: Succeeded", action_name_.c_str());
        // set the action state to succeeded
        as_.setSucceeded(result_);
      }
    } 
  }

protected:
    
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
  std::string action_name_;
  int data_count_, goal_;
  float sum_, sum_sq_;
  actionlib_tutorials::AveragingFeedback feedback_;
  actionlib_tutorials::AveragingResult result_;
  ros::Subscriber sub_;
};

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

  AveragingAction averaging(ros::this_node::getName());
  ros::spin();

  return 0;
}

　　程序中標準差是根據下面的公式計算的：

　　服務端程序會訂閱/random_number話題，並執行回調函數analysisCB。註意在消息回調函數的開頭會調用SimpleActionServer類中的isActive()來檢測goal的狀態，如果不是處於active狀態（The goal is currently being processed by the action server）則退出程序，直到客戶端發起請求，goal開始被server處理。

　　Goals請求由ActionClient發出，ActionServer接收後會創建一個有限狀態機來跟蹤goal的狀態：

　　goal狀態的轉變主要由server端程序發起，可以使用下面一系列的命令：

• setAccepted - After inspecting a goal, decide to start processing it

• setRejected - After inspecting a goal, decide to never process it because it is an invalid request (out of bounds, resources not available, invalid, etc)

• setSucceeded - Notify that goal has been successfully processed

• setAborted - Notify that goal encountered an error during processsing, and had to be aborted

• setCanceled - Notify that goal is no longer being processed, due to a cancel request

　　客戶端也能異步發起狀態轉變：

• CancelRequest: The client notifies the action server that it wants the server to stop processing the goal.

　　狀態機有下面多種狀態：

　　中間狀態：

• Pending - The goal has yet to be processed by the action server

• Active - The goal is currently being processed by the action server

• Recalling - The goal has not been processed and a cancel request has been received from the action client, but the action server has not confirmed the goal is canceled

• Preempting - The goal is being processed, and a cancel request has been received from the action client, but the action server has not confirmed the goal is canceled

　　最終狀態：

• Rejected - The goal was rejected by the action server without being processed and without a request from the action client to cancel

• Succeeded - The goal was achieved successfully by the action server

• Aborted - The goal was terminated by the action server without an external request from the action client to cancel

• Recalled - The goal was canceled by either another goal, or a cancel request, before the action server began processing the goal

• Preempted - Processing of the goal was canceled by either another goal, or a cancel request sent to the action server

Writing the Data Node

　　發布隨機數節點的代碼 gen_numbers.py如下。Python中的random.normalvariate(mu, sigma)函數會返回服從正態分布的隨機數，均值為mu，標準差為sigma.

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float32
import random
def gen_number():
    pub = rospy.Publisher(‘random_number‘, Float32)
    rospy.init_node(‘random_number_generator‘, log_level=rospy.INFO)
    rospy.loginfo("Generating random numbers")

    while not rospy.is_shutdown():
        pub.publish(Float32(random.normalvariate(5, 1)))
        rospy.sleep(0.05)

if __name__ == ‘__main__‘:
  try:
    gen_number()
  except Exception, e:
    print "done"

　　註意不要忘記給文件添加可執行權限：

chmod +x gen_numbers.py

Writing a Threaded Simple Action Client

　　SimpleActionClient的構造函數如下，註意參數spin_thread的設置。如果spin_thread為false則需要自行開啟線程。

// Constructs a SingleGoalActionClient 
actionlib::SimpleActionClient< ActionSpec >::SimpleActionClient ( const std::string & name, bool spin_thread = true )

/*
Parameters 
name: The action name. Defines the namespace in which the action communicates 
spin_thread: If true, spins up a thread to service this action‘s subscriptions. 
             If false, then the user has to call ros::spin() themselves. Defaults to True
*/

　　客戶端程序averaging_client.cpp如下：

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>  // the action library used from implementing simple action clients
#include <actionlib/client/terminal_state.h>        // defines the possible goal states
#include <actionlib_tutorials/AveragingAction.h>    // This includes action message generated from the Averaging.action file
#include <boost/thread.hpp>                          // the boost thread library for spinning the thread

void spinThread()
{
  ros::spin();
}

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

  // create the action client
  actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging", false);
  
  boost::thread spin_thread(&spinThread); // the thread is created and the ros node is started spinning in the background

  ROS_INFO("Waiting for 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(); 

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  actionlib_tutorials::AveragingGoal goal;
  goal.samples = 100;
  ac.sendGoal(goal);

  // The action client now waits for the goal to finish before continuing 
  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.");

  // shutdown the node and join the thread back before exiting
  ros::shutdown();
  
  spin_thread.join();

  //exit
  return 0;
}

　　下面是boost::thread線程管理的一個例子：

#include <boost/thread.hpp> 
#include <iostream> 

void wait(int seconds) 
{ 
  boost::this_thread::sleep(boost::posix_time::seconds(seconds)); 
} 

void thread() 
{ 
  for (int i = 0; i < 5; ++i) 
  { 
    wait(1); 
    std::cout << i << std::endl; 
  } 
} 

int main() 
{ 
  boost::thread t(thread); 
  t.join(); 

  return 0;
} 

　　上述示例中的變量t 一旦被創建，該thread()函數就在其所在線程中被立即執行。為了防止程序終止，就需要對新建線程調用 join() 方法。 join() 方法是一個阻塞調用：它可以暫停當前線程，直到調用 join() 的線程運行結束。 這就使得main() 函數一直會等待到 thread() 運行結束。程序輸出如下圖所示，如果沒有用join方法，則會直接退出。

Running an Action Server and Client with Other Nodes

　　運行server：

rosrun actionlib_tutorials averaging_server

　　運行隨機數發生器節點：

rosrun actionlib_tutorials gen_numbers.py

　　運行client：

rosrun actionlib_tutorials averaging_client

　　計算平均數時查看feedback：

rostopic echo /averaging/feedback 

參考：

actionlib-Tutorials

boost::thread線程管理

ROS actionlib學習（一）

ROS actionlib學習（二）

actionlib-Detailed Description

ROS actionlib學習（三）