ROS探索总结汇总

ROS探索总结（一）——ROS简介

一、历史

       随着机器人领域的快速发展和复杂化，代码的复用性和模块化的需求原来越强烈，而已有的开源机器人系统又不能很好的适应需求。2010年Willow Garage公司发布了开源机器人操作系统ROS（robot operating system），很快在机器人研究领域展开了学习和使用ROS的热潮。

       ROS系统是起源于2007年斯坦福大学人工智能实验室的项目与机器人技术公司Willow Garage的个人机器人项目（Personal Robots Program）之间的合作，2008年之后就由Willow Garage来进行推动。已经有四年多的时间了 (视频)。随着PR2那些不可思议的表现，譬如叠衣服，插插座，做早饭，ROS也得到越来越多的关注。Willow Garage公司也表示希望借助开源的力量使PR2变成“全能”机器人。

       PR2价格高昂，2011年零售价高达40万美元。PR2现主要用于研究。PR2有两条手臂，每条手臂七个关节，手臂末端是一个可以张合的钳子。PR2依靠底部的四个轮子移动。在PR2的头部，胸部，肘部，钳子上安装有高分辨率摄像头，激光测距仪，惯性测量单元，触觉传感器等丰富的传感设备。在PR2的底部有两台8核的电脑作为机器人各硬件的控制和通讯中枢。两台电脑安装有Ubuntu和ROS。

二、设计目标

       ROS是开源的，是用于机器人的一种后操作系统，或者说次级操作系统。它提供类似操作系统所提供的功能，包含硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间的消息传递、程序发行包管理，它也提供一些工具程序和库用于获取、建立、编写和运行多机整合的程序。

       

       ROS的首要设计目标是在机器人研发领域提高代码复用率。ROS是一种分布式处理框架（又名Nodes）。这使可执行文件能被单独设计，并且在运行时松散耦合。这些过程可以封装到数据包（Packages）和堆栈（Stacks）中，以便于共享和分发。ROS还支持代码库的联合系统。使得协作亦能被分发。这种从文件系统级别到社区一级的设计让独立地决定发展和实施工作成为可能。上述所有功能都能由ROS的基础工具实现。

三、主要特点

       ROS的运行架构是一种使用ROS通信模块实现模块间P2P的松耦合的网络连接的处理架构，它执行若干种类型的通讯，包括基于服务的同步RPC（远程过程调用）通讯、基于Topic的异步数据流通讯，还有参数服务器上的数据存储。但是ROS本身并没有实时性。

       ROS的主要特点可以归纳为以下几条：

     （1）点对点设计

       一个使用ROS的系统包括一系列进程，这些进程存在于多个不同的主机并且在运行过程中通过端对端的拓扑结构进行联系。虽然基于中心服务器的那些软件框架也可以实现多进程和多主机的优势，但是在这些框架中，当各电脑通过不同的网络进行连接时，中心数据服务器就会发生问题。
      ROS的点对点设计以及服务和节点管理器等机制可以分散由计算机视觉和语音识别等功能带来的实时计算压力，能够适应多机器人遇到的挑战。

       （2）多语言支持

       在写代码的时候，许多编程者会比较偏向某一些编程语言。这些偏好是个人在每种语言的编程时间、调试效果、语法、执行效率以及各种技术和文化的原因导致的结果。为了解决这些问题，我们将ROS设计成了语言中立性的框架结构。ROS现在支持许多种不同的语言，例如C++、Python、Octave和LISP，也包含其他语言的多种接口实现。

       ROS的特殊性主要体现在消息通讯层，而不是更深的层次。端对端的连接和配置利用XML-RPC机制进行实现，XML-RPC也包含了大多数主要语言的合理实现描述。我们希望ROS能够利用各种语言实现的更加自然，更符合各种语言的语法约定，而不是基于C语言给各种其他语言提供实现接口。然而，在某些情况下利用已经存在的库封装后支持更多新的语言是很方便的，比如Octave的客户端就是通过C++的封装库进行实现的。
       为了支持交叉语言，ROS利用了简单的、语言无关的接口定义语言去描述模块之间的消息传送。接口定义语言使用了简短的文本去描述每条消息的结构，也允许消息的合成，例如下图就是利用接口定义语言描述的一个点的消息：

       每种语言的代码产生器就会产生类似本种语言目标文件，在消息传递和接收的过程中通过ROS自动连续并行的实现。这就节省了重要的编程时间，也避免了错误：之前3行的接口定义文件自动的扩展成137行的C++代码，96行的Python代码，81行的Lisp代码和99行的Octave代码。因为消息是从各种简单的文本文件中自动生成的，所以很容易列举出新的消息类型。在编写的时候，已知的基于ROS的代码库包含超过四百种消息类型，这些消息从传感器传送数据，使得物体检测到了周围的环境。

       最后的结果就是一种语言无关的消息处理，让多种语言可以自由的混合和匹配使用。

       （3）精简与集成

       大多数已经存在的机器人软件工程都包含了可以在工程外重复使用的驱动和算法，不幸的是，由于多方面的原因，大部分代码的中间层都过于混乱，以至于很困难提取出它的功能，也很难把它们从原型中提取出来应用到其他方面。
      为了应对这种趋势，我们鼓励将所有的驱动和算法逐渐发展成为和ROS没有依赖性单独的库。ROS建立的系统具有模块化的特点，各模块中的代码可以单独编译，而且编译使用的CMake工具使它很容易的就实现精简的理念。ROS基本将复杂的代码封装在库里，只是创建了一些小的应用程序为ROS显示库的功能，就允许了对简单的代码超越原型进行移植和重新使用。作为一种新加入的有优势，单元测试当代码在库中分散后也变得非常的容易，一个单独的测试程序可以测试库中很多的特点。
       ROS利用了很多现在已经存在的开源项目的代码，比如说从Player项目中借鉴了驱动、运动控制和仿真方面的代码，从OpenCV中借鉴了视觉算法方面的代码，从OpenRAVE借鉴了规划算法的内容，还有很多其他的项目。在每一个实例中，ROS都用来显示多种多样的配置选项以及和各软件之间进行数据通信，也同时对它们进行微小的包装和改动。ROS可以不断的从社区维护中进行升级，包括从其他的软件库、应用补丁中升级ROS的源代码。

       （4）工具包丰富

       为了管理复杂的ROS软件框架，我们利用了大量的小工具去编译和运行多种多样的ROS组建，从而设计成了内核，而不是构建一个庞大的开发和运行环境。

       这些工具担任了各种各样的任务，例如，组织源代码的结构，获取和设置配置参数，形象化端对端的拓扑连接，测量频带使用宽度，生动的描绘信息数据，自动生成文档等等。尽管我们已经测试通过像全局时钟和控制器模块的记录器的核心服务，但是我们还是希望能把所有的代码模块化。我们相信在效率上的损失远远是稳定性和管理的复杂性上无法弥补的。

       （5）免费并且开源

      ROS所有的源代码都是公开发布的。我们相信这将必定促进ROS软件各层次的调试，不断的改正错误。虽然像Microsoft Robotics Studio和Webots这样的非开源软件也有很多值得赞美的属性，但是我们认为一个开源的平台也是无可为替代的。当硬件和各层次的软件同时设计和调试的时候这一点是尤其真实的。

      ROS以分布式的关系遵循这BSD许可，也就是说允许各种商业和非商业的工程进行开发。ROS通过内部处理的通讯系统进行数据的传递，不要求各模块在同样的可执行功能上连接在一起。如此，利用ROS构建的系统可以很好的使用他们丰富的组件：个别的模块可以包含被各种协议保护的软件，这些协议从GPL到BSD，但是许可的一些“污染物”将在模块的分解上就完全消灭掉。

参考资料：

（1）《开源机器人操作系统——ROS》 张建伟等著

（2）《an open-source Robot Operating System》 paper

（3）  willowgarage公司网站：http://www.willowgarage.com/
（4）  ROS官方wiki：http://www.ros.org

----------------------------------------------------------------

ROS探索总结（二）——ROS总体框架

一、  总体结构

       根据ROS系统代码的维护者和分布来标示，主要有两大部分：

     （1）main：核心部分，主要由Willow Garage公司和一些开发者设计、提供以及维护。它提供了一些分布式计算的基本工具，以及整个ROS的核心部分的程序编写。

     （2）universe：全球范围的代码，有不同国家的ROS社区组织开发和维护。一种是库的代码，如OpenCV、PCL等；库的上一层是从功能角度提供的代码，如人脸识别，他们调用下层的库；最上层的代码是应用级的代码，让机器人完成某一确定的功能。

       一般是从另一个角度对ROS分级的，主要分为三个级别：计算图级、文件系统级、社区级。

二、  计算图级

       计算图是ROS处理数据的一种点对点的网络形式。程序运行时，所有进程以及他们所进行的数据处理，将会通过一种点对点的网络形式表现出来。这一级主要包括几个重要概念：节点（node）、消息（message）、主题（topic）、服务（service）。

       （1）  节点

       节点就是一些直行运算任务的进程。ROS利用规模可增长的方式是代码模块化：一个系统就是典型的由很多节点组成的。在这里，节点也可以被称之为“软件模块”。我们使用“节点”使得基于ROS的系统在运行的时候更加形象化：当许多节点同时运行时，可以很方便的将端对端的通讯绘制成一个图表，在这个图表中，进程就是图中的节点，而端对端的连接关系就是其中弧线连接。

       （2）  消息

       节点之间是通过传送消息进行通讯的。每一个消息都是一个严格的数据结构。原来标准的数据类型（整型，浮点型，布尔型等等）都是支持的，同时也支持原始数组类型。消息可以包含任意的嵌套结构和数组（很类似于C语言的结构structs）。

       （3）  主题

       消息以一种发布/订阅的方式传递。一个节点可以在一个给定的主题中发布消息。一个节点针对某个主题关注与订阅特定类型的数据。可能同时有多个节点发布或者订阅同一个主题的消息。总体上，发布者和订阅者不了解彼此的存在。

       （4）  服务

        虽然基于话题的发布/订阅模型是很灵活的通讯模式，但是它广播式的路径规划对于可以简化节点设计的同步传输模式并不适合。在ROS中，我们称之为一个服务，用一个字符串和一对严格规范的消息定义：一个用于请求，一个用于回应。这类似于web服务器，web服务器是由URIs定义的，同时带有完整定义类型的请求和回复文档。需要注意的是，不像话题，只有一个节点可以以任意独有的名字广播一个服务：只有一个服务可以称之为“分类象征”，比如说，任意一个给出的URI地址只能有一个web服务器。

        在上面概念的基础上，需要有一个控制器可以使所有节点有条不紊的执行，这就是一个ROS的控制器（ROS Master）。

        ROS Master 通过RPC（Remote Procedure Call Protocol，远程过程调用）提供了登记列表和对其他计算图表的查找。没有控制器，节点将无法找到其他节点，交换消息或调用服务。

        比如控制节点订阅和发布消息的模型如下：

        ROS的控制器给ROS的节点存储了主题和服务的注册信息。节点与控制器通信从而报告它们的注册信息。当这些节点与控制器通信的时候，它们可以接收关于其他以注册及节点的信息并且建立与其它以注册节点之间的联系。当这些注册信息改变时控制器也会回馈这些节点，同时允许节点动态创建与新节点之间的连接。
        节点与节点之间的连接是直接的，控制器仅仅提供了查询信息，就像一个DNS服务器。节点订阅一个主题将会要求建立一个与出版该主题的节点的连接，并且将会在同意连接协议的基础上建立该连接。

         另：ROS控制器控制服务：

三、  文件系统级

        ROS文件系统级指的是在硬盘上面查看的ROS源代码的组织形式。

        ROS中有无数的节点、消息、服务、工具和库文件，需要有效的结构去管理这些代码。在ROS的文件系统级，有以下几个重要概念：包（package）、堆（stack）、

        （1）  包

        ROS的软件以包的方式组织起来。包包含节点、ROS依赖库、数据套、配置文件、第三方软件、或者任何其他逻辑构成。包的目标是提供一种易于使用的结构以便于软件的重复使用。总得来说，ROS的包短小精干。

        （2）  堆

        堆是包的集合，它提供一个完整的功能，像“navigation stack”。Stack与版本号关联，同时也是如何发行ROS软件方式的关键。
        ROS是一种分布式处理框架。这使可执行文件能被单独设计，并且在运行时松散耦合。这些过程可以封装到包（Packages）和堆（Stacks）中，以便于共享和分发。下图是在包和堆在文件中的具体结构：

         Manifests (manifest.xml)：提供关于Package元数据，包括它的许可信息和Package之间依赖关系，以及语言特性信息像编译旗帜（编译优化参数）。
         Stack manifests (stack.xml)：提供关于Stack元数据，包括它的许可信息和Stack之间依赖关系。

四、  社区级

        ROS的社区级概念是ROS网络上进行代码发布的一种表现形式。结构如下图所示：

        代码库的联合系统。使得协作亦能被分发。这种从文件系统级别到社区一级的设计让独立地发展和实施工作成为可能。正是因为这种分布式的结构，似的ROS迅速发展，软件仓库中包的数量指数级增加。

参考资料：

（1）《开源机器人操作系统——ROS》 张建伟等著

（2）《an open-source Robot Operating System》 paper

（3）  willowgarage公司网站：http://www.willowgarage.com/
（4）  ROS官方wiki：http://www.ros.org

----------------------------------------------------------------

ROS探索总结（三）——ROS新手教程

前面我们介绍了ROS的特点和结构，接下来就要开始准备动手感受一下ROS的强大了。ROS官网的wiki上针对新手的教程很详细，最好把所有的新手教程都搞清楚，这是后面开发最基础的东西。尽管如此，ROS对于新手来说还是很难上手，这里，我就来总结一下我当时学习的历程，也为其他新手作为一个参考。

一、ROS的安装

        ROS的安装当然是我们开始动手的第一步了，这里我们使用的操作系统是ubuntu，因为ROS在ubuntu上的支持是最好的。

        如果是新手，我建议使用”apt-get“的方法进行安装，不走很简单，按照wiki上说的，大概半个小时就可以安装完毕完全版的ROS：（现在最新版的ROS是groovy，但是我还是习惯使用fuerte）

        http://ros.org/wiki/fuerte/Installation/Ubuntu

        如果想挑战源码编译，当然也没有问题：

        http://ros.org/wiki/fuerte/Installation/Ubuntu/Source

        安装完毕之后运行一下“roscore”，如果没有问题，安装就成功了！

注：groovy版本的安装：

       “apt-get”：http://www.ros.org/wiki/groovy/Installation/Ubuntu

             源码编译：http://www.ros.org/wiki/groovy/Installation/Source

二、ROS的新手教程

        wiki上的新手教程还是很详细的，对代码都有解释，新手一定要把这些例子和代码搞明白：

        http://www.ros.org/wiki/ROS/Tutorials

        上面的教程都是英文的，如果感觉略有压力（本人就是），可以参考下面这两个博客中的部分翻译：

               http://blog.sina.com.cn/s/articlelist_1712413141_0_1.html

            不过往后面的学习都是英文的资料了，还是要努力适应看英文的文档。

        ROS使用的编程语言主要是C++和python，所以也有针对这两种语言的功能包roscpp和rospy，这两个包的教程与上面的教程基本相似，看完上面的教程也可以看看这两个包的教程：

        http://www.ros.org/wiki/rospy_tutorials

        http://www.ros.org/wiki/roscpp/Tutorials

        努力学习完上面的这些教程，你至少应该明白ROS里面的节点和消息是干什么用的了吧，如果还没理解，那就再多看几遍吧！

三、ROS中的常用功能

         ROS中提供了很多强大的功能，我们学习完上面的基本知识之后要继续进行深入。

         1、rviz       

         rviz是ROS中一款强大的3D可视化工具，这个玩意在后面可是要频繁用到的，是必须要弄明白的， 详细的教程可以参考wiki：

         http://www.ros.org/wiki/rviz 

  

         我们可以在里面创建自己的机器人，并且让机器人动起来。还可以创建地图，显示3D点云等等，总之，想在ROS中显示的东东都可以在这里显示出来。当然这些显示都是通过消息的订阅来完成的，机器人通过ROS发布数据，rviz订阅消息接收数据，然后显示，这些数据也是有一定的数据格式的，可以参考下面的链接：

         http://www.ros.org/wiki/rviz/DisplayTypes

             

                看到上面的机器人了吧，是不是很酷，在rviz中，这样的机器人模型是通过urdf文件描述的，具体urdf文件怎么写，参考wiki：

         http://www.ros.org/wiki/urdf

          2、tf

          tf是ROS中的坐标变换系统，在机器人的建模仿真中经常用到。

         ROS中主要有两种坐标系：

        （1）固定坐标系： 用于表示世界的参考坐标系;

        （2）目标坐标系：相对于摄像机视角的参考坐标系。

        教程见：http://www.ros.org/wiki/tf

        3、gazebo

        这个工具是ROS中的物理仿真环境，gazebo本身就是一款机器人的仿真软件，基于ODE的物理引擎，可以模拟机器人以及环境中的很多物理特性，这个软件可以稍作了解，并不是后面开发所必须要的。

        教程见：http://www.ros.org/wiki/simulator_gazebo/Tutorials

四、ROS常用机器人

        1、PR2

             看ROS的应用时，最常见到的机器人就是PR2。这个机器人是ROS的主要维护者（Willow Garage）针对ROS量身定做的机器人，有两个运行ubuntu和ROS的电脑，和两个机器臂以及很多牛逼的传感器，功能是非常强大的，但是售价那是相当的昂贵，国内很少见到，基本都是在国外的研究所里。这款机器人的ROS包比较多，从仿真到导航，所以代码具有比较高的参考价值，当然新手还是先看看其他机器人的代码再来挑战PR2吧，代码比较庞杂。

       wiki：http://www.ros.org/wiki/Robots/PR2

        2、TurtleBot      

             这个机器人应该算是应用ROS小型移动机器人的典型代表了，资料、文档和代码比较多，主要在建立模型和导航定位方面，代码比较容易理解，可以作为新手参考的最佳机器人了，上面rviz中显示的那个机器人就是它了。

        wiki：http://www.ros.org/wiki/Robots/TurtleBot

        3、Husky、Erratic

      

                 这两款机器人和TurtleBot机器人差不多，都是小型的轮式移动机器人，同样可以作为新手学习的参考：

          http://www.ros.org/wiki/Robots/Husky

          http://www.ros.org/wiki/Robots/Erratic

                 这些教程和机器人基本都是我当时学习的时候研究和了解过的，总结在此也让那些比较迷茫的初学者作为参考，可以尽快了解和掌握ROS。再次强调一下，wiki上的新手教程是一定要先熟悉的，后面的内容在后续学习过程中可以一边做一边学习。

         *此文只代表个人观点，仅作为参考

