7.learningAction（行为库）2

最新推荐文章于 2022-05-18 06:52:17 发布

原创最新推荐文章于 2022-05-18 06:52:17 发布 · 502 阅读

0 ·

CC 4.0 BY-SA版权

ROS 专栏收录该内容

13 篇文章

订阅专栏

本文介绍了如何使用ROS创建一个行为服务器，通过目标回调方法编写简单的行为，并详细讲述了行为消息文件的生成、行为服务器的编写以及客户端线程的实现过程。在开始运行前，需要创建数据节点并确保正确编译。

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