< >
Home » ROS与C++入门教程 » ROS与C++入门教程-actionlib-编写简单的action服务器(Goal Callback)

ROS与C++入门教程-actionlib-编写简单的action服务器(Goal Callback)

ROS与C++入门教程-actionlib-编写简单的action服务器(Goal Callback)

说明：

  • 介绍如何用C++编写使用goal回调函数的action服务器
  • 介绍使用simple_action_server库创建平均数action服务器
  • 介绍使用action来处理来自节点的输入数据，平均数来自节点，goal是样本数的平均值，feedback是样本数，样本数据，当前平均值和当前标准偏差，result是所要求的样本数的平均值和标准偏差。

创建action消息

  • goal目标，result结果，feedback反馈

  • 编写action，定义goal, result和feedback消息

  • action消息自动从.action文件生成，更多详细参考actionlib

  • 这个文件定义action的goal, result和feedback话题的类型和格式。

  • 创建actionlib_tutorials/action/Averaging.action

cd ~/catkin_ws/src/actionlib_tutorials/action
touch Averaging.action
vim 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 ~/catkin_ws
$ catkin_make

编写服务器

  • 创建actionlib_tutorials/src/averaging_server.cpp文件，代码参考
cd ~/catkin_ws/src/actionlib_tutorials/src/
touch averaging_server.cpp
vim averaging_server.cpp
  • 代码如下：
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/AveragingAction.h>

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;
  }

  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;
}

代码解释：

  • 代码：
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h>
  • 解释：actionlib/server/simple_action_server.h是action库，用于实现简单的action服务器
  • 代码：
#include <actionlib_tutorials/AveragingAction.h>
  • 解释：这个头文件是Averaging.action文件自动生成的消息头文件
  • 代码：
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));
  • 解释：
    • action构造器，创建action服务器。
    • action服务器需要提供节点句柄，服务器名称，可选的executeCB参数。
    • 本例不使用executeCB参数，而是在构造函数里注册goal和preempt回调函数。
  • 代码：
  //subscribe to the data topic of interest
    sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
    as_.start();
  }
  • 解释：订阅输入的数据，并定义数据处理的回调函数。
  • 代码：
void goalCB()
  {
    // reset helper variables
    data_count_ = 0;
    sum_ = 0;
    sum_sq_ = 0;
    // accept the new goal
    goal_ = as_.acceptNewGoal()->samples;
  }
  • 解释：
    • 这里是在构造函数中引用的goalCB函数。
    • 回调函数不返回任何内容，并且不带参数。
    • 当goalCB被调用时，action需要接受goal并存储任何重要信息。
    • 如果您需要在接受目标之前查看目标，请参阅SimpleActionServer（ExecuteCallbackMethod）教程。
  • 代码：
  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_);

  • 解释：这里，相关数据被放入feedback变量中，然后通过action服务器发布。

  • 代码：

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_);
      }
    } 
  }

  • 解释:

    • 一旦收集到足够的数据，action服务器将设置为成功或失败。
    • 这将停用动作服务器，并且analysisCB函数返回。

  • 代码：

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_;
};

  • 解释：私有变量，用于保存不同的对象，节点句柄，action对象，action名，goal，feedback等。

  • 代码：

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

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

  return 0;
}
  • 解释：main函数启动action线程。会一直运行，等待接受goal。

编译

  • 更改CMakeLists.txt，增加：
add_executable(averaging_server src/averaging_server.cpp)
target_link_libraries(averaging_server ${catkin_LIBRARIES})
  • 编译：
$ cd ~/catkin_ws
$ catkin_make

运行：

  • 新终端，执行：
$ 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 -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 &

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ROS与C++入门教程