----------------------------------------------------------------

ROS探索总结（四）——简单的机器人仿真

前边我们已经介绍了ROS的基本情况，以及新手入门ROS的初级教程，现在就要真正的使用ROS进入机器人世界了。接下来我们涉及到的很多例程都是《ROS by Example》这本书的内容，我是和群里的几个人一起从国外的亚马逊上买到的，还是很有参考价值的，不过前提是你已经熟悉之前的新手教程了。

一、ROS by Example

        这本书是关于国外关于ROS出版的第一本书，主要针对Electric和Fuerte版本，使用机器人主要是TurtleBot。书中详细讲解了关于机器人的基本仿真、导航、路径规划、图像处理、语音识别等等，而且在google的svn上发布了所有代码，可以通过以下命令下载、编译：

[plain]  view plain  copy

1. svn checkout http://ros-by-example.googlecode.com/svn/trunk/rbx_vol_1  
2. rosmake rbx_vol_1  
3. rospack profile          //加入ROS package路径  

二、rviz简单机器人模拟

       1、安装机器人模拟器                       

       rviz是一个显示机器人实体的工具，本身不具有模拟的功能，需要安装一个模拟器arbotix。

[plain]  view plain  copy

1. svn checkout http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix  
2. rosmake arbotix  

       

       2、TurtleBot机器人的模拟

       在书中的rbx_vol_1包里已经为我们写好了模拟的代码，我们先进行实验，完成后再仔细研究代码。
       机器人模拟运行：

[plain]  view plain  copy

1. roscore  
2. roslaunch rbx1_bringup fake_pi_robot.launch  

       然后在终端中可以看到，机器人已经开始运行了，打开rviz界面，才能看到机器人实体。

[plain]  view plain  copy

1. rosrun rviz rviz -d `rospack find rbx1_nav`/sim_fuerte.vcg  

       后面的参数是加载了rviz的配置文件sim_fuerte.vcg。效果如下：

  

       此时的机器人是静止的，需要发布一个消息才能让它动起来。

[plain]  view plain  copy

1. rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'  

       

       如果要让机器人停下来，需要在中断中按下“Ctrl+c”，然后输入：

[plain]  view plain  copy

1. rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'  

       也可以改变发送的topic信息，使机器人走出不同的轨迹。

三、实现分析

       按照上面的仿真过程，我们详细分析每一步的代码实现。   

       1、TurtleBot机器人运行

       机器人运行使用的是launch文件，首先打开fake_turtlebot.launch文件。 

[plain]  view plain  copy

1. <launch>  
2.   <param name="/use_sim_time" value="false" />  
3.   
4.   <!-- Load the URDF/Xacro model of our robot -->  
5.   <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />  
6.      
7.   <param name="robot_description" command="$(arg urdf_file)" />  
8.       
9.   <node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen">  
10.       <rosparam file="$(find rbx1_bringup)/config/fake_turtlebot_arbotix.yaml" command="load" />  
11.       <param name="sim" value="true"/>  
12.   </node>  
13.     
14.   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">  
15.       <param name="publish_frequency" type="double" value="20.0" />  
16.   </node>  
17.     
18.   <!-- We need a static transforms for the wheels -->  
19.   <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_wheel_link 100" />  
20.   <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_wheel_link 100" />  
21.   
22. </launch>  

        文件可以大概分为四个部分：
        （1） 从指定的包中加载urdf文件
        （2） 启动arbotix模拟器
        （3） 启动状态发布节点
        （4） tf坐标系配置

2、rviz配置文件

       在打开rviz的时候需要加载一个.vcg的配置文件，主要对rviz中的插件选项进行默认的配置。这里打开的是sim_fuerte.vcg文件，由于文件比较长，这里只列举重点的部分。

[plain]  view plain  copy

1. Background\ ColorB=0.12549  
2. Background\ ColorG=0.12549  
3. Background\ ColorR=0.12549  
4. Camera\ Config=158.108 0.814789 0.619682 -1.57034  
5. Camera\ Type=rviz::FixedOrientationOrthoViewController  
6. Fixed\ Frame=/odom  
7. Grid.Alpha=0.5  
8. Grid.Cell\ Size=0.5  
9. Grid.ColorB=0.941176  
10. Grid.ColorG=0.941176  
11. Grid.ColorR=0.941176  
12. Grid.Enabled=1  
13. Grid.Line\ Style=0  
14. Grid.Line\ Width=0.03  
15. Grid.Normal\ Cell\ Count=0  
16. Grid.OffsetX=0  
17. Grid.OffsetY=0  
18. Grid.OffsetZ=0  
19. Grid.Plane=0  

        上面的代码是配置背景颜色和网格属性的，对应rviz中的选项如下图所示。

        

        其中比较重要的一个选项是Camera的type，这个选项是控制开发者的观察角度的，书中用的是FixedOrientationOrthoViewController的方式，就是上面图中的俯视角度，无法看到机器人的三维全景，所以可以改为OrbitViewController方式，如下图所示：

3、发布topic

        要让机器人动起来，还需要给他一些运动需要的信息，这些信息都是通过topic的方式发布的。
        这里的topic就是速度命令，针对这个topic，我们需要发布速度的信息，在ROS中已经为我们写好了一些可用的数据结构，这里用的是Twist信息的数据结构。在终端中可以看到Twist的结构如下：

              

        用下面的命令进行消息的发布，其中主要包括力的大小和方向。

[plain]  view plain  copy

1. Background\ ColorB=0.12549  
2. Background\ ColorG=0.12549  
3. Background\ ColorR=0.12549  
4. Camera\ Config=158.108 0.814789 0.619682 -1.57034  
5. Camera\ Type=rviz::FixedOrientationOrthoViewController  
6. Fixed\ Frame=/odom  
7. Grid.Alpha=0.5  
8. Grid.Cell\ Size=0.5  
9. Grid.ColorB=0.941176  
10. Grid.ColorG=0.941176  
11. Grid.ColorR=0.941176  
12. Grid.Enabled=1  
13. Grid.Line\ Style=0  
14. Grid.Line\ Width=0.03  
15. Grid.Normal\ Cell\ Count=0  
16. Grid.OffsetX=0  
17. Grid.OffsetY=0  
18. Grid.OffsetZ=0  
19. Grid.Plane=0  

4、节点关系图

----------------------------------------------------------------

ROS探索总结（五）——创建简单的机器人模型smartcar

前面我们使用的是已有的机器人模型进行仿真，这一节我们将建立一个简单的智能车机器人smartcar，为后面建立复杂机器人打下基础。

一、创建硬件描述包

[plain]  view plain  copy

1. roscreat-pkg  smartcar_description  urdf  

二、智能车尺寸数据

        因为建立的是一个非常简单的机器人，所以我们尽量使用简单的元素：使用长方体代替车模，使用圆柱代替车轮，具体尺寸如下：

三、建立urdf文件

        在smartcar_description文件夹下建立urdf文件夹，创建智能车的描述文件smartcar.urdf，描述代码如下：

[plain]  view plain  copy

1. <?xml version="1.0"?>  
2. <robot name="smartcar">  
3.   <link name="base_link">  
4.     <visual>  
5.       <geometry>  
6.         <box size="0.25 .16 .05"/>  
7.     </geometry>  
8.     <origin rpy="0 0 1.57075" xyz="0 0 0"/>  
9.     <material name="blue">  
10.         <color rgba="0 0 .8 1"/>  
11.     </material>  
12.     </visual>  
13.  </link>  
14.   
15.  <link name="right_front_wheel">  
16.     <visual>  
17.       <geometry>  
18.         <cylinder length=".02" radius="0.025"/>  
19.       </geometry>  
20.       <material name="black">  
21.         <color rgba="0 0 0 1"/>  
22.       </material>  
23.     </visual>  
24.   </link>  
25.   
26.   <joint name="right_front_wheel_joint" type="continuous">  
27.     <axis xyz="0 0 1"/>  
28.     <parent link="base_link"/>  
29.     <child link="right_front_wheel"/>  
30.     <origin rpy="0 1.57075 0" xyz="0.08 0.1 -0.03"/>  
31.     <limit effort="100" velocity="100"/>  
32.     <joint_properties damping="0.0" friction="0.0"/>  
33.   </joint>  
34.   
35.   <link name="right_back_wheel">  
36.     <visual>  
37.       <geometry>  
38.         <cylinder length=".02" radius="0.025"/>  
39.       </geometry>  
40.       <material name="black">  
41.         <color rgba="0 0 0 1"/>  
42.       </material>  
43.     </visual>  
44.   </link>  
45.   
46.   <joint name="right_back_wheel_joint" type="continuous">  
47.     <axis xyz="0 0 1"/>  
48.     <parent link="base_link"/>  
49.     <child link="right_back_wheel"/>  
50.     <origin rpy="0 1.57075 0" xyz="0.08 -0.1 -0.03"/>  
51.     <limit effort="100" velocity="100"/>  
52.     <joint_properties damping="0.0" friction="0.0"/>  
53.  </joint>  
54.   
55.  <link name="left_front_wheel">  
56.     <visual>  
57.       <geometry>  
58.         <cylinder length=".02" radius="0.025"/>  
59.       </geometry>  
60.       <material name="black">  
61.         <color rgba="0 0 0 1"/>  
62.       </material>  
63.     </visual>  
64.   </link>  
65.   
66.   <joint name="left_front_wheel_joint" type="continuous">  
67.     <axis xyz="0 0 1"/>  
68.     <parent link="base_link"/>  
69.     <child link="left_front_wheel"/>  
70.     <origin rpy="0 1.57075 0" xyz="-0.08 0.1 -0.03"/>  
71.     <limit effort="100" velocity="100"/>  
72.     <joint_properties damping="0.0" friction="0.0"/>  
73.   </joint>  
74.   
75.   <link name="left_back_wheel">  
76.     <visual>  
77.       <geometry>  
78.         <cylinder length=".02" radius="0.025"/>  
79.       </geometry>  
80.       <material name="black">  
81.         <color rgba="0 0 0 1"/>  
82.       </material>  
83.     </visual>  
84.   </link>  
85.   
86.   <joint name="left_back_wheel_joint" type="continuous">  
87.     <axis xyz="0 0 1"/>  
88.     <parent link="base_link"/>  
89.     <child link="left_back_wheel"/>  
90.     <origin rpy="0 1.57075 0" xyz="-0.08 -0.1 -0.03"/>  
91.     <limit effort="100" velocity="100"/>  
92.     <joint_properties damping="0.0" friction="0.0"/>  
93.   </joint>  
94.   
95.   <link name="head">  
96.     <visual>  
97.       <geometry>  
98.         <box size=".02 .03 .03"/>  
99.       </geometry>  
100.       <material name="white">  
101.           <color rgba="1 1 1 1"/>  
102.       </material>  
103.     </visual>  
104.   </link>  
105.   
106.   <joint name="tobox" type="fixed">  
107.     <parent link="base_link"/>  
108.     <child link="head"/>  
109.     <origin xyz="0 0.08 0.025"/>  
110.   </joint>  
111. </robot>  

四、建立launch命令文件

        在smartcar_description文件夹下建立launch文件夹，创建智能车的描述文件 base.urdf.rviz.launch，描述代码如下：

[plain]  view plain  copy

1. <launch>  
2.     <arg name="model" />  
3.     <arg name="gui" default="False" />  
4.     <param name="robot_description" textfile="$(find smartcar_description)/urdf/smartcar.urdf" />  
5.     <param name="use_gui" value="$(arg gui)"/>  
6.     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>  
7.     <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />  
8.     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" />  
9. </launch>  

五、效果演示

        在终端中输入显示命令：

[plain]  view plain  copy

1. roslaunch  smartcar_description  base.urdf.rviz.launch  gui:=true  

        显示效果如下图所示，使用gui中的控制bar可以控制四个轮子单独旋转。

----------------------------------------------------------------

ROS探索总结（六）——使用smartcar进行仿真

之前的博客中，我们使用rviz进行了TurtleBot的仿真，而且使用urdf文件建立了自己的机器人smartcar，本篇博客是将两者进行结合，使用smartcar机器人在rviz中进行仿真。

一、模型完善

        之前我们使用的都是urdf文件格式的模型，在很多情况下，ROS对urdf文件的支持并不是很好，使用宏定义的.xacro文件兼容性更好，扩展性也更好。所以我们把之前的urdf文件重新整理编写成.xacro文件。
        .xacro文件主要分为三部分：

1、机器人主体

[plain]  view plain  copy

1. <?xml version="1.0"?>  
2. <robot name="smartcar" xmlns:xacro="http://ros.org/wiki/xacro">  
3.   <property name="M_PI" value="3.14159"/>  
4.   
5.   <!-- Macro for SmartCar body. Including Gazebo extensions, but does not include Kinect -->  
6.   <include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/>  
7.   
8.   <property name="base_x" value="0.33" />  
9.   <property name="base_y" value="0.33" />  
10.   
11.   <xacro:macro name="smartcar_body">  
12.   
13.   
14.     <link name="base_link">  
15.     <inertial>  
16.       <origin xyz="0 0 0.055"/>  
17.       <mass value="1.0" />  
18.       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
19.     </inertial>  
20.     <visual>  
21.       <geometry>  
22.         <box size="0.25 .16 .05"/>  
23.       </geometry>  
24.       <origin rpy="0 0 0" xyz="0 0 0.055"/>  
25.       <material name="blue">  
26.       <color rgba="0 0 .8 1"/>  
27.       </material>  
28.    </visual>  
29.    <collision>  
30.       <origin rpy="0 0 0" xyz="0 0 0.055"/>  
31.       <geometry>  
32.         <box size="0.25 .16 .05" />  
33.       </geometry>  
34.     </collision>  
35.   </link>  
36.   
37.   
38.  <link name="left_front_wheel">  
39.     <inertial>  
40.       <origin  xyz="0.08 0.08 0.025"/>  
41.       <mass value="0.1" />  
42.        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
43.     </inertial>  
44.     <visual>  
45.       <geometry>  
46.         <cylinder length=".02" radius="0.025"/>  
47.       </geometry>  
48.       <material name="black">  
49.         <color rgba="0 0 0 1"/>  
50.       </material>  
51.     </visual>  
52.     <collision>  
53.       <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>  
54.       <geometry>  
55.          <cylinder length=".02" radius="0.025"/>  
56.       </geometry>  
57.     </collision>  
58.   </link>  
59.   
60.   <joint name="left_front_wheel_joint" type="continuous">  
61.     <axis xyz="0 0 1"/>  
62.     <parent link="base_link"/>  
63.     <child link="left_front_wheel"/>  
64.     <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>  
65.     <limit effort="100" velocity="100"/>  
66.     <joint_properties damping="0.0" friction="0.0"/>  
67.   </joint>  
68.   
69.   <link name="right_front_wheel">  
70.     <inertial>  
71.       <origin xyz="0.08 -0.08 0.025"/>  
72.       <mass value="0.1" />  
73.        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
74.     </inertial>  
75.     <visual>  
76.       <geometry>  
77.         <cylinder length=".02" radius="0.025"/>  
78.       </geometry>  
79.       <material name="black">  
80.         <color rgba="0 0 0 1"/>  
81.       </material>  
82.     </visual>  
83.     <collision>  
84.       <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>  
85.       <geometry>  
86.          <cylinder length=".02" radius="0.025"/>  
87.       </geometry>  
88.     </collision>  
89.   </link>  
90.   
91.   <joint name="right_front_wheel_joint" type="continuous">  
92.     <axis xyz="0 0 1"/>  
93.     <parent link="base_link"/>  
94.     <child link="right_front_wheel"/>  
95.     <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>  
96.     <limit effort="100" velocity="100"/>  
97.     <joint_properties damping="0.0" friction="0.0"/>  
98.  </joint>  
99.   
100.  <link name="left_back_wheel">  
101.     <inertial>  
102.       <origin xyz="-0.08 0.08 0.025"/>  
103.       <mass value="0.1" />  
104.        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
105.     </inertial>  
106.     <visual>  
107.       <geometry>  
108.         <cylinder length=".02" radius="0.025"/>  
109.       </geometry>  
110.       <material name="black">  
111.         <color rgba="0 0 0 1"/>  
112.       </material>  
113.    </visual>  
114.    <collision>  
115.        <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>  
116.       <geometry>  
117.          <cylinder length=".02" radius="0.025"/>  
118.       </geometry>  
119.     </collision>  
120.   </link>  
121.   
122.   <joint name="left_back_wheel_joint" type="continuous">  
123.     <axis xyz="0 0 1"/>  
124.     <parent link="base_link"/>  
125.     <child link="left_back_wheel"/>  
126.     <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>  
127.     <limit effort="100" velocity="100"/>  
128.     <joint_properties damping="0.0" friction="0.0"/>  
129.   </joint>  
130.   
131.   <link name="right_back_wheel">  
132.     <inertial>  
133.        <origin xyz="-0.08 -0.08 0.025"/>  
134.        <mass value="0.1" />  
135.        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
136.     </inertial>  
137.     <visual>  
138.       <geometry>  
139.         <cylinder length=".02" radius="0.025"/>  
140.       </geometry>  
141.       <material name="black">  
142.         <color rgba="0 0 0 1"/>  
143.       </material>  
144.    </visual>  
145.    <collision>  
146.       <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>  
147.       <geometry>  
148.          <cylinder length=".02" radius="0.025"/>  
149.       </geometry>  
150.     </collision>  
151.   </link>  
152.   
153.   
154.   <joint name="right_back_wheel_joint" type="continuous">  
155.     <axis xyz="0 0 1"/>  
156.     <parent link="base_link"/>  
157.     <child link="right_back_wheel"/>  
158.     <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>  
159.     <limit effort="100" velocity="100"/>  
160.     <joint_properties damping="0.0" friction="0.0"/>  
161.   </joint>  
162.   
163.   <link name="head">  
164.     <inertial>  
165.       <origin xyz="0.08 0 0.08"/>  
166.       <mass value="0.1" />  
167.       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>  
168.     </inertial>  
169.     <visual>  
170.       <geometry>  
171.         <box size=".02 .03 .03"/>  
172.       </geometry>  
173.       <material name="white">  
174.         <color rgba="1 1 1 1"/>  
175.       </material>  
176.      </visual>  
177.      <collision>  
178.       <origin xyz="0.08 0 0.08"/>  
179.       <geometry>  
180.          <cylinder length=".02" radius="0.025"/>  
181.       </geometry>  
182.     </collision>  
183.   </link>  
184.   
185.   <joint name="tobox" type="fixed">  
186.     <parent link="base_link"/>  
187.     <child link="head"/>  
188.     <origin xyz="0.08 0 0.08"/>  
189.   </joint>  
190.   </xacro:macro>  
191.   
192. </robot>  

