ROS actionlib学习（三）

　　下面这个例子将展示用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

原文地址：https://www.cnblogs.com/21207-iHome/p/8304658.html

时间： 2024-07-30 04:16:17

ROS actionlib学习（三）的相关文章

ROS actionlib学习（二）

在ROS actionlib学习(一)中的例子展示了actionlib最基本的用法,下面我们看一个稍微实际一点的例子,用actionlib计算斐波那契数列,并发布反馈(feedback)和结果(result).斐波那契数列指的是这样一个数列: 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233,377,610,987,1597,2584,4181,6765,10946,17711,28657,46368........ 这个数列从第3项开始,每一项都等于

ROS actionlib学习（一）

actionlib是ROS中一个很重要的功能包集合,尽管在ROS中已经提供了srevice机制来满足请求-响应式的使用场景,但是假如某个请求执行时间很长,在此期间用户想查看执行的进度或者取消这个请求的话,service机制就不能满足了,但是actionlib可满足用户这种需求.例如,控制机器人运动到地图中某一目标位置,这个过程可能复杂而漫长,执行过程中还可能强制中断或反馈信息,这时actionlib就能大展伸手了. actionlib使用client-server工作模式,ActionClien

算法学习三阶段

?? 第一阶段:练经典经常使用算法,以下的每一个算法给我打上十到二十遍,同一时候自己精简代码, 由于太经常使用,所以要练到写时不用想,10-15分钟内打完,甚至关掉显示器都能够把程序打 出来. 1.最短路(Floyd.Dijstra,BellmanFord) 2.最小生成树(先写个prim,kruscal 要用并查集,不好写) 3.大数(高精度)加减乘除 4.二分查找. (代码可在五行以内) 5.叉乘.判线段相交.然后写个凸包. 6.BFS.DFS,同一时候熟练hash 表(要熟,要灵活,代码要

Jetty学习三：配置概览-需要配置什么

上一节讲述了怎么配置Jetty,这节将告诉你使用Jetty你需要配置些什么. 配置Server Server实例是Jetty服务端的中心协调对象,它为所有其他Jetty服务端组件提供服务和生命周期管理.在标准Jetty发布中,核心的服务端配置是在etc/jetty.xml文件中,你也能在其中包含其他服务端配置,可以包括: 1)ThreadPool Server实例提供了一个线程池,你可以在etc/jetty.xml中配置最大线程数和最小线程数. 2)Handlers Jetty服务端只能有一个H

ZigBee学习三 UART通信

ZigBee学习三 UART通信 本实验只对coordinator.c文件进行改动就可以实现串口的收发. 修改coordinator.c文件 byte GenericApp_TransID; // This is the unique message ID (counter) afAddrType_t GenericApp_DstAddr; unsigned char uartbuf[128];/**************************************************

Spark学习三：Spark Schedule以及idea的安装和导入源码

Spark学习三:Spark Schedule以及idea的安装和导入源码 标签(空格分隔): Spark Spark学习三Spark Schedule以及idea的安装和导入源码 一RDD操作过程中的数据位置 二Spark Schedule 三Idea导入spark源码 一,RDD操作过程中的数据位置 [hadoop001@xingyunfei001 spark-1.3.0-bin-2.5.0]$ bin/spark-shell --master local[2] val rdd = sc.t

mongodb学习(三)

菜鸟啊...先吐槽一下自己 一 准备工作: 1.安装服务端: 去官网下载 http://www.mongodb.org/downloads 其实也自带了客户端 shell 2.安装客户端: mongoVUE http://blog.mongovue.com/ 并不是完全免费 破解方法: http://yhv5.com/mongovue_480.html 将服务端下载下来后直接安装 我下载在D盘也安装在D盘的... 启动mongodb的服务端不需要各种命令....直接鼠标左键双击bin中的mong

c++ boost库学习三：实用工具

noncopyable 大家都知道定义一个空类的时候,它实际包含了构造函数,拷贝构造函数,赋值操作符和析构函数等. 这样就很容易产生一个问题,就是当用户调用A a(“^_^") 或者A c="^_^" 时会发生一些意想不到的行为,所以很多时候我们需要禁用这样的用法. 一种方法就是把拷贝构造函数和赋值操作符显式的定义为private,但是这样需要很多代码. 于是boost库为大家提供了一个简单的方法:只需要将类继承于noncopyable就可以了. #include "

scala学习三---文件里读取文本行

学习了scala的基本知识后,发现了scala是集函数式和指令式结合为一体的一种语言,代码更加简洁,但是对于用习惯了java的人来说,还真的不是一件易事~~ 今天学习scala脚本读取文本文件 列子如下: import scala.io.Source if(args.length>0){ for(line <- Source.fromFile(args(0)).getLines) print(line.length+" "+line) }else{ Console.err.