7.learningAction（行爲庫）2

1.使用目標回調方法(Goal Callback Method)編寫一個簡單的行爲服務器

開始編寫行爲前，先創建行爲消息文件Averaging.action

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

手動生成消息

rosrun actionlib_msgs genaction.py -o msg/ action/Averaging.action

在編譯過程中自動生成消息文件，在CMakeList.txt中添加

find_package(catkin REQUIRED COMPONENTS actionlib std_msgs message_generation) 
add_action_files(DIRECTORY action FILES Averaging.action)
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

2.編寫簡單的行爲服務器

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/AveragingAction.h>

class AveragingAction
{
public:
//在行爲構造函數中，創建一個行爲服務器
//行爲服務器加載一個節點句柄(node handle)、行爲名稱和可選的回調(executeCB)。
//在本示例中創建的行爲服務器不需要回調(executeCB)參數。
//在行爲服務器構造之後，等價替換成註冊的目標(goal)和搶佔(preempt)回調用於行爲的構造函數。    
  AveragingAction(std::string name) : 
    as_(nh_, name, false),
    action_name_(name)
  {
//註冊目標和反饋回調函數
as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));     as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));

//訂閱感興趣的話題數據,建立一個數據回調，該回調會處理行爲並且行爲服務器開啓
    sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
    as_.start();
  }

  ~AveragingAction(void)
  {
  }
//這裏是一個目標(goalCB)回調函數，參考構造函數。這個回調函數不返回任何東西也不需要任何參數。當目標回調(goalCB)調用行爲時，需要接收目標並且存儲任何重要的信息。
  void goalCB()
  {
    // 重置幫助變量
    data_count_ = 0;
    sum_ = 0;
    sum_sq_ = 0;
    // 接收新目標
    goal_ = as_.acceptNewGoal()->samples;
  }
//行爲事件聲明，當回調發生，行爲代碼會運行，否則一個會創建一個搶佔回調來保證行爲響應及時來取消請求。回調函數不需要參數，並且會設置搶佔(preempted)到行爲服務器。
  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // 設置行爲狀態爲搶佔(preempted)
    as_.setPreempted();
  }
//這裏模擬回調得到訂閱數據通道的消息格式，並且在繼續處理數據之前檢查行爲是否處於活躍狀態。
  void analysisCB(const std_msgs::Float32::ConstPtr& msg)
  {
    // 確保行爲還沒有被取消
    if (!as_.isActive())
      return;

    data_count_++;
    feedback_.sample = data_count_;
    feedback_.data = msg->data;
    //處理std_dev和數據含義 
    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_);
//一旦收集到足夠的數據，行爲服務器會設置成功或失敗。這將禁用行爲服務器並且analysisCB函數會向之前描述的那樣立即返回。
    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());
        //設置行爲狀態爲崩潰(aborted)
        as_.setAborted(result_);
      }
      else 
      {
        ROS_INFO("%s: Succeeded", action_name_.c_str());
        // 設置行爲狀態爲成功(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;
}

CMakelist.txt

add_executable(averaging_server src/averaging_server.cpp)
target_link_libraries(averaging_server ${catkin_LIBRARIES})

3.編寫簡單的客戶端線程

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <actionlib_tutorials/AveragingAction.h>
#include <boost/thread.hpp>
/*
actionlib/server/simple_action_client.h是行爲庫，實現簡單行爲客戶端。
actionlib/client/terminal_state.h 定義目標可能的狀態。
boost線程庫用於循環示例中的線程
*/

//一個簡單的函數用於循環線程，該函數會在代碼之後用到。這個線程會在後臺循環ros節點
void spinThread()
{
  ros::spin();
}

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

// 創建一個行爲客戶端
//在行爲定義中的行爲客戶端模板，制定和行爲服務器通信的消息類型。
//行爲客戶端構造函數會加載兩個參數，包括需要連接的服務名和一個可選的布爾類型用於循環一個線程。
//這裏創建的行爲客戶端使用服務名和自動循環選項爲false作爲參數，這意味着一個必須創建一個線程。 

 actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging");

//創建一個線程，然後在後臺開啓一個ros節點。通過使用這個方法你可以創建多個線程用於你的行爲客戶端，如果有必要的話。
  boost::thread spin_thread(&spinThread);

  ROS_INFO("Waiting for action server to start.");
//由於行爲服務器又可以沒有掛起或運行，行爲客戶端將在繼續之前等待行爲服務器開啓
  ac.waitForServer();

//這裏創建一個目標消息，設置目標值並且發送到行爲服務器
  ROS_INFO("Action server started, sending goal.");
  actionlib_tutorials::AveragingGoal goal;
  goal.samples = 100;
  ac.sendGoal(goal);

//行爲客戶端在繼續之前，等待目標結束。超時時間設置爲30秒，這意味着函數會在30秒後如果目標沒有完成，將會返回false。
  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.");

  // 關閉節點，在退出前加入線程
  ros::shutdown();
  spin_thread.join();

  //exit
  return 0;
}

CMakeList.txt

add_executable(averaging_client src/averaging_client.cpp)
target_link_libraries(averaging_client ${catkin_LIBRARIES})

在開始運行服務器和客戶端之前，需要創建一個數據節點

#!/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