2、gazebo属性部分

[plain]  view plain  copy

1. <?xml version="1.0"?>  
2.   
3. <robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"   
4.     xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"   
5.     xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"   
6.     xmlns:xacro="http://ros.org/wiki/xacro"   
7.     name="smartcar_gazebo">  
8.   
9. <!-- ASUS Xtion PRO camera for simulation -->  
10. <!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package -->  
11. <xacro:macro name="smartcar_sim">  
12.     <gazebo reference="base_link">  
13.         <material>Gazebo/Blue</material>  
14.     </gazebo>  
15.   
16.     <gazebo reference="right_front_wheel">  
17.         <material>Gazebo/FlatBlack</material>  
18.     </gazebo>  
19.   
20.     <gazebo reference="right_back_wheel">  
21.         <material>Gazebo/FlatBlack</material>  
22.     </gazebo>  
23.   
24.     <gazebo reference="left_front_wheel">  
25.         <material>Gazebo/FlatBlack</material>  
26.     </gazebo>  
27.   
28.     <gazebo reference="left_back_wheel">  
29.         <material>Gazebo/FlatBlack</material>  
30.     </gazebo>  
31.   
32.     <gazebo reference="head">  
33.         <material>Gazebo/White</material>  
34.     </gazebo>  
35.   
36. </xacro:macro>  
37.   
38. </robot>  

3、主文件

[cpp]  view plain  copy

1. <span style="font-size:14px;"><?xml version="1.0"?>  
2.   
3. <robot name="smartcar"    
4.     xmlns:xi="http://www.w3.org/2001/XInclude"  
5.     xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"  
6.     xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"  
7.     xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"  
8.     xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"  
9.     xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"  
10.     xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"  
11.     xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"  
12.     xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"  
13.     xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"  
14.     xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"  
15.     xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"  
16.     xmlns:xacro="http://ros.org/wiki/xacro">  
17.   
18.   <include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" />  
19.   
20.   <!-- Body of SmartCar, with plates, standoffs and Create (including sim sensors) -->  
21.   <smartcar_body/>  
22.   
23.   <smartcar_sim/>  
24.   
25. </robot></span>  

二、lanuch文件

        在launch文件中要启动节点和模拟器。

[plain]  view plain  copy

1. <launch>  
2.     <param name="/use_sim_time" value="false" />  
3.       
4.     <!-- Load the URDF/Xacro model of our robot -->  
5.     <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />  
6.     <arg name="gui" default="false" />  
7.   
8.     <param name="robot_description" command="$(arg urdf_file)" />  
9.     <param name="use_gui" value="$(arg gui)"/>  
10.   
11.     <node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen">  
12.         <rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" />  
13.         <param name="sim" value="true"/>  
14.     </node>  
15.   
16.     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >  
17.     </node>  
18.   
19.     <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">  
20.         <param name="publish_frequency" type="double" value="20.0" />  
21.     </node>  
22.   
23.      <!-- We need a static transforms for the wheels -->  
24.     <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" />  
25.     <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" />  
26.   
27.     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.vcg" />  
28. </launch>  

三、仿真测试

        首先运行lanuch，既可以看到rviz中的机器人：

[plain]  view plain  copy

1. roslaunch smartcar_description smartcar_display.rviz.launch  

        

         发布一条动作的消息。

[plain]  view plain  copy

1. rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'  

四、节点关系

----------------------------------------------------------------

ROS探索总结（七）——smartcar源码上传

看到前面写的博客还是帮助了很多ROS的学习者，我感到非常荣幸。其实我也是一名ROS的新手，ROS的相关资料少，上手难度大，我现在也在摸索着学习，还希望大家都将自己的学习成果在网上或者ROS群里分享。

        我看到有些人在运行我前面写的smartcar程序，为了方便大家的学习，我这两天整理了一下代码，已经上传到csdn上，下载请见：

        http://download.csdn.net/detail/hcx25909/5487985

        代码的内容主要是前边用到的机器人smartcar的urdf文件，已经运行使用的launch文件，大家下载后可以直接编译使用，由于代码在我的计算机上编译时带有路径信息，所以大家编译的时候请使用下面的命令先清除再编译：

[cpp]  view plain  copy

1. rosmake smartcar_description --pre-clean  

       代码中的urdf文件里室机器人的urdf描述，和上一篇博客中讲的是一样的，launch文件夹中主要有以下文件：

        第一个实在gazebo仿真中用到的，需要先运行gazebo，然后再运行。但是貌似有问题，我每次加载到gazebo中的时候似乎物理参数不对，机器人在gazebo里飞来飞去的，至今还没有解决。如果哪位仁兄发现问题了，还请多多指教。

        第二个文件是我在绘制模型的时候用到的，主要功能是在rviz中显示机器人模型，文件中的以下代码：

[cpp]  view plain  copy

1. <arg name="gui" default="false" />  

        如果把false改为true，则会打开一个gui的调试界面，可以通过滑动条旋转车轮，如下图所示。默认是false。

        第三个文件是在方针的时候用到的，就是前面博客中使用的，会打开arbotix仿真器。

        根目录中还有rviz的配置文件urdf.vcg，只要大家在rviz中修改界面或者参数，推出的时候保存，就会自动修改这个文件了。

        config文件夹下的是仿真用到的配置文件。

 

PS：       

        代码中还有很多问题，请大家多多指教，共同进步。

----------------------------------------------------------------

ROS探索总结（八）——键盘控制

 如果尝试过前面的例子，有没有感觉每次让机器人移动还要在终端里输入指令，这也太麻烦了，有没有办法通过键盘来控制机器人的移动呢？答案室当然的了。我研究了其他几个机器人键盘控制的代码，还是有所收获的，最后移植到了smartcar上，实验成功。

一、创建控制包

        首先，我们为键盘控制单独建立一个包：

[plain]  view plain  copy

1. roscreate-pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp  
2. rosmake  

        如果你已经忘记了怎么建立包，请参考：http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

二、简单的消息发布

        在机器人仿真中，主要控制机器人移动的就是        在机器人仿真中，主要控制机器人移动的就是Twist()结构，如果我们编程将这个结构通过程序发布成topic，自然就可以控制机器人了。我们先用简单的python来尝试一下。

        之前的模拟中，我们使用的都是在命令行下进行的消息发布，现在我们需要把这些命令转换成python代码，封装到一个单独的节点中去。针对之前的命令行，我们可以很简单的在smartcar_teleop /scripts文件夹下编写如下的控制代码：

[python]  view plain  copy

1. #!/usr/bin/env python  
2. import roslib; roslib.load_manifest('smartcar_teleop')  
3. import rospy  
4. from geometry_msgs.msg import Twist  
5. from std_msgs.msg import String  
6.   
7. class Teleop:  
8.     def __init__(self):  
9.         pub = rospy.Publisher('cmd_vel', Twist)  
10.         rospy.init_node('smartcar_teleop')  
11.         rate = rospy.Rate(rospy.get_param('~hz', 1))  
12.         self.cmd = None  
13.       
14.         cmd = Twist()  
15.         cmd.linear.x = 0.2  
16.         cmd.linear.y = 0  
17.         cmd.linear.z = 0  
18.         cmd.angular.z = 0  
19.         cmd.angular.z = 0  
20.         cmd.angular.z = 0.5  
21.   
22.         self.cmd = cmd  
23.         while not rospy.is_shutdown():  
24.             str = "hello world %s" % rospy.get_time()  
25.             rospy.loginfo(str)  
26.             pub.publish(self.cmd)  
27.             rate.sleep()  
28.   
29. if __name__ == '__main__':Teleop()  

       python代码在ROS重视不需要编译的。先运行之前教程中用到的smartcar机器人，在rviz中进行显示，然后新建终端，输入如下命令：

[plain]  view plain  copy

1. rosrun smartcar_teleop teleop.py  

        也可以建立一个launch文件运行：

[plain]  view plain  copy

1. <launch>  
2.   <arg name="cmd_topic" default="cmd_vel" />  
3.   <node pkg="smartcar_teleop" type="teleop.py" name="smartcar_teleop">  
4.     <remap from="cmd_vel" to="$(arg cmd_topic)" />  
5.   </node>  
6. </launch>  

        在rviz中看一下机器人是不是动起来了！

三、加入键盘控制

        当然前边的程序不是我们要的，我们需要的键盘控制。

1、移植

        因为ROS的代码具有很强的可移植性，所以用键盘控制的代码其实可以直接从其他机器人包中移植过来，在这里我主要参考的是erratic_robot，在这个机器人的代码中有一个erratic_teleop包，可以直接移植过来使用。
        首先，我们将其中src文件夹下的keyboard.cpp代码文件直接拷贝到我们smartcar_teleop包的src文件夹下，然后修改CMakeLists.txt文件，将下列代码加入文件底部：

[plain]  view plain  copy

1. rosbuild_add_boost_directories()  
2. rosbuild_add_executable(smartcar_keyboard_teleop src/keyboard.cpp)  
3. target_link_libraries(smartcar_keyboard_teleop boost_thread)  

        编译完成后，运行smartcar模型。重新打开一个终端，打开键盘控制节点：

        

        在终端中按下键盘里的“W”、“S”、“D”、“A”以及“Shift”键进行机器人的控制。效果如下图：

2、复用 

       因为代码是我们直接复制过来的，其中有很多与之前erratic机器人相关的变量，我们把代码稍作修改，变成自己机器人可读性较强的代码。

[cpp]  view plain  copy

1. #include <termios.h>  
2. #include <signal.h>  
3. #include <math.h>  
4. #include <stdio.h>  
5. #include <stdlib.h>  
6. #include <sys/poll.h>  
7.   
8. #include <boost/thread/thread.hpp>  
9. #include <ros/ros.h>  
10. #include <geometry_msgs/Twist.h>  
11.   
12. #define KEYCODE_W 0x77  
13. #define KEYCODE_A 0x61  
14. #define KEYCODE_S 0x73  
15. #define KEYCODE_D 0x64  
16.   
17. #define KEYCODE_A_CAP 0x41  
18. #define KEYCODE_D_CAP 0x44  
19. #define KEYCODE_S_CAP 0x53  
20. #define KEYCODE_W_CAP 0x57  
21.   
22. class SmartCarKeyboardTeleopNode  
23. {  
24.     private:  
25.         double walk_vel_;  
26.         double run_vel_;  
27.         double yaw_rate_;  
28.         double yaw_rate_run_;  
29.           
30.         geometry_msgs::Twist cmdvel_;  
31.         ros::NodeHandle n_;  
32.         ros::Publisher pub_;  
33.   
34.     public:  
35.         SmartCarKeyboardTeleopNode()  
36.         {  
37.             pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);  
38.               
39.             ros::NodeHandle n_private("~");  
40.             n_private.param("walk_vel", walk_vel_, 0.5);  
41.             n_private.param("run_vel", run_vel_, 1.0);  
42.             n_private.param("yaw_rate", yaw_rate_, 1.0);  
43.             n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);  
44.         }  
45.           
46.         ~SmartCarKeyboardTeleopNode() { }  
47.         void keyboardLoop();  
48.           
49.         void stopRobot()  
50.         {  
51.             cmdvel_.linear.x = 0.0;  
52.             cmdvel_.angular.z = 0.0;  
53.             pub_.publish(cmdvel_);  
54.         }  
55. };  
56.   
57. SmartCarKeyboardTeleopNode* tbk;  
58. int kfd = 0;  
59. struct termios cooked, raw;  
60. bool done;  
61.   
62. int main(int argc, char** argv)  
63. {  
64.     ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);  
65.     SmartCarKeyboardTeleopNode tbk;  
66.       
67.     boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));  
68.       
69.     ros::spin();  
70.       
71.     t.interrupt();  
72.     t.join();  
73.     tbk.stopRobot();  
74.     tcsetattr(kfd, TCSANOW, &cooked);  
75.       
76.     return(0);  
77. }  
78.   
79. void SmartCarKeyboardTeleopNode::keyboardLoop()  
80. {  
81.     char c;  
82.     double max_tv = walk_vel_;  
83.     double max_rv = yaw_rate_;  
84.     bool dirty = false;  
85.     int speed = 0;  
86.     int turn = 0;  
87.       
88.     // get the console in raw mode  
89.     tcgetattr(kfd, &cooked);  
90.     memcpy(&raw, &cooked, sizeof(struct termios));  
91.     raw.c_lflag &=~ (ICANON | ECHO);  
92.     raw.c_cc[VEOL] = 1;  
93.     raw.c_cc[VEOF] = 2;  
94.     tcsetattr(kfd, TCSANOW, &raw);  
95.       
96.     puts("Reading from keyboard");  
97.     puts("Use WASD keys to control the robot");  
98.     puts("Press Shift to move faster");  
99.       
100.     struct pollfd ufd;  
101.     ufd.fd = kfd;  
102.     ufd.events = POLLIN;  
103.       
104.     for(;;)  
105.     {  
106.         boost::this_thread::interruption_point();  
107.           
108.         // get the next event from the keyboard  
109.         int num;  
110.           
111.         if ((num = poll(&ufd, 1, 250)) < 0)  
112.         {  
113.             perror("poll():");  
114.             return;  
115.         }  
116.         else if(num > 0)  
117.         {  
118.             if(read(kfd, &c, 1) < 0)  
119.             {  
120.                 perror("read():");  
121.                 return;  
122.             }  
123.         }  
124.         else  
125.         {  
126.             if (dirty == true)  
127.             {  
128.                 stopRobot();  
129.                 dirty = false;  
130.             }  
131.               
132.             continue;  
133.         }  
134.           
135.         switch(c)  
136.         {  
137.             case KEYCODE_W:  
138.                 max_tv = walk_vel_;  
139.                 speed = 1;  
140.                 turn = 0;  
141.                 dirty = true;  
142.                 break;  
143.             case KEYCODE_S:  
144.                 max_tv = walk_vel_;  
145.                 speed = -1;  
146.                 turn = 0;  
147.                 dirty = true;  
148.                 break;  
149.             case KEYCODE_A:  
150.                 max_rv = yaw_rate_;  
151.                 speed = 0;  
152.                 turn = 1;  
153.                 dirty = true;  
154.                 break;  
155.             case KEYCODE_D:  
156.                 max_rv = yaw_rate_;  
157.                 speed = 0;  
158.                 turn = -1;  
159.                 dirty = true;  
160.                 break;  
161.                   
162.             case KEYCODE_W_CAP:  
163.                 max_tv = run_vel_;  
164.                 speed = 1;  
165.                 turn = 0;  
166.                 dirty = true;  
167.                 break;  
168.             case KEYCODE_S_CAP:  
169.                 max_tv = run_vel_;  
170.                 speed = -1;  
171.                 turn = 0;  
172.                 dirty = true;  
173.                 break;  
174.             case KEYCODE_A_CAP:  
175.                 max_rv = yaw_rate_run_;  
176.                 speed = 0;  
177.                 turn = 1;  
178.                 dirty = true;  
179.                 break;  
180.             case KEYCODE_D_CAP:  
181.                 max_rv = yaw_rate_run_;  
182.                 speed = 0;  
183.                 turn = -1;  
184.                 dirty = true;  
185.                 break;                
186.             default:  
187.                 max_tv = walk_vel_;  
188.                 max_rv = yaw_rate_;  
189.                 speed = 0;  
190.                 turn = 0;  
191.                 dirty = false;  
192.         }  
193.         cmdvel_.linear.x = speed * max_tv;  
194.         cmdvel_.angular.z = turn * max_rv;  
195.         pub_.publish(cmdvel_);  
196.     }  
197. }  

参考链接：http://ros.org/wiki/turtlebot_teleop/Tutorials/Teleoperation
                  http://www.ros.org/wiki/simulator_gazebo/Tutorials/TeleopErraticSimulation

3、创新

        虽然很多机器人的键盘控制使用的都是C++编写的代码，但是考虑到python的强大，我们还是需要尝试使用python来编写程序。
        首先需要理解上面C++程序的流程。在上面的程序中，我们单独创建了一个线程来读取中断中的输入，然后根据输入发布不同的速度和角度消息。介于线程的概念还比较薄弱，在python中使用循环替代线程。然后需要考虑的只是如何使用python来处理中断中的输入字符，通过上网查找资料，发现使用的API和C++的基本是一致的。最终的程序如下：

[python]  view plain  copy

