Contents

  1. 创建一个行为消息
  2. 编写一个简单的行为服务器
    1. 代码
    2. 代码解释
  3. 编译 & 运行行为
  4. 发送目标到行为服务器

创建一个行为消息

开始编写一个行为之前，很重要的事是定义目标、结果和反馈消息。行为消息会自动从.action文件中生成，对于更多细节关于行为文件查看actionlib文档。这个文件定义了目标、结果和行为反馈话题的类型和格式。用你最喜欢的编辑器创建actionlib_tutorials/action/Averaging.action，然后用以下内容加载： 

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

为了从这个文件中手动生成消息： 

$ roscd actionlib_tutorials
$ rosrun actionlib_msgs genaction.py -o msg/ action/Averaging.action

你将会看到： 

  • Generating for action Averaging

在编译过程中，自动生成消息文件，添加以下内容到CMakeLists.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)

然后运行： 

  • $ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make

编写一个简单的行为服务器

首先，用你最喜欢的编辑器创建actionlib_tutorials/src/averaging_server.cpp，然后用添加以下内容：

代码

   1 #include <ros/ros.h>
   2 #include <std_msgs/Float32.h>
   3 #include <actionlib/server/simple_action_server.h>
   4 #include <actionlib_tutorials/AveragingAction.h>
   5 
   6 class AveragingAction
   7 {
   8 public:
   9     
  10   AveragingAction(std::string name) : 
  11     as_(nh_, name, false),
  12     action_name_(name)
  13   {
  14     //注册目标和反馈回调函数
  15     as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
  16     as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));
  17 
  18     //订阅感兴趣的话题数据
  19     sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
  20     as_.start();
  21   }
  22 
  23   ~AveragingAction(void)
  24   {
  25   }
  26 
  27   void goalCB()
  28   {
  29     // 重置帮助变量
  30     data_count_ = 0;
  31     sum_ = 0;
  32     sum_sq_ = 0;
  33     // 接收新目标
  34     goal_ = as_.acceptNewGoal()->samples;
  35   }
  36 
  37   void preemptCB()
  38   {
  39     ROS_INFO("%s: Preempted", action_name_.c_str());
  40     // 设置行为状态为抢占(preempted)
  41     as_.setPreempted();
  42   }
  43 
  44   void analysisCB(const std_msgs::Float32::ConstPtr& msg)
  45   {
  46     // 确保行为还没有被取消
  47     if (!as_.isActive())
  48       return;
  49     
  50     data_count_++;
  51     feedback_.sample = data_count_;
  52     feedback_.data = msg->data;
  53     //处理std_dev和数据含义 
  54     sum_ += msg->data;
  55     feedback_.mean = sum_ / data_count_;
  56     sum_sq_ += pow(msg->data, 2);
  57     feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
  58     as_.publishFeedback(feedback_);
  59 
  60     if(data_count_ > goal_) 
  61     {
  62       result_.mean = feedback_.mean;
  63       result_.std_dev = feedback_.std_dev;
  64 
  65       if(result_.mean < 5.0)
  66       {
  67         ROS_INFO("%s: Aborted", action_name_.c_str());
  68         //设置行为状态为崩溃(aborted)
  69         as_.setAborted(result_);
  70       }
  71       else 
  72       {
  73         ROS_INFO("%s: Succeeded", action_name_.c_str());
  74         // 设置行为状态为成功(succeeded)
  75         as_.setSucceeded(result_);
  76       }
  77     } 
  78   }
  79 
  80 protected:
  81     
  82   ros::NodeHandle nh_;
  83   actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
  84   std::string action_name_;
  85   int data_count_, goal_;
  86   float sum_, sum_sq_;
  87   actionlib_tutorials::AveragingFeedback feedback_;
  88   actionlib_tutorials::AveragingResult result_;
  89   ros::Subscriber sub_;
  90 };
  91 
  92 int main(int argc, char** argv)
  93 {
  94   ros::init(argc, argv, "averaging");
  95 
  96   AveragingAction averaging(ros::this_node::getName());
  97   ros::spin();
  98 
  99   return 0;
 100 }

(查看存储库的代码)

代码解释

现在，让我们分块解释代码。 

   1 #include <ros/ros.h>
   2 #include <std_msgs/Float32.h>
   3 #include <actionlib/server/simple_action_server.h>
   4

使用的actionlib/server/simple_action_server.h是行为库，从简单行为实现。 

   4 #include <actionlib_tutorials/AveragingAction.h>
   5

这个包含从以上Averaging.action文件中生成的消息。这个头文件自动从AveragingAction.msg文件生成，更多关于消息定义的信息，查看msg页面。 

   6 class AveragingAction
   7 {
   8 public:
   9     
  10   AveragingAction(std::string name) : 
  11     as_(nh_, name, false),
  12     action_name_(name)
  13   {
  14     //注册目标和反馈回调函数
  15     as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
  16     as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));

在行为构造函数中，创建一个行为服务器。行为服务器加载一个节点句柄(node handle)、行为名称和可选的回调(executeCB)。在本示例中创建的行为服务器不需要回调(executeCB)参数。在行为服务器构造之后，等价替换成注册的目标(goal)和抢占(preempt)回调用于行为的构造函数。 

  18     //订阅感兴趣的话题数据
  19     sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
  20     as_.start();
  21   }