1. #!/usr/bin/env python  
2. # -*- coding: utf-8 -*  
3.   
4. import  os  
5. import  sys  
6. import  tty, termios  
7. import roslib; roslib.load_manifest('smartcar_teleop')  
8. import rospy  
9. from geometry_msgs.msg import Twist  
10. from std_msgs.msg import String  
11.   
12. # 全局变量  
13. cmd = Twist()  
14. pub = rospy.Publisher('cmd_vel', Twist)  
15.   
16. def keyboardLoop():  
17.     #初始化  
18.     rospy.init_node('smartcar_teleop')  
19.     rate = rospy.Rate(rospy.get_param('~hz', 1))  
20.   
21.     #速度变量  
22.     walk_vel_ = rospy.get_param('walk_vel', 0.5)  
23.     run_vel_ = rospy.get_param('run_vel', 1.0)  
24.     yaw_rate_ = rospy.get_param('yaw_rate', 1.0)  
25.     yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)  
26.   
27.     max_tv = walk_vel_  
28.     max_rv = yaw_rate_  
29.   
30.     #显示提示信息  
31.     print "Reading from keyboard"  
32.     print "Use WASD keys to control the robot"  
33.     print "Press Caps to move faster"  
34.     print "Press q to quit"  
35.   
36.     #读取按键循环  
37.     while not rospy.is_shutdown():  
38.         fd = sys.stdin.fileno()  
39.         old_settings = termios.tcgetattr(fd)  
40.         #不产生回显效果  
41.         old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO  
42.         try :  
43.             tty.setraw( fd )  
44.             ch = sys.stdin.read( 1 )  
45.         finally :  
46.             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)  
47.   
48.         if ch == 'w':  
49.             max_tv = walk_vel_  
50.             speed = 1  
51.             turn = 0  
52.         elif ch == 's':  
53.             max_tv = walk_vel_  
54.             speed = -1  
55.             turn = 0  
56.         elif ch == 'a':  
57.             max_rv = yaw_rate_  
58.             speed = 0  
59.             turn = 1  
60.         elif ch == 'd':  
61.             max_rv = yaw_rate_  
62.             speed = 0  
63.             turn = -1  
64.         elif ch == 'W':  
65.             max_tv = run_vel_  
66.             speed = 1  
67.             turn = 0  
68.         elif ch == 'S':  
69.             max_tv = run_vel_  
70.             speed = -1  
71.             turn = 0  
72.         elif ch == 'A':  
73.             max_rv = yaw_rate_run_  
74.             speed = 0  
75.             turn = 1  
76.         elif ch == 'D':  
77.             max_rv = yaw_rate_run_  
78.             speed = 0  
79.             turn = -1  
80.         elif ch == 'q':  
81.             exit()  
82.         else:  
83.             max_tv = walk_vel_  
84.             max_rv = yaw_rate_  
85.             speed = 0  
86.             turn = 0  
87.   
88.         #发送消息  
89.         cmd.linear.x = speed * max_tv;  
90.         cmd.angular.z = turn * max_rv;  
91.         pub.publish(cmd)  
92.         rate.sleep()  
93.         #停止机器人  
94.         stop_robot();  
95.   
96. def stop_robot():  
97.     cmd.linear.x = 0.0  
98.     cmd.angular.z = 0.0  
99.     pub.publish(cmd)  
100.   
101. if __name__ == '__main__':  
102.     try:  
103.         keyboardLoop()  
104.     except rospy.ROSInterruptException:  
105.         pass  

参考链接：http://blog.csdn.net/marising/article/details/3173848
                  http://nullege.com/codes/search/termios.ICANON

四、节点关系图

        代码包我已上传：http://download.csdn.net/detail/hcx25909/5496381

        希望大家一同学习，共同进步！

----------------------------------------------------------------

ROS探索总结（九）——操作杆控制

对于移动机器人，键盘的控制往往满足不了我们的需求，以前看好多电影里边都是用一个摇杆来控制机器人的，简直帅爆了，正好我这里有一个操作杆，那就来尝试感受一下。

        操作杆（joystick）控制会更加有操作感，ROS中的很多机器人也带有操作杆的相关代码，只需要简单的移植即可。我们使用的是赛钛客（saitek）的一款操作杆，如下图所示：

        使用的移植代码是clearpath_husky机器人中的Python代码。
        参考链接： http://www.ros.org/wiki/joy

一、测试操作杆驱动

        首先将操作杆的接口插入电脑，然后在终端中输入：

[plain]  view plain  copy

1. ls /dev/input/   

        显示如下：

        其中的js0就代表我们的操作杆。然后测试操作杆的操作是否有效，输入:

[plain]  view plain  copy

1. sudo jstest /dev/input/js0  

        然后会在终端中显示操作杆的各个控制值的即时值，操作操作杆，如果每个按键和操作都有效，说明操作杆是正常的。最后在ROS中的节点里进行测试。打开joy节点：

[plain]  view plain  copy

1. rosrun joy joy_node  

        再打开一个窗口，输入下面命令，查看数据:

[plain]  view plain  copy

1. rostopic echo joy  

        操作操作杆，窗口下面的数据就开始刷新。

二、控制代码

        从上面的测试中，我们发现在前后左右摇动操作杆时，相应改变的数据是Axes中0号和1号位的数据，也是我们最常用的数据，其他按键对应的位置也可以找到，编程的时候就是利用的这些数据位置，所以一定要找到每个按键的对应编号。

        ROS中已经为我们建立了操作杆的数据结构：

        我们主要用到的就是axes和buttons数据。最终的代码如下：

[python]  view plain  copy

1. import roslib; roslib.load_manifest('smartcar_teleop')  
2. import rospy  
3.   
4. from sensor_msgs.msg import Joy  
5. from geometry_msgs.msg import Twist  
6. from std_msgs.msg import String  
7.   
8. class Teleop:  
9.     def __init__(self):  
10.         rospy.init_node('smartcar_teleop_joy')  
11.   
12.         self.turn_scale = rospy.get_param('~turn_scale')  
13.         self.drive_scale = rospy.get_param('~drive_scale')  
14.         self.deadman_button = rospy.get_param('~deadman_button', 0)  
15.   
16.         self.cmd = None  
17.         cmd_pub = rospy.Publisher('cmd_vel', Twist)  
18.   
19.         announce_pub = rospy.Publisher('/smartcar/announce/teleops',  
20.                                        String, latch=True)  
21.         announce_pub.publish(rospy.get_namespace());  
22.   
23.         rospy.Subscriber("joy", Joy, self.callback)  
24.         rate = rospy.Rate(rospy.get_param('~hz', 20))  
25.           
26.         while not rospy.is_shutdown():  
27.             rate.sleep()  
28.             if self.cmd:  
29.                 cmd_pub.publish(self.cmd)  
30.   
31.     def callback(self, data):  
32.         """ Receive joystick data, formulate Twist message. """  
33.         cmd = Twist()  
34.         cmd.linear.x = data.axes[1] * self.drive_scale  
35.         cmd.angular.z = data.axes[0] * self.turn_scale  
36.   
37.         if data.buttons[self.deadman_button] == 1:  
38.             self.cmd = cmd  
39.         else:  
40.             self.cmd = None  
41.   
42. if __name__ == "__main__": Teleop()  

三、机器人控制

        首先来创建一个launch文件（teleop_joy.launch）:

[plain]  view plain  copy

1. <launch>  
2.   <arg name="drive_speed" default="1.0" />  
3.   <arg name="turn_speed" default="1.0" />  
4.   <arg name="joy_dev" default="/dev/input/js0" />  
5.   <arg name="cmd_topic" default="cmd_vel" />  
6.   
7.   <node pkg="joy" type="joy_node" name="joy_node">  
8.     <param name="dev" value="$(arg joy_dev)" />  
9.     <param name="deadzone" value="0.3" />  
10.   </node>  
11.   
12.   <node pkg="smartcar_teleop" type="teleop_joy.py" name="smartcar_teleop">  
13.     <param name="turn_scale" value="$(arg turn_speed)" />  
14.     <param name="drive_scale" value="$(arg drive_speed)" />  
15.     <remap from="cmd_vel" to="$(arg cmd_topic)" />  
16.   </node>  
17. </launch>  

        在rviz中打开我们的机器人模型，然后打开操作杆的控制节点：

[plain]  view plain  copy

1. roslaunch smartcar_display.rviz.launch  
2. roslaunch smartcar_teleop teleop_joy.launch  

        然后按住刹车键进行操作，机器人就可以开始移动了：

        在新终端中输入：

[plain]  view plain  copy

1. rostopic echo joy  

         可以查看到实时的操作杆控制数据：

四、节点关系图

----------------------------------------------------------------

ROS探索总结（十）——语音控制

 如今语音识别在PC机和智能手机上炒的火热，ROS走在技术的最前沿当然也不会错过这么帅的技术。ROS中使用了CMU Sphinx和Festival开源项目中的代码，发布了独立的语音识别包，而且可以将识别出来的语音转换成文字，然后让机器人智能处理后说话。

一、语音识别包

1、安装

        安装很简单，直接使用ubuntu命令即可，首先安装依赖库：

[plain]  view plain  copy

1. $ sudo apt-get install gstreamer0.10-pocketsphinx  
2. $ sudo apt-get install ros-fuerte-audio-common  
3. $ sudo apt-get install libasound2  

        然后来安装ROS包：

[plain]  view plain  copy

1. $ svn checkout http://albany-ros-pkg.googlecode.com/svn/trunk/rharmony  
2. $ rosmake pocketsphinx  

        其中的核心文件就是nodes文件夹下的recognizer.py文件了。这个文件通过麦克风收集语音信息，然后调用语音识别库进行识别生成文本信息，通过/recognizer/output消息发布，其他节点就可以订阅该消息然后进行相应的处理了。

2、测试

        安装完成后我们就可以运行测试了。
        首先，插入你的麦克风设备，然后在系统设置里测试麦克风是否有语音输入。
        然后，运行包中的测试程序：        

[plain]  view plain  copy

1. $ roslaunch pocketsphinx robocup.launch  

        此时，在终端中会看到一大段的信息。尝试说一些简单的语句，当然，必须是英语，例如：bring me the glass，come with me，看看能不能识别出来。
       《ros by example》这本书中写得测试还是很准确的，但是我在测试中感觉识别相当不准确，可能是我的英语太差了吧。

        

        我们也可以直接看ROS最后发布的结果消息：

[plain]  view plain  copy

1. $ rostopic echo /recognizer/output  

        

二、语音库

1、查看语音库

        这个语音识别时一种离线识别的方法，将一些常用的词汇放到一个文件中，作为识别的文本库，然后分段识别语音信号，最后在库中搜索对应的文本信息。如果想看语音识别库中有哪些文本信息，可以通过下面的指令进行查询：

[plain]  view plain  copy

1. $ roscd pocketsphinx/demo  
2. $ more robocup.corpus  

2、添加语音库

        我们可以自己向语音库中添加其他的文本识别信息，《ros by example》自带的例程中是带有语音识别的例程的，而且有添加语音库的例子。
        首先看看例子中要添加的文本信息：

[plain]  view plain  copy

1. $ roscd rbx1_speech/config  
2. $ more nav_commands.txt  

        

        这就是需要添加的文本，我们也可以修改其中的某些文本，改成自己需要的。
        然后我们要把这个文件在线生成语音信息和库文件，这一步需要登陆网站http://www.speech.cs.cmu.edu/tools/lmtool-new.html，根据网站的提示上传文件，然后在线编译生成库文件。

        把下载的文件都解压放在rbx1_speech包的config文件夹下。我们可以给这些文件改个名字：

[plain]  view plain  copy

1. $ roscd rbx1_speech/config  
2. $ rename -f 's/3026/nav_commands/' *  

        在rbx1_speech/launch文件夹下看看voice_nav_commands.launch这个文件：

[plain]  view plain  copy

1. <launch>  
2. <node name="recognizer" pkg="pocketsphinx" type="recognizer.py"  
3. output="screen">  
4. <param name="lm" value="$(find rbx1_speech)/config/nav_commands.lm"/>  
5. <param name="dict" value="$(find rbx1_speech)/config/nav_commands.dic"/>  
6. </node>  
7. </launch>  

        可以看到，这个launch文件在运行recognizer.py节点的时候使用了我们生成的语音识别库和文件参数，这样就可以实用我们自己的语音库来进行语音识别了。
        通过之前的命令来测试一下效果如何吧：

[plain]  view plain  copy

1. $ roslaunch rbx1_speech voice_nav_commands.launch  
2. $ rostopic echo /recognizer/output  

三、语音控制

        有了语音识别，我们就可以来做很多犀利的应用了，首先先来尝试一下用语音来控制机器人动作。

1、机器人控制节点

        前面说到的recognizer.py会将最后识别的文本信息通过消息发布，那么我们来编写一个机器人控制节点接收这个消息，进行相应的控制即可。
        在pocketsphinx包中本身有一个语音控制发布Twist消息的例程voice_cmd_vel.py，rbx1_speech包对其进行了一些简化修改，在nodes文件夹里可以查看voice_nav.py文件：

[python]  view plain  copy

1. #!/usr/bin/env python  
2.   
3. """ 
4.   voice_nav.py 
5.    
6.   Allows controlling a mobile base using simple speech commands. 
7.    
8.   Based on the voice_cmd_vel.py script by Michael Ferguson in 
9.   the pocketsphinx ROS package. 
10.    
11.   See http://www.ros.org/wiki/pocketsphinx 
12. """  
13.   
14. import roslib; roslib.load_manifest('rbx1_speech')  
15. import rospy  
16. from geometry_msgs.msg import Twist  
17. from std_msgs.msg import String  
18. from math import copysign  
19.   
20. class VoiceNav:  
21.     def __init__(self):  
22.         rospy.init_node('voice_nav')  
23.           
24.         rospy.on_shutdown(self.cleanup)  
25.           
26.         # Set a number of parameters affecting the robot's speed  
27.         self.max_speed = rospy.get_param("~max_speed", 0.4)  
28.         self.max_angular_speed = rospy.get_param("~max_angular_speed", 1.5)  
29.         self.speed = rospy.get_param("~start_speed", 0.1)  
30.         self.angular_speed = rospy.get_param("~start_angular_speed", 0.5)  
31.         self.linear_increment = rospy.get_param("~linear_increment", 0.05)  
32.         self.angular_increment = rospy.get_param("~angular_increment", 0.4)  
33.           
34.         # We don't have to run the script very fast  
35.         self.rate = rospy.get_param("~rate", 5)  
36.         r = rospy.Rate(self.rate)  
37.           
38.         # A flag to determine whether or not voice control is paused  
39.         self.paused = False  
40.           
41.         # Initialize the Twist message we will publish.  
42.         self.cmd_vel = Twist()  
43.   
44.         # Publish the Twist message to the cmd_vel topic  
45.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
46.           
47.         # Subscribe to the /recognizer/output topic to receive voice commands.  
48.         rospy.Subscriber('/recognizer/output', String, self.speech_callback)  
49.           
50.         # A mapping from keywords or phrases to commands  
51.         self.keywords_to_command = {'stop': ['stop', 'halt', 'abort', 'kill', 'panic', 'off', 'freeze', 'shut down', 'turn off', 'help', 'help me'],  
52.                                     'slower': ['slow down', 'slower'],  
53.                                     'faster': ['speed up', 'faster'],  
54.                                     'forward': ['forward', 'ahead', 'straight'],  
55.                                     'backward': ['back', 'backward', 'back up'],  
56.                                     'rotate left': ['rotate left'],  
57.                                     'rotate right': ['rotate right'],  
58.                                     'turn left': ['turn left'],  
59.                                     'turn right': ['turn right'],  
60.                                     'quarter': ['quarter speed'],  
61.                                     'half': ['half speed'],  
62.                                     'full': ['full speed'],  
63.                                     'pause': ['pause speech'],  
64.                                     'continue': ['continue speech']}  
65.           
66.         rospy.loginfo("Ready to receive voice commands")  
67.           
68.         # We have to keep publishing the cmd_vel message if we want the robot to keep moving.  
69.         while not rospy.is_shutdown():  
70.             self.cmd_vel_pub.publish(self.cmd_vel)  
71.             r.sleep()                         
72.               
73.     def get_command(self, data):  
74.         # Attempt to match the recognized word or phrase to the   
75.         # keywords_to_command dictionary and return the appropriate  
76.         # command  
77.         for (command, keywords) in self.keywords_to_command.iteritems():  
78.             for word in keywords:  
79.                 if data.find(word) > -1:  
80.                     return command  
81.           
82.     def speech_callback(self, msg):  
83.         # Get the motion command from the recognized phrase  
84.         command = self.get_command(msg.data)  
85.           
86.         # Log the command to the screen  
87.         rospy.loginfo("Command: " + str(command))  
88.           
89.         # If the user has asked to pause/continue voice control,  
90.         # set the flag accordingly   
91.         if command == 'pause':  
92.             self.paused = True  
93.         elif command == 'continue':  
94.             self.paused = False  
95.           
96.         # If voice control is paused, simply return without  
97.         # performing any action  
98.         if self.paused:  
99.             return         
100.           
101.         # The list of if-then statements should be fairly  
102.         # self-explanatory  
103.         if command == 'forward':      
104.             self.cmd_vel.linear.x = self.speed  
105.             self.cmd_vel.angular.z = 0  
106.               
107.         elif command == 'rotate left':  
108.             self.cmd_vel.linear.x = 0  
109.             self.cmd_vel.angular.z = self.angular_speed  
110.                   
111.         elif command == 'rotate right':    
112.             self.cmd_vel.linear.x = 0        
113.             self.cmd_vel.angular.z = -self.angular_speed  
114.               
115.         elif command == 'turn left':  
116.             if self.cmd_vel.linear.x != 0:  
117.                 self.cmd_vel.angular.z += self.angular_increment  
118.             else:          
119.                 self.cmd_vel.angular.z = self.angular_speed  
120.                   
121.         elif command == 'turn right':      
122.             if self.cmd_vel.linear.x != 0:  
123.                 self.cmd_vel.angular.z -= self.angular_increment  
124.             else:          
125.                 self.cmd_vel.angular.z = -self.angular_speed  
126.                   
127.         elif command == 'backward':  
128.             self.cmd_vel.linear.x = -self.speed  
129.             self.cmd_vel.angular.z = 0  
130.               
131.         elif command == 'stop':   
132.             # Stop the robot!  Publish a Twist message consisting of all zeros.           
133.             self.cmd_vel = Twist()  
134.           
135.         elif command == 'faster':  
136.             self.speed += self.linear_increment  
137.             self.angular_speed += self.angular_increment  
138.             if self.cmd_vel.linear.x != 0:  
139.                 self.cmd_vel.linear.x += copysign(self.linear_increment, self.cmd_vel.linear.x)  
140.             if self.cmd_vel.angular.z != 0:  
141.                 self.cmd_vel.angular.z += copysign(self.angular_increment, self.cmd_vel.angular.z)  
142.               
143.         elif command == 'slower':  
144.             self.speed -= self.linear_increment  
145.             self.angular_speed -= self.angular_increment  
146.             if self.cmd_vel.linear.x != 0:  
147.                 self.cmd_vel.linear.x -= copysign(self.linear_increment, self.cmd_vel.linear.x)  
148.             if self.cmd_vel.angular.z != 0:  
149.                 self.cmd_vel.angular.z -= copysign(self.angular_increment, self.cmd_vel.angular.z)  
150.                   
151.         elif command in ['quarter', 'half', 'full']:  
152.             if command == 'quarter':  
153.                 self.speed = copysign(self.max_speed / 4, self.speed)  
154.           
155.             elif command == 'half':  
156.                 self.speed = copysign(self.max_speed / 2, self.speed)  
157.               
158.             elif command == 'full':  
159.                 self.speed = copysign(self.max_speed, self.speed)  
160.               
161.             if self.cmd_vel.linear.x != 0:  
162.                 self.cmd_vel.linear.x = copysign(self.speed, self.cmd_vel.linear.x)  
163.   
164.             if self.cmd_vel.angular.z != 0:  
165.                 self.cmd_vel.angular.z = copysign(self.angular_speed, self.cmd_vel.angular.z)  
166.                   
167.         else:  
168.             return  
169.   
170.         self.cmd_vel.linear.x = min(self.max_speed, max(-self.max_speed, self.cmd_vel.linear.x))  
171.         self.cmd_vel.angular.z = min(self.max_angular_speed, max(-self.max_angular_speed, self.cmd_vel.angular.z))  
172.   
173.     def cleanup(self):  
174.         # When shutting down be sure to stop the robot!  
175.         twist = Twist()  
176.         self.cmd_vel_pub.publish(twist)  
177.         rospy.sleep(1)  
178.   
179. if __name__=="__main__":  
180.     try:  
181.         VoiceNav()  
182.         rospy.spin()  
183.     except rospy.ROSInterruptException:  
184.         rospy.loginfo("Voice navigation terminated.")  

        可以看到，代码中定义了接收到各种命令时的控制策略。

2、仿真测试

        和前面一样，我们在rviz中进行仿真测试。
        首先是运行一个机器人模型：

[plain]  view plain  copy

1. $ roslaunch rbx1_bringup fake_turtlebot.launch  

        

        然后打开rviz：

[plain]  view plain  copy

1. $ rosrun rviz rviz -d `rospack find rbx1_nav`/sim_fuerte.vcg  

        如果不喜欢在终端里看输出，可以打开gui界面：

[plain]  view plain  copy

1. $ rxconsole  

        再打开语音识别的节点：

[plain]  view plain  copy

1. $ roslaunch rbx1_speech voice_nav_commands.launch  

        最后就是机器人的控制节点了：

[plain]  view plain  copy

1. $ roslaunch rbx1_speech turtlebot_voice_nav.launch  

       OK，打开上面这一堆的节点之后，就可以开始了。可以使用的命令如下：

        下图是我的测试结果，不过感觉准确度还是欠佳：

四、播放语音

        现在机器人已经可以按照我们说的话行动了，要是机器人可以和我们对话就更好了。ROS中已经集成了这样的包，下面就来尝试一下。
        运行下面的命令：

[plain]  view plain  copy

1. $ rosrun sound_play soundplay_node.py  
2. $ rosrun sound_play say.py "Greetings Humans. Take me to your leader."  

        有没有听见声音！ROS通过识别我们输入的文本，让机器人读了出来。发出这个声音的人叫做kal_diphone，如果不喜欢，我们也可以换一个人来读：

[plain]  view plain  copy

1. $ sudo apt-get install festvox-don  
2. $ rosrun sound_play say.py "Welcome to the future" voice_don_diphone  

       哈哈，这回换了一个人吧，好吧，这不也是我们的重点。
       在rbx1_speech/nodes文件夹中有一个让机器人说话的节点talkback.py:

[python]  view plain  copy

1. #!/usr/bin/env python  
2.   
3. """ 
4.     talkback.py - Version 0.1 2012-01-10 
5.      
6.     Use the sound_play client to say back what is heard by the pocketsphinx recognizer. 
7.      
8.     Created for the Pi Robot Project: http://www.pirobot.org 
9.     Copyright (c) 2012 Patrick Goebel.  All rights reserved. 
10.  
11.     This program is free software; you can redistribute it and/or modify 
12.     it under the terms of the GNU General Public License as published by 
13.     the Free Software Foundation; either version 2 of the License, or 
14.     (at your option) any later version.5 
15.      
16.     This program is distributed in the hope that it will be useful, 
17.     but WITHOUT ANY WARRANTY; without even the implied warranty of 
18.     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
19.     GNU General Public License for more details at: 
20.      
21.     http://www.gnu.org/licenses/gpl.htmlPoint 
22. """  
23.   
24. import roslib; roslib.load_manifest('rbx1_speech')  
25. import rospy  
26. from std_msgs.msg import String  
27. from sound_play.libsoundplay import SoundClient  
28. import sys  
29.   
30. class TalkBack:  
31.     def __init__(self, script_path):  
32.         rospy.init_node('talkback')  
33.   
34.         rospy.on_shutdown(self.cleanup)  
35.           
36.         # Set the default TTS voice to use  
37.         self.voice = rospy.get_param("~voice", "voice_don_diphone")  
38.           
39.         # Set the wave file path if used  
40.         self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")  
41.           
42.         # Create the sound client object  
43.         self.soundhandle = SoundClient()  
44.           
45.         # Wait a moment to let the client connect to the  
46.         # sound_play server  
47.         rospy.sleep(1)  
48.           
49.         # Make sure any lingering sound_play processes are stopped.  
50.         self.soundhandle.stopAll()  
51.           
52.         # Announce that we are ready for input  
53.         self.soundhandle.playWave(self.wavepath + "/R2D2a.wav")  
54.         rospy.sleep(1)  
55.         self.soundhandle.say("Ready", self.voice)  
56.           
57.         rospy.loginfo("Say one of the navigation commands...")  
58.   
59.         # Subscribe to the recognizer output and set the callback function  
60.         rospy.Subscriber('/recognizer/output', String, self.talkback)  
61.           
62.     def talkback(self, msg):  
63.         # Print the recognized words on the screen  
64.         rospy.loginfo(msg.data)  
65.           
66.         # Speak the recognized words in the selected voice  
67.         self.soundhandle.say(msg.data, self.voice)  
68.           
69.         # Uncomment to play one of the built-in sounds  
70.         #rospy.sleep(2)  
71.         #self.soundhandle.play(5)  
72.           
73.         # Uncomment to play a wave file  
74.         #rospy.sleep(2)  
75.         #self.soundhandle.playWave(self.wavepath + "/R2D2a.wav")  
76.   
77.     def cleanup(self):  
78.         self.soundhandle.stopAll()  
79.         rospy.loginfo("Shutting down talkback node...")  
80.   
81. if __name__=="__main__":  
82.     try:  
83.         TalkBack(sys.path[0])  
84.         rospy.spin()  
85.     except rospy.ROSInterruptException:  
86.         rospy.loginfo("Talkback node terminated.")  

         我们来运行看一下效果：

[plain]  view plain  copy

1. $ roslaunch rbx1_speech talkback.launch  

         然后再说话，ROS不仅将文本信息识别出来了，而且还读了出来，厉害吧。当然了，现在还没有加入什么人工智能的算法，不能让机器人和我们聪明的说话，不过这算是基础了，以后有时间再研究一下人工智能就更犀利了。

----------------------------------------------------------------

ROS探索总结（十一）——机器视觉

机器视觉在计算机时代已经越来越流行，摄像头价格越来越低廉，部分集成深度传感器的混合型传感器也逐渐在研究领域普及，例如微软推出的Kinect，而且与之配套的软件功能十分强大，为开发带来了极大的便利。ROS集成了Kinect的的驱动包OpenNI，而且使用OpenCV库可以进行多种多样的图像处理。

        注：本章内容及代码均参考《ROS by Example》书中的第十章。

一、图像显示

        我们从最基础的开始，想办法显示Kinect的图像数据。

1、安装驱动包

       安装步骤很简单：

[plain]  view plain  copy

1. $sudo apt-get install ros-fuerte-openni-kinect  

2、测试

        首先运行kinect节点：

[plain]  view plain  copy

1. $roslaunch rbx1_vision openni_node_fuerte.launch  

        然后我们调用ROS的image_view包来直接显示摄像头的数据库。image_view包的介绍可以参考：

        http://www.ros.org/wiki/image_view。 

[plain]  view plain  copy

1. $rosrun image_view image_view image:=/camera/rgb/image_color  

        我们可以看到弹出了一个独立的图像显示框：

3、分析数据

        下图是我们上面测试中的节点图。

        我们可以使用如下的命令来查看节点之间发送的图像消息是什么样的：

[plain]  view plain  copy

1. rostopic echo /camera/rgb/image_color  

        然后就会看到数据刷刷的在显示，有没有感觉看不清楚，我们使用终端的重定向功能将数据全部存储到文件中：

[plain]  view plain  copy

1. rostopic echo /camera/rgb/image_color > test  

        好了，现在看看文件中是不是已经有数据了，我们来看一下其中的一帧数据：

[plain]  view plain  copy

1. header:   
2.   seq: 19285  
3.   stamp:   
4.     secs: 1370867560  
5.     nsecs: 538447820  
6.   frame_id: camera_rgb_optical_frame  
7. height: 240  
8. width: 320  
9. encoding: rgb8  
10. is_bigendian: 0  
11. step: 960  
12. data: [223, 225, 225, 220, 225, 225……………..  

        从数据中我们可以的出来几个很重要的参数，首先是图像的分辨率：240*320，编码的格式是rgb8，也就是说图像应该有240*320=76800个像素，而每个像素由八位的R、G、B三个数据组成，因此我们可以预计下面的data中应该有76800*3=230400个八位的数据了。
        我们可以验证一下data中到底有多少个数据，这个很简单了，数一下就行了，不过好像有点多，我使用的是linux的“wc”命令。首先我一帧数据复制到单独的文件中，每个数据都是用“，”号隔开的，只要计算“，”的数量就知道数据的数量了。

        结果和我们预想的是一样的。知道这个数据格式以后，我们以后就可以直接把其他摄像头的数据装换成这种格式的数据就可以直接在ROS中使用了。

4、rviz显示图像

        我们再来点高级的。rviz是我们经常使用的工具，把图像显示在rviz中才更有应用价值。rviz已经为我们提供好了显示图像的接口，使用非常简单。
        首先按照上一节的方法运行kinect节点，然后打开rviz：

[plain]  view plain  copy

1. rosrun rviz rviz  

        然后修改“Fixed Frame”为/camera_rgb_color，修改“Target Frame”为<Fixed Frame>，接着点击add，选择camera类型。添加成功后选择camera菜单下的Iamge Topic选项，选择/camera/rgb/image_color，确定后下面的显示框内就显示图像了。

二、深度显示

        使用kinect的一大特色就是可以获得传感器的深度数据，也就是物体距离传感器的距离，传说kinect的可识别范围在60cm到10m之间。  

1、显示深度图像

        首先也需要运行Kinect的节点：

[plain]  view plain  copy

1. roslaunch openni_launch openni.launch  

        这一步我使用的是ROS安装openni包中的节点，使用与前面相同的命令运行的节点在后面无法产生深度数据。
然后同样适用iamge_view包就可以简单的显示深度图像了：

[plain]  view plain  copy

1. rosrun image_view disparity_view image:=/camera/depth/disparity  

        

2、rviz中显示深度

        首先运行rviz：

[plain]  view plain  copy

1. rosrun rviz rviz  

        然后修改“Fixed Frame”和“Target Frame”，在add中添加PointCloud2类型，修改“topic”，具体参数如下图所示

：

----------------------------------------------------------------

ROS探索总结（十二）——坐标系统

在机器人的控制中，坐标系统是非常重要的，在ROS使用tf软件库进行坐标转换。

        相关链接：http://www.ros.org/wiki/tf/Tutorials#Learning_tf

一、tf简介

        我们通过一个小小的实例来介绍tf的作用。

1、安装turtle包

[plain]  view plain  copy

1. $ rosdep install turtle_tf rviz  
2. $ rosmake turtle_tf rviz  

2、运行demo

        运行简单的demo：

[plain]  view plain  copy

1. $ roslaunch turtle_tf turtle_tf_demo.launch  

        然后就会看到两只小乌龟了。

        该例程中带有turtlesim仿真，可以在终端激活的情况下进行键盘控制。

        可以发现，第二只乌龟会跟随你移动的乌龟进行移动。

3、demo分析

        接下来我们就来看一看到底ROS做了什么事情。
        这个例程使用tf建立了三个参考系：a world frame, a turtle1 frame, and a turtle2 frame。然后使用tf broadcaster发布乌龟的参考系，并且使用tf listener计算乌龟参考系之间的差异，使得第二只乌龟跟随第一只乌龟。
        我们可以使用tf工具来具体研究。

[plain]  view plain  copy

1. $ rosrun tf view_frames  

        然后会看到一些提示，并且生成了一个frames.pdf文件。

        该文件描述了参考系之间的联系。三个节点分别是三个参考系，而/world是其他两个乌龟参考系的父参考系。还包含一些调试需要的发送频率、最近时间等信息。
        tf还提供了一个tf_echo工具来查看两个广播参考系之间的关系。我们可以看一下第二只得乌龟坐标是怎么根据第一只乌龟得出来的。

[plain]  view plain  copy

1. $ rosrun tf tf_echo turtle1 turtle2  

        控制一只乌龟，在终端中会看到第二只乌龟的坐标转换关系。

        我们也可以通过rviz的图形界面更加形象的看到这三者之间的关系。

[plain]  view plain  copy

1. $ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.vcg  

        移动乌龟，可以看到在rviz中的坐标会跟随变化。其中左下角的是/world，其他两个是乌龟的参考系。
       下面我们就来详细分析这个实例。

二、Writing a tf broadcaster

1、创建包

[plain]  view plain  copy

1. $ roscd tutorials  
2. $ roscreate-pkg learning_tf tf roscpp rospy turtlesim  
3. $ rosmake learning_tf  

2、broadcast transforms

        我们首先看一下如何把参考系发布到tf。
        代码文件：/nodes/turtle_tf_broadcaster.py

[python]  view plain  copy

1. #!/usr/bin/env python    
2. import roslib  
3. roslib.load_manifest('learning_tf')  
4. import rospy  
5.   
6.   
7. import tf  
8. import turtlesim.msg  
9.   
10.   
11. def handle_turtle_pose(msg, turtlename):  
12.     br = tf.TransformBroadcaster()  
13.     br.sendTransform((msg.x, msg.y, 0),  
14.                      tf.transformations.quaternion_from_euler(0, 0, msg.theta),  
15.                      rospy.Time.now(),  
16.                      turtlename,  
17.                      "world")  #发布乌龟的平移和翻转  
18.   
19.   
20. if __name__ == '__main__':  
21.     rospy.init_node('turtle_tf_broadcaster')  
22.     turtlename = rospy.get_param('~turtle')   #获取海龟的名字（turtle1，turtle2）  
23.     rospy.Subscriber('/%s/pose' % turtlename,  
24.                      turtlesim.msg.Pose,  
25.                      handle_turtle_pose,  
26.                      turtlename)   #订阅 topic "turtleX/pose"  
27.     rospy.spin()  

        创建launch文件start_demo.launch：

[plain]  view plain  copy

1. <launch>  
2.     <!-- Turtlesim Node-->  
3.     <node pkg="turtlesim" type="turtlesim_node" name="sim"/>  
4.     <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>  
5.   
6.   
7.     <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >  
8.       <param name="turtle" type="string" value="turtle1" />  
9.     </node>  
10.     <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >  
11.       <param name="turtle" type="string" value="turtle2" />   
12.     </node>  
13.   
14.   
15.   </launch>  

        运行：

[plain]  view plain  copy

1. $ roslaunch learning_tf start_demo.launch  

        可以看到界面中只有移植乌龟了，打开tf_echo的信息窗口：

[plain]  view plain  copy

1. $ rosrun tf tf_echo /world /turtle1   

        world参考系的原点在最下角，对于turtle1的转换关系，其实就是turtle1在world参考系中所在的坐标位置以及旋转角度。

三、Writing a tf listener

             这一步，我们将看到如何使用tf进行参考系转换。首先写一个tf listener（nodes/turtle_tf_listener.py）：

[python]  view plain  copy

1. #!/usr/bin/env python    
2. import roslib  
3. roslib.load_manifest('learning_tf')  
4. import rospy  
5. import math  
6. import tf  
7. import turtlesim.msg  
8. import turtlesim.srv  
9.   
10. if __name__ == '__main__':  
11.     rospy.init_node('tf_turtle')  
12.   
13.     listener = tf.TransformListener() #TransformListener创建后就开始接受tf广播信息，最多可以缓存10s  
14.   
15.     rospy.wait_for_service('spawn')  
16.     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)  
17.     spawner(4, 2, 0, 'turtle2')  
18.   
19.     turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity)  
20.   
21.     rate = rospy.Rate(10.0)  
22.     while not rospy.is_shutdown():  
23.         try:  
24.             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))  
25.         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):  
26.             continue  
27.   
28.         angular = 4 * math.atan2(trans[1], trans[0])  
29.         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)  
30.         turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))  
31.   
32.         rate.sleep()  

        在launch文件中添加下面的节点：

[plain]  view plain  copy

1. <launch>  
2.     ...  
3.     <node pkg="learning_tf" type="turtle_tf_listener.py"   
4.           name="listener" />  
5. </launch>  

        然后在运行，就可以看到两只turtle了，也就是我们在最开始见到的那种跟随效果。

四、Adding a frame

        在很多应用中，添加一个参考系是很有必要的，比如在一个world参考系下，有很一个激光扫描节点，tf可以帮助我们将激光扫描的信息坐标装换成全局坐标。