这里，建立一个数据回调，该回调会处理行为并且行为服务器开启。 

  27   void goalCB()
  28   {
  29     // 重置帮助变量
  30     data_count_ = 0;
  31     sum_ = 0;
  32     sum_sq_ = 0;
  33     // 接收新目标
  34     goal_ = as_.acceptNewGoal()->samples;
  35   }

这里是一个目标(goalCB)回调函数，参考构造函数。这个回调函数不返回任何东西也不需要任何参数。当目标回调(goalCB)调用行为时，需要接收目标并且存储任何重要的信息。如果你需要在你接收它之前查看目标，查阅SimpleActionServer(ExecuteCallbackMethod) 教程。 

  37   void preemptCB()
  38   {
  39     ROS_INFO("%s: Preempted", action_name_.c_str());
  40     // 设置行为状态为抢占(preempted)
  41     as_.setPreempted();
  42   }

行为事件声明，当回调发生，行为代码会运行，否则一个会创建一个抢占回调来保证行为响应及时来取消请求。回调函数不需要参数，并且会设置抢占(preempted)到行为服务器。 

  44   void analysisCB(const std_msgs::Float32::ConstPtr& msg)
  45   {
  46     // 确保行为还没有被取消
  47     if (!as_.isActive())
  48       return;

这里模拟回调得到订阅数据通道的消息格式，并且在继续处理数据之前检查行为是否处于活跃状态。 

  50     data_count_++;
  51     feedback_.sample = data_count_;
  52     feedback_.data = msg->data;
  53     //处理std_dev和数据含义 
  54     sum_ += msg->data;
  55     feedback_.mean = sum_ / data_count_;
  56     sum_sq_ += pow(msg->data, 2);
  57     feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
  58     as_.publishFeedback(feedback_);

这里，把相关数据放到反馈变量中，然后在行为服务器提供的反馈通道发布出去。 

  60     if(data_count_ > goal_) 
  61     {
  62       result_.mean = feedback_.mean;
  63       result_.std_dev = feedback_.std_dev;
  64 
  65       if(result_.mean < 5.0)
  66       {
  67         ROS_INFO("%s: Aborted", action_name_.c_str());
  68         //设置行为状态为崩溃(aborted)
  69         as_.setAborted(result_);
  70       }
  71       else 
  72       {
  73         ROS_INFO("%s: Succeeded", action_name_.c_str());
  74         // 设置行为状态为成功(succeeded)
  75         as_.setSucceeded(result_);
  76       }
  77     } 
  78   }

一旦收集到足够的数据，行为服务器会设置成功或失败。这将禁用行为服务器并且analysisCB函数会向之前描述的那样立即返回。 

  80 protected:
  81     
  82   ros::NodeHandle nh_;
  83   actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
  84   std::string action_name_;
  85   int data_count_, goal_;
  86   float sum_, sum_sq_;
  87   actionlib_tutorials::AveragingFeedback feedback_;
  88   actionlib_tutorials::AveragingResult result_;
  89   ros::Subscriber sub_;
  90 };

这些是行为类的受保护的变量。在构造行为的过程中，构造节点句柄然后传递到行为服务器。构造的行为服务器正如以上描述那样。创建的反馈和结果消息用于在行为中发布。创建的订阅也会保存节点句柄。 

  92 int main(int argc, char** argv)
  93 {
  94   ros::init(argc, argv, "averaging");
  95 
  96   AveragingAction averaging(ros::this_node::getName());
  97   ros::spin();
  98 
  99   return 0;
 100 }

最后的main函数，创建行为并且循环节点。行为会运行和等待接收目标。

编译 & 运行行为

在你的CMakeLists.txt文件中添加以下几行： 

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

在你使用catkin_make编译之后，在一个新终端开启一个roscore。 

$ roscore

然后运行行为服务器： 

$ rosrun actionlib_tutorials averaging_server

你会看到类似如下输出： 

  • [ INFO] 1250790662.410962000: Started node [/averaging], pid [29267], bound on [aqy], xmlrpc port [39746], tcpros port [49573], logging to [~/ros/ros/log/averaging_29267.log], using [real] time

检查你的行为运行正常，使用rostopic list命令查看发布情况： 

$ rostopic list -v

你会看到类似如下输出： 

  • Published topics:
 * /averaging/status [actionlib_msgs/GoalStatusArray] 1 publisher
 * /averaging/result [actionlib_tutorials/AveragingActionResult] 1 publisher
 * /averaging/feedback [actionlib_tutorials/AveragingActionFeedback] 1 publisher
 * /rosout [roslib/Log] 1 publisher
 * /rosout_agg [roslib/Log] 1 publisher

Subscribed topics:
 * /time [unknown type] 2 subscribers
 * /rosout [roslib/Log] 1 subscriber
 * /averaging/goal [unknown type] 1 subscriber
 * /averaging/cancel [unknown type] 1 subscriber

另外你可以查看节点： 

$ rosrun rqt_graph rqt_graph &

这里显示了你的行为服务器发布反馈、状态和期望的结果通道，以及订阅目标和期望取消通道。行为服务器运行正常。

发送目标到行为服务器

对于下一步使用你的行为，你需要按下Ctrl-C行为服务器，然后 创建一个简单的行为客户端线程。

Wiki: cn/actionlib_tutorials/Tutorials/SimpleActionServer(GoalCallbackMethod)/groovy (last edited 2017-03-19 06:26:37 by Playfish)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0