1、tf消息结构

        tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。

2、建立固定参考系（fixed frame）

        我们以turtle1作为父参考系，建立一个新的参考系“carrot1”。代码如下（nodes/fixed_tf_broadcaster.py）：

[plain]  view plain  copy

1. #!/usr/bin/env python    
2. import roslib  
3. roslib.load_manifest('learning_tf')  
4.   
5. import rospy  
6. import tf  
7.   
8. if __name__ == '__main__':  
9.     rospy.init_node('my_tf_broadcaster')  
10.     br = tf.TransformBroadcaster()  
11.     rate = rospy.Rate(10.0)  
12.     while not rospy.is_shutdown():  
13.         br.sendTransform((0.0, 2.0, 0.0),  
14.                          (0.0, 0.0, 0.0, 1.0),  
15.                          rospy.Time.now(),  
16.                          "carrot1",  
17.                          "turtle1") #建立一个新的参考系，父参考系为turtle1，并且距离父参考系2米  
18.         rate.sleep()  

       在launch文件中添加节点：

[plain]  view plain  copy

1. <launch>  
2.   ...  
3.   <node pkg="learning_tf" type="fixed_tf_broadcaster.py"  
4.         name="broadcaster_fixed" />  
5. </launch>  

        运行，还是看到两只乌龟和之前的效果一样。新添加的参考系并没有对其他参考系产生什么影响。打开nodes/turtle_tf_listener.py文件，将turtle1改成carrot1：

[python]  view plain  copy

1. (trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))  

        重新运行，现在乌龟之间的跟随关系就改变了：

3、建立移动参考系（moving frame）

        我们建立的新参考系是一个固定的参考系，在仿真过程中不会改变，如果我们要把carrot1参考系和turtle1参考系之间的关系设置可变的，可以修改代码如下：

[python]  view plain  copy

1. #!/usr/bin/env python    
2. import roslib  
3. roslib.load_manifest('learning_tf')  
4.   
5. import rospy  
6. import tf  
7. import math  
8.   
9. if __name__ == '__main__':  
10.     rospy.init_node('my_tf_broadcaster')  
11.     br = tf.TransformBroadcaster()  
12.     rate = rospy.Rate(10.0)  
13.     while not rospy.is_shutdown():  
14.         t = rospy.Time.now().to_sec() * math.pi  
15.         br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),  
16.                          (0.0, 0.0, 0.0, 1.0),  
17.                          rospy.Time.now(),  
18.                          "carrot1",  
19.                          "turtle1")  
20.         rate.sleep()  

        这次carrot1的位置现对于turtle1来说是一个三角函数关系了。

代码下载：http://download.csdn.net/detail/hcx25909/5708199

----------------------------------------------------------------

ROS探索总结（十三）——导航与定位框架

导航与定位是机器人研究中的重要部分。
        一般机器人在陌生的环境下需要使用激光传感器（或者深度传感器转换成激光数据），先进行地图建模，然后在根据建立的地图进行导航、定位。在ROS中也有很多完善的包可以直接使用。
        在ROS中，进行导航需要使用到的三个包是：
      （1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
      （2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
      （3） amcl：根据已经有的地图进行定位。

       参考链接：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup

                         http://www.ros.org/wiki/navigation/Tutorials

       整体导航包的格局如下图所示：

        其中白色框内的是ROS已经为我们准备好的必须使用的组件，灰色框内的是ROS中可选的组件，蓝色的是用户需要提供的机器人平台上的组件。

1、sensor transforms

        其中涉及到使用tf进行传感器坐标的转换，因为我们使用的机器人的控制中心不一定是在传感器上，所以要把传感器的数据转换成在控制中心上的坐标信息。如下图所示，传感器获取的数据是在base_laser的坐标系统中的，但是我们控制的时候是以base_link为中心，所以要根据两者的位置关心进行坐标的变换。

        变换的过程不需要我们自己处理，只需要将base_laser和base_link两者之间的位置关系告诉tf，就可以自动转换了。具体的实现可以参见：http://blog.csdn.net/hcx25909/article/details/9255001。

2、sensor sources

        这里是机器人导航传感器数据输入，一般只有两种：
      （1） 激光传感器数据
      （2） 点云数据
      具体实现见：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Sensors

3、odometry source

        机器人的导航需要输入里程计的数据（tf，nav_msgs/Odometry_message），具体实现见：
        http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom

4、base controller

        在导航过程中，该部分负责将之前得出来的数据转封装成具体的线速度和转向角度信息（Twist），并且发布给硬件平台。

5、map_server

       在导航过程中，地图并不是必须的，此时相当于是在一个无限大的空地上进行导航，并没有任何障碍物。但是考虑到实际情况，在我们使用导航的过程中还是要一个地图的。
       具体实现见：http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData
       在ROS的导航中，costmap_2d这个包主要负责根据传感器的信息建立和更新二维或三维的地图。ROS的地图（costmap）采用网格（grid）的形式，每个网格的值从0~255，分为三种状态：占用（有障碍物）、无用（空闲的）、未知。具体状态和值的对应关系如下：

        

        上图共分为五个部分：（下面的红色框图是机器人的轮廓，旁边的黑框是上图的映射位置）
      （1） Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
      （2） Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
      （3） Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
      （4） Freespace（自由空间）：没有障碍物的空间。
      （5） Unknown（未知）：未知的空间。
       具体可见：http://www.ros.org/wiki/costmap_2d

----------------------------------------------------------------

ROS探索总结（十四）——move_base（路径规划）

在上一篇的博客中，我们一起学习了ROS定位于导航的总体框架，这一篇我们主要研究其中最重要的move_base包。

           在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：
      （1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划；
      （2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。

一、数据结构

        ROS中定义了MoveBaseActionGoal数据结构来存储导航的目标位置数据，其中最重要的就是位置坐标（position）和方向（orientation）。

[plain]  view plain  copy

1. rosmsg show MoveBaseActionGoal  
2.   
3. [move_base_msgs/MoveBaseActionGoal]:  
4. std_msgs/Header header  
5.   uint32 seq  
6.   time stamp  
7.   string frame_id  
8. actionlib_msgs/GoalID goal_id  
9.   time stamp  
10.   string id  
11. move_base_msgs/MoveBaseGoal goal  
12.   geometry_msgs/PoseStamped target_pose  
13.     std_msgs/Header header  
14.       uint32 seq  
15.       time stamp  
16.       string frame_id  
17.     geometry_msgs/Pose pose  
18.       geometry_msgs/Point position  
19.         float64 x  
20.         float64 y  
21.         float64 z  
22.       geometry_msgs/Quaternion orientation  
23.         float64 x  
24.         float64 y  
25.         float64 z  
26.         float64 w  

二、配置文件

        move_base使用前需要配置一些参数：运行成本、机器人半径、到达目标位置的距离，机器人移动的速度，这些参数都在rbx1_nav包的以下几个配置文件中：
        • base_local_planner_params.yaml
        • costmap_common_params.yaml
        • global_costmap_params.yaml
        • local_costmap_params.yaml

三、全局路径规划（global planner）

        在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。
        navfn通过Dijkstra最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。将来在算法上应该还会加入A*算法。
        具体见：http://www.ros.org/wiki/navfn?distro=fuerte

四、本地实时规划（local planner）

        本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。

        base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。
其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
      （1） 采样机器人当前的状态（dx,dy,dtheta）；
      （2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
      （3） 利用一些评价标准为多条路线打分。
      （4） 根据打分，选择最优路径。
      （5） 重复上面过程。
      具体参见：http://www.ros.org/wiki/base_local_planner?distro=groovy

五、ArbotiX仿真——手动设定目标

        在这一步，我们暂时使用空白地图（blank_map.pgm），就在空地上进行无障碍仿真。
        首先运行ArbotiX节点，并且加载机器人的URDF文件。

[plain]  view plain  copy

1. roslaunch rbx1_bringup fake_turtlebot.launch  

         然后运行move_base和加载空白地图的launch文件（fake_move_base_blank_map.launch）：

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_move_base_blank_map.launch  

        该文件的具体内容如下：

[plain]  view plain  copy

1. <launch>  
2.   <!-- Run the map server with a blank map -->  
3.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>  
4.       
5.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
6.   
7.   <!-- Run a static transform between /odom and /map -->  
8.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
9.   
10. </launch>  

        其中调用了fake_move_base.launch文件，是运行move_base节点并进行参数配置。

        然后调用rviz就可以看到机器人了。

[plain]  view plain  copy

1. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

        

        我们先以1m的速度进行一下测试：

        让机器人前进一米：

[plain]  view plain  copy

1. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
2. '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'  

        让机器人后退一米，回到原来的位置：

[plain]  view plain  copy

1. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
2. '{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'  

        在rviz中的轨迹图如下：

        在机器人移动过程中，有一条蓝色的线（被黄线挡住了）就是机器人的全局规划的路径；红色的箭头是实施规划的路线，会不断更新，有的时候会呈现很大的弧线，那是因为机器人在转向的过程中尽量希望保持平稳的角度。如果觉得路径规划的精度不够，可以修改配置文件中的pdist_scale参数进行修正。
        然后我们可以认为的确定目标位置，点击rviz上方的2D Nav Goal按键，然后左键选择目标位置，机器人就开始自动导航了。

六、ArbotiX仿真——带有障碍物的路径规划

        首先我们让机器人走一个正方形的路线。先通过上面的命令，让机器人回到原始位置（0，0，0），然后按reset按键，把所有的箭头清除。接着运行走正方形路径的代码：

[plain]  view plain  copy

1. rosrun rbx1_nav move_base_square.py  

        在rviz中可以看到：

        四个顶角的粉色圆盘就是我们设定的位置，正方形比较规则，可见定位还是比较准确的。然我们先来分析一下走正方形路线的代码：

[python]  view plain  copy

1. #!/usr/bin/env python  
2. import roslib; roslib.load_manifest('rbx1_nav')  
3. import rospy  
4. import actionlib  
5. from actionlib_msgs.msg import *  
6. from geometry_msgs.msg import Pose, Point, Quaternion, Twist  
7. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
8. from tf.transformations import quaternion_from_euler  
9. from visualization_msgs.msg import Marker  
10. from math import radians, pi  
11.   
12. class MoveBaseSquare():  
13.     def __init__(self):  
14.         rospy.init_node('nav_test', anonymous=False)  
15.           
16.         rospy.on_shutdown(self.shutdown)  
17.           
18.         # How big is the square we want the robot to navigate?  
19.         # 设定正方形的尺寸，默认是一米  
20.         square_size = rospy.get_param("~square_size", 1.0) # meters  
21.           
22.         # Create a list to hold the target quaternions (orientations)  
23.         # 创建一个列表，保存目标的角度数据  
24.         quaternions = list()  
25.           
26.         # First define the corner orientations as Euler angles  
27.         # 定义四个顶角处机器人的方向角度（Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)  
28.         euler_angles = (pi/2, pi, 3*pi/2, 0)  
29.           
30.         # Then convert the angles to quaternions  
31.         # 将上面的Euler angles转换成Quaternion的格式  
32.         for angle in euler_angles:  
33.             q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')  
34.             q = Quaternion(*q_angle)  
35.             quaternions.append(q)  
36.           
37.         # Create a list to hold the waypoint poses  
38.         # 创建一个列表存储导航点的位置  
39.         waypoints = list()  
40.           
41.         # Append each of the four waypoints to the list.  Each waypoint  
42.         # is a pose consisting of a position and orientation in the map frame.  
43.         # 创建四个导航点的位置（角度和坐标位置）  
44.         waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))  
45.         waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))  
46.         waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))  
47.         waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))  
48.           
49.         # Initialize the visualization markers for RViz  
50.         # 初始化可视化标记  
51.         self.init_markers()  
52.           
53.         # Set a visualization marker at each waypoint   
54.         # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记)  
55.         for waypoint in waypoints:             
56.             p = Point()  
57.             p = waypoint.position  
58.             self.markers.points.append(p)  
59.               
60.         # Publisher to manually control the robot (e.g. to stop it)  
61.         # 发布TWist消息控制机器人  
62.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
63.           
64.         # Subscribe to the move_base action server  
65.         # 订阅move_base服务器的消息  
66.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
67.           
68.         rospy.loginfo("Waiting for move_base action server...")  
69.           
70.         # Wait 60 seconds for the action server to become available  
71.         # 等待move_base服务器建立  
72.         self.move_base.wait_for_server(rospy.Duration(60))  
73.           
74.         rospy.loginfo("Connected to move base server")  
75.         rospy.loginfo("Starting navigation test")  
76.           
77.         # Initialize a counter to track waypoints  
78.         # 初始化一个计数器，记录到达的顶点号  
79.         i = 0  
80.           
81.         # Cycle through the four waypoints  
82.         # 主循环,环绕通过四个定点  
83.         while i < 4 and not rospy.is_shutdown():  
84.             # Update the marker display  
85.             # 发布标记指示四个目标的位置，每个周期发布一起，确保标记可见  
86.             self.marker_pub.publish(self.markers)  
87.               
88.             # Intialize the waypoint goal  
89.             # 初始化goal为MoveBaseGoal类型  
90.             goal = MoveBaseGoal()  
91.               
92.             # Use the map frame to define goal poses  
93.             # 使用map的frame定义goal的frame id  
94.             goal.target_pose.header.frame_id = 'map'  
95.               
96.             # Set the time stamp to "now"  
97.             # 设置时间戳  
98.             goal.target_pose.header.stamp = rospy.Time.now()  
99.               
100.             # Set the goal pose to the i-th waypoint  
101.             # 设置目标位置是当前第几个导航点  
102.             goal.target_pose.pose = waypoints[i]  
103.               
104.             # Start the robot moving toward the goal  
105.             # 机器人移动  
106.             self.move(goal)  
107.               
108.             i += 1  
109.           
110.     def move(self, goal):  
111.             # Send the goal pose to the MoveBaseAction server  
112.             # 把目标位置发送给MoveBaseAction的服务器  
113.             self.move_base.send_goal(goal)  
114.               
115.             # Allow 1 minute to get there  
116.             # 设定1分钟的时间限制  
117.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))   
118.               
119.             # If we don't get there in time, abort the goal  
120.             # 如果一分钟之内没有到达，放弃目标  
121.             if not finished_within_time:  
122.                 self.move_base.cancel_goal()  
123.                 rospy.loginfo("Timed out achieving goal")  
124.             else:  
125.                 # We made it!  
126.                 state = self.move_base.get_state()  
127.                 if state == GoalStatus.SUCCEEDED:  
128.                     rospy.loginfo("Goal succeeded!")  
129.                       
130.     def init_markers(self):  
131.         # Set up our waypoint markers  
132.         # 设置标记的尺寸  
133.         marker_scale = 0.2  
134.         marker_lifetime = 0 # 0 is forever  
135.         marker_ns = 'waypoints'  
136.         marker_id = 0  
137.         marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}  
138.           
139.         # Define a marker publisher.  
140.         # 定义一个标记的发布者  
141.         self.marker_pub = rospy.Publisher('waypoint_markers', Marker)  
142.           
143.         # Initialize the marker points list.  
144.         # 初始化标记点的列表  
145.         self.markers = Marker()  
146.         self.markers.ns = marker_ns  
147.         self.markers.id = marker_id  
148.         self.markers.type = Marker.SPHERE_LIST  
149.         self.markers.action = Marker.ADD  
150.         self.markers.lifetime = rospy.Duration(marker_lifetime)  
151.         self.markers.scale.x = marker_scale  
152.         self.markers.scale.y = marker_scale  
153.         self.markers.color.r = marker_color['r']  
154.         self.markers.color.g = marker_color['g']  
155.         self.markers.color.b = marker_color['b']  
156.         self.markers.color.a = marker_color['a']  
157.           
158.         self.markers.header.frame_id = 'map'  
159.         self.markers.header.stamp = rospy.Time.now()  
160.         self.markers.points = list()  
161.   
162.     def shutdown(self):  
163.         rospy.loginfo("Stopping the robot...")  
164.         # Cancel any active goals  
165.         self.move_base.cancel_goal()  
166.         rospy.sleep(2)  
167.         # Stop the robot  
168.         self.cmd_vel_pub.publish(Twist())  
169.         rospy.sleep(1)  
170.   
171. if __name__ == '__main__':  
172.     try:  
173.         MoveBaseSquare()  
174.     except rospy.ROSInterruptException:  
175.         rospy.loginfo("Navigation test finished.")  

        但是，在实际情况中，往往需要让机器人自动躲避障碍物。move_base包的一个强大的功能就是可以在全局规划的过程中自动躲避障碍物，而不影响全局路径。障碍物可以是静态的（比如墙、桌子等），也可以是动态的（比如人走过）。
        现在我们尝试在之前的正方形路径中加入障碍物。把之前运行fake_move_base_blank_map.launch的中断Ctrl-C掉，然后运行：

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_move_base_obstacle.launch  

        然后就会看到在rviz中出现了障碍物。然后在运行之前走正方形路线的代码：

[plain]  view plain  copy

1. rosrun rbx1_nav move_base_square.py   

        这回我们可以看到，在全局路径规划的时候，机器人已经将障碍物绕过去了，下过如下图：

        在上图中，黑色的线是障碍物，周围浅色的椭圆形是根据配置文件中的inflation_radius参数计算出来的安全缓冲区。全局规划的路径基本已经是最短路径了。在仿真的过程中也可以动态重配置那四个配置文件，修改仿真参数。

----------------------------------------------------------------

ROS探索总结（十五）——amcl（导航与定位）

在理解了move_base的基础上，我们开始机器人的定位与导航。gmaping包是用来生成地图的，需要使用实际的机器人获取激光或者深度数据，所以我们先在已有的地图上进行导航与定位的仿真。

        amcl是移动机器人二维环境下的概率定位系统。它实现了自适应（或kld采样）的蒙特卡罗定位方法，其中针对已有的地图使用粒子滤波器跟踪一个机器人的姿态。

一、测试

        首先运行机器人节点：

[plain]  view plain  copy

1. roslaunch rbx1_bringup fake_turtlebot.launch  

        然后运行amcl节点，使用测试地图：        

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml  

        可以看一下fake_amcl.launch这个文件的内容：

[plain]  view plain  copy

1. <launch>  
2.   <!-- Set the name of the map yaml file: can be overridden on the command line. -->  
3.   <arg name="map" default="test_map.yaml" />  
4.   <!-- Run the map server with the desired map -->  
5.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/$(arg map)"/>  
6.   <!-- The move_base node -->  
7.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
8.     
9.   <!-- Run fake localization compatible with AMCL output -->  
10.   <node pkg="fake_localization" type="fake_localization"  name="fake_localization" output="screen" />  
11.   <!-- For fake localization we need static transforms between /odom and /map and /map and /world -->  
12.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster"   
13. args="0 0 0 0 0 0 /odom /map 100" />  
14. </launch>  

         这个lanuch文件作用是加载地图，并且调用fake_move_base.launch文件打开move_base节点并加载配置文件，最后运行amcl。
         然后运行rviz：

[plain]  view plain  copy

1. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

         这时在rvoiz中就应该显示出了地图和机器人：

        现在就可以通过rviz在地图上选择目标位置了，然后就会看到机器人自动规划出一条全局路径，并且导航前进：

二、自主导航 

        在实际应用中，我们往往希望机器人能够自主进行定位和导航，不需要认为的干预，这样才更智能化。在这一节的测试中，我们让目标点在地图中随机生成，然后机器人自动导航到达目标。
        这里运行的主要文件是：fake_nav_test.launch，让我们来看一下这个文件的内容：   

[plain]  view plain  copy

1. <launch>  
2.   
3.   <param name="use_sim_time" value="false" />  
4.   
5.   <!-- Start the ArbotiX controller -->  
6.   <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />  
7.   
8.   <!-- Run the map server with the desired map -->  
9.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>  
10.   
11.   <!-- The move_base node -->  
12.   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">  
13.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />  
14.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />  
15.     <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />  
16.     <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />  
17.     <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />  
18.     <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />  
19.   </node>  
20.     
21.   <!-- Run fake localization compatible with AMCL output -->  
22.   <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />  
23.     
24.   <!-- For fake localization we need static transform between /odom and /map -->  
25.   <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
26.     
27.   <!-- Start the navigation test -->  
28.   <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">  
29.     <param name="rest_time" value="1" />  
30.     <param name="fake_test" value="true" />  
31.   </node>  
32.     
33. </launch>  

        这个lanuch的功能比较多：
      （1） 加载机器人驱动
      （2） 加载地图
      （3） 启动move_base节点，并且加载配置文件
      （4） 运行amcl节点
      （5） 然后加载nav_test.py执行文件，进行随机导航
        相当于是把我们之前实验中的多个lanuch文件合成了一个文件。
        现在开始进行测试，先运行ROS：

[plain]  view plain  copy

1. roscore  

        然后我们运行一个监控的窗口，可以实时看到机器人发送的数据：

[plain]  view plain  copy

1. rxconsole  

        接着运行lanuch文件，并且在一个新的终端中打开rviz：

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_nav_test.launch  
2. （打开新终端）  
3. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_test_fuerte.vcg  

        好了，此时就看到了机器人已经放在地图当中了。然后我们点击rviz上的“2D Pose Estimate”按键，然后左键在机器人上单击，让绿色的箭头和黄色的箭头重合，机器人就开始随机选择目标导航了：

        

         在监控窗口中，我们可以看到机器人发送的状态信息：

        其中包括距离信息、状态信息、目标的编号、成功率和速度等信息。

三、导航代码分析

[plain]  view plain  copy

1. #!/usr/bin/env python  
2.   
3. import roslib; roslib.load_manifest('rbx1_nav')  
4. import rospy  
5. import actionlib  
6. from actionlib_msgs.msg import *  
7. from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
8. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
9. from random import sample  
10. from math import pow, sqrt  
11.   
12. class NavTest():  
13.     def __init__(self):  
14.         rospy.init_node('nav_test', anonymous=True)  
15.           
16.         rospy.on_shutdown(self.shutdown)  
17.           
18.         # How long in seconds should the robot pause at each location?  
19.         # 在每个目标位置暂停的时间  
20.         self.rest_time = rospy.get_param("~rest_time", 10)  
21.           
22.         # Are we running in the fake simulator?  
23.         # 是否仿真？  
24.         self.fake_test = rospy.get_param("~fake_test", False)  
25.           
26.         # Goal state return values  
27.         # 到达目标的状态  
28.         goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
29.                        'SUCCEEDED', 'ABORTED', 'REJECTED',  
30.                        'PREEMPTING', 'RECALLING', 'RECALLED',  
31.                        'LOST']  
32.           
33.         # Set up the goal locations. Poses are defined in the map frame.    
34.         # An easy way to find the pose coordinates is to point-and-click  
35.         # Nav Goals in RViz when running in the simulator.  
36.         # Pose coordinates are then displayed in the terminal  
37.         # that was used to launch RViz.  
38.         # 设置目标点的位置  
39.         # 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点  
40.         # 在终端中就会看到坐标信息  
41.         locations = dict()  
42.           
43.         locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))  
44.         locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))  
45.         locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))  
46.         locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))  
47.         locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))  
48.         locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))  
49.           
50.         # Publisher to manually control the robot (e.g. to stop it)  
51.         # 发布控制机器人的消息  
52.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
53.           
54.         # Subscribe to the move_base action server  
55.         # 订阅move_base服务器的消息  
56.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
57.           
58.         rospy.loginfo("Waiting for move_base action server...")  
59.           
60.         # Wait 60 seconds for the action server to become available  
61.         # 60s等待时间限制  
62.         self.move_base.wait_for_server(rospy.Duration(60))  
63.           
64.         rospy.loginfo("Connected to move base server")  
65.           
66.         # A variable to hold the initial pose of the robot to be set by   
67.         # the user in RViz  
68.         # 保存机器人的在rviz中的初始位置  
69.         initial_pose = PoseWithCovarianceStamped()  
70.           
71.         # Variables to keep track of success rate, running time,  
72.         # and distance traveled  
73.         # 保存成功率、运行时间、和距离的变量  
74.         n_locations = len(locations)  
75.         n_goals = 0  
76.         n_successes = 0  
77.         i = n_locations  
78.         distance_traveled = 0  
79.         start_time = rospy.Time.now()  
80.         running_time = 0  
81.         location = ""  
82.         last_location = ""  
83.           
84.         # Get the initial pose from the user  
85.         # 获取初始位置(仿真中可以不需要)  
86.         rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")  
87.         rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
88.         self.last_location = Pose()  
89.         rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)  
90.           
91.         # Make sure we have the initial pose  
92.         # 确保有初始位置  
93.         while initial_pose.header.stamp == "":  
94.             rospy.sleep(1)  
95.               
96.         rospy.loginfo("Starting navigation test")  
97.           
98.         # Begin the main loop and run through a sequence of locations  
99.         # 开始主循环，随机导航  
100.         while not rospy.is_shutdown():  
101.             # If we've gone through the current sequence,  
102.             # start with a new random sequence  
103.             # 如果已经走完了所有点，再重新开始排序  
104.             if i == n_locations:  
105.                 i = 0  
106.                 sequence = sample(locations, n_locations)  
107.                 # Skip over first location if it is the same as  
108.                 # the last location  
109.                 # 如果最后一个点和第一个点相同，则跳过  
110.                 if sequence[0] == last_location:  
111.                     i = 1  
112.               
113.             # Get the next location in the current sequence  
114.             # 在当前的排序中获取下一个目标点  
115.             location = sequence[i]  
116.                           
117.             # Keep track of the distance traveled.  
118.             # Use updated initial pose if available.  
119.             # 跟踪形式距离  
120.             # 使用更新的初始位置  
121.             if initial_pose.header.stamp == "":  
122.                 distance = sqrt(pow(locations[location].position.x -   
123.                                     locations[last_location].position.x, 2) +  
124.                                 pow(locations[location].position.y -   
125.                                     locations[last_location].position.y, 2))  
126.             else:  
127.                 rospy.loginfo("Updating current pose.")  
128.                 distance = sqrt(pow(locations[location].position.x -   
129.                                     initial_pose.pose.pose.position.x, 2) +  
130.                                 pow(locations[location].position.y -   
131.                                     initial_pose.pose.pose.position.y, 2))  
132.                 initial_pose.header.stamp = ""  
133.               
134.             # Store the last location for distance calculations  
135.             # 存储上一次的位置，计算距离  
136.             last_location = location  
137.               
138.             # Increment the counters  
139.             # 计数器加1  
140.             i += 1  
141.             n_goals += 1  
142.           
143.             # Set up the next goal location  
144.             # 设定下一个目标点  
145.             self.goal = MoveBaseGoal()  
146.             self.goal.target_pose.pose = locations[location]  
147.             self.goal.target_pose.header.frame_id = 'map'  
148.             self.goal.target_pose.header.stamp = rospy.Time.now()  
149.               
150.             # Let the user know where the robot is going next  
151.             # 让用户知道下一个位置  
152.             rospy.loginfo("Going to: " + str(location))  
153.               
154.             # Start the robot toward the next location  
155.             # 向下一个位置进发  
156.             self.move_base.send_goal(self.goal)  
157.               
158.             # Allow 5 minutes to get there  
159.             # 五分钟时间限制  
160.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   
161.               
162.             # Check for success or failure  
163.             # 查看是否成功到达  
164.             if not finished_within_time:  
165.                 self.move_base.cancel_goal()  
166.                 rospy.loginfo("Timed out achieving goal")  
167.             else:  
168.                 state = self.move_base.get_state()  
169.                 if state == GoalStatus.SUCCEEDED:  
170.                     rospy.loginfo("Goal succeeded!")  
171.                     n_successes += 1  
172.                     distance_traveled += distance  
173.                     rospy.loginfo("State:" + str(state))  
174.                 else:  
175.                   rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
176.               
177.             # How long have we been running?  
178.             # 运行所用时间  
179.             running_time = rospy.Time.now() - start_time  
180.             running_time = running_time.secs / 60.0  
181.               
182.             # Print a summary success/failure, distance traveled and time elapsed  
183.             # 输出本次导航的所有信息  
184.             rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
185.                           str(n_goals) + " = " +   
186.                           str(100 * n_successes/n_goals) + "%")  
187.             rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
188.                           " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
189.             rospy.sleep(self.rest_time)  
190.               
191.     def update_initial_pose(self, initial_pose):  
192.         self.initial_pose = initial_pose  
193.   
194.     def shutdown(self):  
195.         rospy.loginfo("Stopping the robot...")  
196.         self.move_base.cancel_goal()  
197.         rospy.sleep(2)  
198.         self.cmd_vel_pub.publish(Twist())  
199.         rospy.sleep(1)  
200.         
201. def trunc(f, n):  
202.     # Truncates/pads a float f to n decimal places without rounding  
203.     slen = len('%.*f' % (n, f))  
204.     return float(str(f)[:slen])  
205.   
206. if __name__ == '__main__':  
207.     try:  
208.         NavTest()  
209.         rospy.spin()  
210.     except rospy.ROSInterruptException:  
211.         rospy.loginfo("AMCL navigation test finished.")  

----------------------------------------------------------------

ROS探索总结（十六）——HRMRP机器人的设计

1. HRMRP简介

        HRMRP（Hybrid Real-time Mobile Robot Platform，混合实时移动机器人平台）机器人是我在校期间和实验室的其他小伙伴一起从零开始设计并开发的一款机器人平台，其中大部分扩展电路、驱动和ROS相关的底层功能都是我们自己做的。该机器人平台具有软硬件可编程、灵活性强、模块化、易扩展、实时性强等特点，机器人的整体结构如下图所示。

      HRMRP具备丰富的传感器和执行器，在该平台的基础上，我们设计并实现了机器人SLAM、自主导航、人脸识别、机械臂控制等功能。在设计开发完成之初，我们参加了2013年的OpenHW大赛，并且获得了全国一等奖，此外还在中国智能机器人学术会议上发表学术论文一篇，获得了会议的十佳论文奖。这个机器人陪伴我走过了研究生的三年时光，接下来的几篇博客，我会详细介绍HRMRP从设计到实现方面的很多细节。就机器人的性能来讲，很多方面是超越已有同等级的很多机器人的，但是由于我们开发能力、时间的限制，并没有在最终的应用中充分发挥他的潜力，这也给我留下了一些遗憾。

      废话不多说，先让大家对该机器人有一个整体的印象：

      演示视频：OpenHW大赛机器人演示

      相关论文：《基于嵌入式系统的室内移动机器人定位与导航》 

2. HRMRP的总体架构

        如下图所示，我们根据层次化、模块化的思想，设计的HRMRP的总体架构。

3. 硬件层

3.1 机械平台 

       HRMRP 主体结构为铝合金材质，尺寸为 316mm×313mm×342mm （高×宽×长），装配两个驱动轮与一个万向轮。驱动轮由两个 30W 的直流电机带动，转速可达 83 转/分钟，机器人最快速度 1.5m/s。HRMRP 还装有一个六自由度机械臂， 可以完成三维空间内的夹取操作。 

3.2 控制平台 

       嵌入式系统具备小型化、低功耗、低成本、高灵活性等显著的特点， 电子技术的 发展，也促使可编程门阵列FPGA在嵌入式系统中 得到了越来越广泛的应用，很大程度上改善了嵌入式系统硬件的灵活度与繁琐计算的 实时化。 HRMRP的控制平台即基于 Xilinx 最新一代集成 FPGA 与 ARM 的片上系统 （System-on-Chip，SoC）——Zynq。 

       Zynq 由处理系统（Processor System， PS）与可编程逻辑（Programmable Logic， PL）两部分组成。其中 PS 基于 ARM Cortex-A9 双核处理器构建，包含常用的外设接口，例如网络、 USB、内存控制器等。而 PL 由 Xilinx 的 7 系列 FPGA 构成，支持动 态重配置，可以使用 Verilog 语言编程使用。在HRMRP 中，PS通过操作系统控制所 有功能正常有序的实现，而 PL作为协处理器一方面可以对复杂的运算并行加速处理， 另一方面可以进行 I/O 接口扩展，为多传感器和执行器设计统一的接口，提高系统硬 件配置的灵活性。

3.3 传感器系统 

       在机器人核心传感器的选择上， HRMRP使用了高性价比、高集成度的微软 Kinect 传感器。 除此还装配有超声波、加速度、里程计、陀螺仪等多种传感器， 确保机器人 平台可以采集到丰富的传感信息。

4. 驱动层

       驱动层的主要工作是采集或预处理硬件层的数据，下发操作系统层的指令，为底层硬件与上层功能模块提供相应的数据传输通道。由于我们采用的“ARM+FPGA”异构控制平台，为配合硬件层硬件功能，驱动层也分为两部分，分别放置于硬件的PS端和PL端。

       PS端主要驱动连接到ARM处理器的外设，例如通过PS中的OpenNI驱动 Kinect， 并且提供 PL 端到 PS 端的接口。而在PL端中，利用可编程硬件的灵活性和并行处理能力，来进行 I/O 扩展与算法的硬件加速， 如下图所示。

       在I/O 扩展方面，在传统的设计实现当中，由于种类繁多的传感器、 执行器对接口的要求各不相同，会占用大量 I/O 资源，增加处理器的负担。而在HRMRP的ARM+FPGA系统当中，通过定义一组标准的硬件接口， 连接传感器和电机等外设，可使用编程逻辑取代繁杂的电路连接工作，满足各种不同需求的硬件外设。

       在硬件加速方面，一般来说PS端适合常用接口的驱动、网络数据的处理等功能，而PL端适合于规律性的算法处理，在HRMRP中主要负责Kinect的数据预处理工作（这里我们将OpenNI中的部分代码放入FPGA中进行加速）。PS与PL相互配合，提高了系统数据处理的实时性。

5. 操作系统层

        操作系统层是机器人平台的控制核心，集成了机器人的功能模块，负责行为控制、 数据上传、指令解析、人机交互等功能。为与 ROS 通讯接口保持一致，使用Ubuntu12.04作为操作系统，运行于Zynq的PS端ARM处理器之中。ROS为用户的不同需求提供了大小和功能不同的多种安装包，为了减少ARM端的执行压力， HRMRP编译移植了仅包括 ROS 基本通讯机制的核心库。继承了ROS的优势，机器人平台具备ROS通讯以及功能包运行的能力，与上层网络指令无缝连接，结合开源软件库，极大的丰富了机器人的功能模块与应用范围。 

       HRMRP是一种较为典型的高性能、低成本机器人平台。与现在研究和应用中使用较为广泛的TurtleBot、 Pioneer等机器人相比，HRMRP具有相似的结构与尺寸，同样可以完成多种多样的机器人应用，但是在接口的可扩展性、传感器的丰富度以及成本控制等方面，具备更好的综合性能。

       今天就针对HRMRP的设计写到这里，下篇继续针对细节实现进行分析。

----------------------------------------------------------------

ROS探索总结（十七）——构建完整的机器人应用系统

 上一篇博客介绍了HRMRP机器人平台的设计，基于该平台，可以完成丰富的机器人应用，以较为典型的机器人导航为例，如何使用HRMRP来完成相应的功能？本篇博客将详细介绍如何将HRMRP应用到实际的应用当中。

       1. 系统架构

       ROS作为一个分布式框架，从微观的角度讲，分布式体现在节点的布局和配置上，而从宏观的角度来讲，这种分布式可以体现在多机器人、多主机集成的系统当中。ROS社区中针对多机器人系统并没有很多的涉及，相关应用也比较少。在HRMRP机器人的基础上，我们试图去提出一种多机器人实现的框架，如下图所示：

       2. Server （服务器）

       由于机器人架构多种多样，处理应用的能力也各不相同，在不同场合下的需求也有差异，我们设计了服务器层来提高机器人应用的计算能力，负责调度、分配多机器人应用中的任务，同时为用户提供友好、易用的人机交互界面。

       分布的机器人节点与服务器都采用ROS框架设计，使用无线网络通讯，可以快速集成ROS社区中丰富的应用功能。在多机器人系统当中，通过机器人之间的信息共享和与任务协作，可以让每个机器人在充分发挥自己的能力的同时，获得更多额外的应用潜力。

      

        3. Robot Node（机器人节点）

       机器人节点是应用的执行者与信息的采集者。在该系统中可以集成多种采用ROS框架的机器人，这里以我们设计的HRMRP机器人平台为例，上一篇博客中已经进行了详细的介绍。

        4. 机器人导航

        HRMRP机器人平台采用嵌入式系统作为主控，对于机器人导航等复杂算法的处理能力有限，于是我们将应用的处理在服务器端实现，机器人将采集到的周围环境信息和自身传感器信息发布，由服务器订阅消息并完成处理和显示，再向下发布控制指令。

        数据的处理流程如下：

        SLAM的效果如下：

        导航效果如下：

     

       5. 多机器人实验

       当然，该系统对多机器人的支持也是很好的，由于时间有限，我们只做了一个简单的机器人跟随实验。该实验使用了两个机器人，除HRMRP机器人之外，还使用树莓派制作了一个简单的小型机器人。在实验中，HRMRP机器人在地图上自主导航前进，服务器负责应用的处理与显示，同时将HRMRP的位置信息转发给树莓派机器人，树莓派机器人收到信息后，紧跟HRMRP。

       实验效果如下：

      

      6.相关论文

《A New ROS-based Hybrid Architecture for Heterogeneous Multi-Robot Systems》

----------------------------------------------------------------

ROS探索总结（十八）——重读tf

在之前的博客中，有讲解tf的相关内容，本篇博客重新整理了tf的介绍和学习内容，对tf的认识会更加系统。

1 tf简介

1.1 什么是tf

        tf是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。

        tf的相关设计思想，可以参见：tf设计

1.2 tf可以做什么

        一个机器人系统通常有很多三维的参考系，而且会随着时间的推移发生变化，例如全局参考系（world frame），机器人中心参考系（base frame），机械夹参考系（gripper frame），机器人头参考系（head frame）等等。tf可以以时间为轴，跟踪这些参考系（默认是10秒之内的），并且允许用户提出如下的申请：

• 五秒钟之前，机器人头参考系相对于全局参考系的关系是什么样的？
• 机器人夹取的物体相对于机器人中心参考系的位置在哪里？
• 机器人中心参考系相对于全局参考系的位置在哪里？

        tf可以在分布式系统中进行操作，也就是说一个机器人系统中所有的参考系变换关系，对于所有节点组件，都是可用的，所有订阅tf消息的节点都会缓冲一份所有参考系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。

1.3 tf的使用流程

        想要使用tf功能包，总体来讲可以分为以下两个步骤：

      （1）   监听tf变换

        接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。

      （2）   广播tf变换

        向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

2 tf实例理解

        参见：ROS探索总结（十二）——坐标系统

3 tf基础教程

        参见：ROS探索总结（十二）——坐标系统

4 tf功能包的API说明

• 数据类型
• 变换与参考系
• 发布tf变换广播
• 接收并使用tf广播
• 异常

5 命令行工具

        虽然tf是ROS中的一个代码链接库，但是仍然提供了丰富的命令行工具来帮助用户调试和创建tf变换。

5.1 tf_monitor

        tf_monitor工具的功能是打印tf树中的所有参考系信息，通过输入参数来查看指定参考系之间的信息。

        命令格式如下：

• tf_monitor
• tf_monitor <source_frame> <target_target>

        示例结果如下图所示：

5.2 tf_echo

        tf_echo工具的功能是查看指定参考系之间的变换关系。

        命令的格式如下：

• tf_echo <source_frame> <target_frame>

        示例效果如下图所示：

5.3 static_transform_publisher

        static_transform_publisher工具的功能是发布两个参考系之间的静态坐标变换，两个参考系一般不发生相对位置变化。

命令的格式如下：

• static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
• static_transform_publisher x y z qx qy qz qw frame_id child_frame_id  period_in_ms

         以上两种命令格式，需要设置坐标的偏移和旋转参数，偏移参数都使用相对于xyz三轴的坐标位移，而旋转参数第一种命令格式使用以弧度为单位的 yaw/pitch/roll三个角度（yaw是围绕x轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕z轴旋转的翻滚角），而第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位，一般100ms比较合适。

        该命令不仅可以在终端中使用，还可以在launch文件中使用，使用方式如下：

[html]  view plain  copy

1. <launch>  
2. <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />  
3. </launch>  

5.4 view_frames

        view_frames 是可视化的调试工具，可以生成pdf文件，来显示整棵tf树的信息。

        命令行的执行方式如下：

[html]  view plain  copy

1. $ rosrun tf view_frames  
2. $ evince frames.pdf  

5.5 roswtf plugin

        roswtf是ROS中自查的工具，也可以作为组件使用。针对tf，roswtf可以检查tf的配置并发现常见问题。

       命令的使用方式如下：

[html]  view plain  copy

1. $ roswtf  

6 如何调试tf

        参见：调试tf问题

参考链接

• tf : http://wiki.ros.org/tf
• tf设计：http://wiki.ros.org/tf/Design
• tf教程：http://wiki.ros.org/tf/Tutorials
• tf: The transform library：http://wiki.ros.org/Papers/TePRA2013_Foote

----------------------------------------------------------------

ROS探索总结（十九）——如何配置机器人的导航功能

1、概述

        ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是，如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程。作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时，为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能，配置导航功能包的一些参数。

 

2、硬件要求

        尽管导航功能包设计得尽可能通用，但是仍然对机器人的硬件有以下三个要求：

       （1）导航功能包仅对差分等轮式机器人有效，并且假设机器人可直接使用速度指令进行控制，速度指令的格式为：x方向速度、y方向速度、速度向量角度。

       （2）导航功能包要求机器人必须安装有激光雷达等二维平面测距设备。

       （3）导航功能包以正方型的机器人为模型进行开发，所以对于正方形或者圆形外形的机器人支持度较好，而对于其他外形的机器人来讲，虽然仍然可以正常使用，但是表现则很有可能不佳。

 

3、机器人配置

        导航功能包的结构如上图所示，在自己的机器人平台上实现自主导航，简单来说，就是按照上图将需要的功能按照需求完成即可。其中白色的部分是ROS功能包已经完成的部分，不需要我们去实现，灰色的是可选的部分，也由ROS完成，在使用中根据需求使用，需要关注的重点部分是蓝色部分，这些需要我们根据输入输出的要求完成相应的功能。

3.1、ROS

        首先，请确保你的机器人安装了ROS框架。

3.2、tf变换(sensortransforms)

        导航功能包要求机器人以tf树的形式发布各个相关参考系的变换关系。

3.3、传感器信息(sensor sources)

        导航功能包需要采集机器人的传感器信息，以达到实时避障的效果。这些传感器要求能够通过ROS发布

sensor_msgs/LaserScan或者sensor_msgs/PointCloud 格式的消息，也就是二维雷达信息或者三维点云数据。ROS社区已经支持大部分激光雷达、Kinect等设备的驱动，可以直接使用社区提供的驱动功能包发布满足要求的传感器信息。如果你使用的传感器没有ROS支持，或者你想使用自己的驱动，也可以自己将传感器信息封装成要求的格式。

3.4、里程计信息(odometrysource)

      导航功能包要求机器人发布nav_msgs/Odometry格式的里程计信息，同时在也要发布相应的tf变换。

3.5、机器人控制器（base_controller）

      导航功能包最终的输出是针对机器人geometry_msgs/Twist格式的控制指令，这就要求机器人控制节点具备解析控制指令中速度、角度的能力，并且最终通过这些指令控制机器人完成相应的运动目标。

3.6、地图（map_server）

       地图并不是导航功能所必需的。

 

4、导航功能包集的配置

        在满足以上条件的前提下，我们来针对导航功能进行一些配置。

4.1、创建一个功能包

       首先，我们需要创建一个功能包，用来存储导航需要用到的所有的配置文件和launch启动文件。在创建功能包的时候，我们需要添加相关的所有依赖，包括机器人配置中使用到的功能包，当然不要忘记了move_base功能包，因为该包有很多我们后面需要用到的接口。找到合适的位置，输入以下命令来创建包：

[html]  view plain  copy

1. catkin_create_pkg my_robot_name_2dnav move_base my_tf_configuration_depmy_odom_configuration_dep my_sensor_configuration_dep  

4.2、创建机器人启动文件

        现在，我们已经有了一个存储各种文件的工作空间，下一步，我们来创建一个机器人启动文件，用来启动机器人配置中所提到的所有硬件，并发布相应的消息和变换关系。

        打开编辑器，输入以下格式的内容，并保存为my_robot_configuration.launch命名的文件：

[html]  view plain  copy

1. <launch>  
2.   <nodepkgnodepkg="sensor_node_pkg" type="sensor_node_type"name="sensor_node_name" output="screen">  
3.     <paramnameparamname="sensor_param" value="param_value" />  
4.  </node>  
5.    
6.   <nodepkgnodepkg="odom_node_pkg" type="odom_node_type"name="odom_node" output="screen">  
7.     <paramnameparamname="odom_param" value="param_value" />  
8.  </node>  
9.    
10.   <nodepkgnodepkg="transform_configuration_pkg"type="transform_configuration_type"name="transform_configuration_name" output="screen">  
11.     <paramnameparamname="transform_configuration_param" value="param_value"/>  
12.  </node>  
13. </launch>  

   让我们来详细的解读以上内容的含义：

[html]  view plain  copy

1. <launch>  
2.   <nodepkgnodepkg="sensor_node_pkg" type="sensor_node_type"name="sensor_node_name" output="screen">  
3.     <paramnameparamname="sensor_param" value="param_value" />  

      这部分代码用来启动机器人的传感器，根据以上格式，修改你所使用到的传感器驱动包名称、类型、命名等信息，并且添加驱动包节点需要使用到的参数。当然，如果你需要使用多个传感器，可以使用相同的方法，启动多个传感器的驱动节点。

[html]  view plain  copy

1. <node pkg="odom_node_pkg"type="odom_node_type" name="odom_node"output="screen">  
2.     <paramnameparamname="odom_param" value="param_value" />  
3.  </node>  

      这部分代码用来启动机器人上的里程计，根据需要修改功能包名、类型、节点名、参数。

[html]  view plain  copy

1. <nodepkgnodepkg="transform_configuration_pkg"type="transform_configuration_type"name="transform_configuration_name" output="screen">  
2.    <paramnameparamname="transform_configuration_param" value="param_value"/>  
3. </node>  

      这部分代码需要启动机器人相关的坐标变换。

4.3、代价地图的配置 (local_costmap)& (global_costmap)

       导航功能包使用两种代价地图存储周围环境中的障碍信息，一种用于全局路径规划，一种用于本地路径规划和实时避障。两种代价地图需要使用一些共同和独立的配置文件：通用配置文件，全局规划配置文件，本地规划配置文件。以下将详细讲解这三种配置文件：

（1）通用配置文件（Common Configuration (local_costmap) &(global_costmap)）

      代价地图用来存储周围环境的障碍信息，其中需要注明地图关注的机器人传感器消息，以便于地图信息进行更行。针对两种代价地图通用的配置选项，创建名为costmap_common_params.yaml的配置文件：

[html]  view plain  copy

1. obstacle_range: 2.5  
2. raytrace_range: 3.0  
3. footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  
4. #robot_radius: ir_of_robot  
5. inflation_radius: 0.55  
6.   
7. observation_sources: laser_scan_sensor point_cloud_sensor  
8.   
9. laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}  
10.   
11. point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}  

      详细解析以上配置文件的内容：

[html]  view plain  copy

1. obstacle_range: 2.5  
2. raytrace_range: 3.0  

       这两个参数用来设置代价地图中障碍物的相关阈值。obstacle_range参数用来设置机器人检测障碍物的最大范围，设置为2.5意为在2.5米范围内检测到的障碍信息，才会在地图中进行更新。raytrace_range参数用来设置机器人检测自由空间的最大范围，设置为3.0意为在3米范围内，机器人将根据传感器的信息，清除范围内的自由空间。

[html]  view plain  copy

1. footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  
2. #robot_radius: ir_of_robot  
3. inflation_radius: 0.55  

       这些参数用来设置机器人在二维地图上的占用面积，如果机器人外形是圆形，则需要设置机器人的外形半径。所有参数以机器人的中心作为坐标（0，0）点。inflation_radius参数是设置障碍物的膨胀参数，也就是机器人应该与障碍物保持的最小安全距离，这里设置为0.55意为为机器人规划的路径应该与机器人保持0.55米以上的安全距离。

[html]  view plain  copy

1. observation_sources: laser_scan_sensorpoint_cloud_sensor  

     observation_sources参数列出了代价地图需要关注的所有传感器信息，每一个传感器信息都将在后边列出详细信息。

[html]  view plain  copy

1. laser_scan_sensor: {sensor_frame: frame_name, data_type:LaserScan, topic: topic_name, marking: true, clearing: true}  

       以激光雷达为例，sensor_frame标识传感器的参考系名称，data_type表示激光数据或者点云数据使用的消息类型，topic_name表示传感器发布的话题名称，而marking和clearing参数用来表示是否需要使用传感器的实时信息来添加或清楚代价地图中的障碍物信息。

（2）全局规划配置文件（Global Configuration (global_costmap)）

        全局规划配置文件用来存储用于全局代价地图的配置参数，我们使用global_costmap_params.yaml来命名，内容如下：

[html]  view plain  copy

1. global_costmap:  
2.  global_frame: /map  
3.  robot_base_frame: base_link  
4.  update_frequency: 5.0  
5.   static_map:true  

     global_frame参数用来表示全局代价地图需要在那个参考系下运行，这里我们选择了map这个参考系。robot_base_frame参数表示代价地图可以参考的机器人本体的参考系。update_frequency参数绝地全局地图信息更新的频率，单位是Hz。static_map参数决定代价地图是否需要根据map_server提供的地图信息进行初始化，如果你不需要使用已有的地图或者map_server，最好将该参数设置为false。

（3）本地规划配置文件（Local Configuration (local_costmap)）

       本地规划配置文件用来存储用于本地代价地图的配置参数，命名为local_costmap_params.yaml，内容如下：

[html]  view plain  copy

1. local_costmap:  
2.  global_frame: odom  
3.  robot_base_frame: base_link  
4.  update_frequency: 5.0  
5.  publish_frequency: 2.0  
6.   static_map:false  
7.  rolling_window: true  
8.   width: 6.0  
9.   height: 6.0  
10.   resolution:0.05  

      "global_frame", "robot_base_frame","update_frequency", 和 "static_map"参数的意义与全局规划配置文件中的参数相同。publish_frequency设置代价地图发布可视化信息的频率，单位是Hz。rolling_window参数是用来设置在机器人移动过程中是否需要滚动窗口，以保持机器人处于中心位置。"width," "height," 和"resolution" 设置设置代价地图长（米）、高（米）和分辨率（米/格）。分辨率可以设置的与静态地图不同，但是一般情况下两者是相同的。

4.4 本地规划器配置

       本地规划器base_local_planner的主要作用是根据规划的全局路径，计算发布给机器人的速度指令。该规划器需要我们根据机器人的规格，配置一些相应的参数。我们创建名为base_local_planner_params.yaml的配置文件：

[html]  view plain  copy

1. TrajectoryPlannerROS:  
2.   max_vel_x: 0.45  
3.   min_vel_x: 0.1  
4.   max_vel_theta: 1.0  
5.   min_in_place_vel_theta: 0.4  
6.    
7.   acc_lim_theta: 3.2  
8.   acc_lim_x: 2.5  
9.   acc_lim_y: 2.5  
10.    
11.   holonomic_robot: true  

      该配置文件声明了机器人的本地规划采用Trajectory Rollout算法。第一段设置了机器人的速度阈值，第二段设置了机器人的加速度阈值。

4.5 为导航功能包创建一个启动文件

      到此为止，我们已经创建完毕所有需要用到的配置文件，接下来我们需要创建一个启动文件，来启动所有需要的功能。创建move_base.launch的文件：

[html]  view plain  copy

1. <launch>  
2.   <masterautomasterauto="start"/>  
3.    
4.   <!-- Runthe map server -->  
5.   <nodenamenodename="map_server" pkg="map_server"type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>  
6.    
7.   <!---Run AMCL -->  
8.   <includefileincludefile="$(find amcl)/examples/amcl_omni.launch" />  
9.    
10.   <nodepkgnodepkg="move_base" type="move_base" respawn="false"name="move_base" output="screen">  
11.    <rosparam file="$(find my_robot_name_2dnav)/costmap_common_params.yaml"command="load" ns="global_costmap" />  
12.    <rosparam file="$(findmy_robot_name_2dnav)/costmap_common_params.yaml" command="load"ns="local_costmap" />  
13.    <rosparam file="$(findmy_robot_name_2dnav)/local_costmap_params.yaml" command="load" />  
14.    <rosparam file="$(findmy_robot_name_2dnav)/global_costmap_params.yaml" command="load"/>  
15.    <rosparam file="$(findmy_robot_name_2dnav)/base_local_planner_params.yaml"command="load" />  
16.  </node>  
17. </launch>  

      在该配置文件中，你需要修改的只有map-server输入的地图，以及如果使用差分驱动的机器人，需要修改"amcl_omni.launch"成"amcl_diff.launch" 。

4.6 AMCL功能包的设置

      AMCL有许多的参数设置，会影响机器人的定位效果，具体参考amcldocumentation。

5、运行导航功能包

       现在，我们已经完成了所有需要的工作，最后一步，运行启动文件，开始导航之旅：

[html]  view plain  copy

1. roslaunch my_robot_configuration.launch  
2. roslaunch move_base.launch  

        现在导航功能包应该已经可以顺利运行了，但这绝对不是结束，因为你只能从终端里看到一端乱蹦的代码，如何使用更友好的方式进行机器人导航呢？如果你想使用UI界面，请参考rviz and navigationtutorial，如果你想使用代码，请参考Sending SimpleNavigation Goals 。

----------------------------------------------------------------

欢迎大家转载我的文章。

转载请注明：转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客