用SDF文件模拟激光雷达
在上一篇文章中，我们介绍了如何使用Gazebo提供的模型编辑器来创建一个机器人模型，并在仿真中使用。 通过GUI的形式创建机器人模型的过程是很繁琐的，尤其是在机器人比较复杂的时候。而且我个人在使用过程中，发现它的GUI界面还是有很多Bug的，所以推荐尽量使用SDF文件来描述机器人。 在本文中，我们将用SDF文件来描述一个激光雷达。
SDF是一个XML格式的描述文件，用来在仿真、可视化和控制中描述物体和环境。最初，它是Gazebo机器人仿真器的一部分，为了科学研究而设计的。经过几年的发展， SDF已经是一个稳定的、鲁棒的、可扩展的描述方法，可以很好的描述机器人、静态和动态的物体、光照、地形、甚至是物理特性。
在本文中我们要模拟的是Velodyne HDL-32 LiDAR，其外形和机械尺寸如下图1所示。它由上下两个圆柱体构成， 下面是一个固定的基座，上面是一个可以旋转的激光传感器。随着上部传感器的旋转，不断的扫描周围环境，通过SLAM等技术我们就可以构建机器人所在环境的地图。 为方便讲解，下面我们根据传感器的外形先简单的用两个圆柱体来描述传感器。
 |
| 图1. HDL-32外形尺寸 |
1. 创建基本的SDF模型
首先，我们找一个路径创建"velodyne.world"文件。如下的代码所示，这里我们在"~/catkin_ws/src/learning_gazebo"目录中完成。 第二行中所用的vim是一个比较流行的文本编辑器，读者可以根据自己的喜好自由选择，比如说Ubuntu原生的gedit等。
$ cd ~/catkin_ws/src/learning_gazebo
$ vim velodyne.world
"*.world"文件用SDF描述了一次仿真中的所有想定，包括环境、灯光、机器人等各种仿真中需要考虑的元素，Gazebo服务器根据这个世界文件来初始化一次仿真。
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>
如左面代码所示，这里我们初步建立一个有地面和光照的世界。
其中<sdf>标签是一个SDF文件的根节点，它有一个版本号标识着这个SDF文件遵循的标准版本。目前最新版本是V1.6，一般我们使用的是V1.5。 它可以有<world>，<model>，<actor>，<light>等不同类型的子标签。详细解释可以参考链接。
<world>则描述了一次仿真中的各个对象，它有一个"name"的属性，唯一标识了一个世界。在同一个<sdf>标签下，可以有多个不同的<world>。 关于标签<world>的详细描述可以参考链接，这里我们定义了一个称为"default"的世界。
在<world>中，我们先后通过include的形式添加了"sun"和"ground_plane"两个模型。一般情况下，这两个模型的文件就在"~/.gazebo/models"目录中。 如果没有找到，我们也可以从官网下载这些模型保存到这个目录下。
在Gazebo中好像很强调模型的复用，以至于向大地、阳光等一些比较通用的设置都专门通过模型的形式发布。 Gazebo鼓励大家把自己创建的新模型发布到社区中，这样人们就可以根据自己的需要下载修改自己的模型，毕竟站在巨人的肩膀上往往可以少走一些弯路。
下面我们就要在这个叫做"default"的世界中创建一个模型来描述HDL32。让我们把下面的代码拷贝到</world>标签之前。虽然已经做了比较详细的注释， 这里我们还是稍微解释一下这段代码。这里我们创建了一个叫做"velodyne_hdl-32"的模型，它由"base"和"top"两个link构成，分别对应着HDL32的基座和扫描器两个部分。 在每一个link中我们都在<pose>中指定了其位置和姿态，并在<collision>和<visual>中描述了碰撞和视图模型。 其中的数值都是根据Velodyne公司提供的参数计算得到的。
<model name="velodyne_hdl-32">
<!-- Give the base link a unique name -->
<link name="base">
<!-- Offset the base by half the lenght of the cylinder -->
<pose>0 0 0.029335 0 0 0</pose>
<collision name="base_collision">
<geometry>
<cylinder>
<!-- Radius and length provided by Velodyne -->
<radius>.04267</radius>
<length>.05867</length>
</cylinder>
</geometry>
</collision>
<!-- The visual is mostly a copy of the collision -->
<visual name="base_visual">
<geometry>
<cylinder>
<radius>.04267</radius>
<length>.05867</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- Give the base link a unique name -->
<link name="top">
<!-- Vertically offset the top cylinder by the length of the bottom
cylinder and half the length of this cylinder. -->
<pose>0 0 0.095455 0 0 0</pose>
<collision name="top_collision">
<geometry>
<cylinder>
<!-- Radius and length provided by Velodyne -->
<radius>0.04267</radius>
<length>0.07357</length>
</cylinder>
</geometry>
</collision>
<!-- The visual is mostly a copy of the collision -->
<visual name="top_visual">
<geometry>
<cylinder>
<radius>0.04267</radius>
<length>0.07357</length>
</cylinder>
</geometry>
</visual>
</link>
</model>
我们可以通过指令运行Gazebo加载我们新建的带有"velodyne_hdl-32"模型的world文件。
$ gazebo velodyne.world -u
下图显示的是hdl32的碰撞模型。在Gazebo中默认显示的是视图模型，即<visual>标签描述的图形，它描述了一个模型看起来应该是什么样子。
而碰撞模型，<collision>，标签则描述了模型与其它模型碰撞时的行为是实际物理仿真时需要的参数。在Gazebo的GUI界面中，通过右击模型
然后选择View->Collisions，就可以看到碰撞模型，就是这里显示的橘黄色的圆柱体。
它跟视图模型似乎没有什么区别，那是因为我们在建立模型的时候给两者相同的参数。感兴趣的读者可以试着修改一下模型的尺寸看看会产生什么样的效果。
 |
| 图2. Gazebo下模型"velodyne_hdl-32"的碰撞模型 |
2 添加惯性数据
现在我们的模型还缺少向惯性数据这样的动力学属性。在物理引擎中都是根据惯性信息来计算当一个力施加在模型上的行为。一个错误的惯性数据将产生很奇怪的现象， 所以我们需要提供一组比较准确的惯性数据。一般我们说的惯性数据包括质量(mess)和惯性张量(inertia matrix)两个部分。
质量很好理解，中学我们就在接触不再讲了。惯性张量主要是对物体旋转状态的惯性描述，它是一个3×3的对称矩阵： $$ \boldsymbol{I} = \begin{bmatrix} I_{xx} & I_{xy} & I_{zx} \\ I_{xy} & I_{yy} & I_{yz} \\ I_{zx} & I_{yz} & I_{zz} \end{bmatrix} = \begin{bmatrix} \sum m(y^2 + z^2) & -\sum mxy & -\sum mxz \\ -\sum myz & \sum m(z^2 + x^2) & -\sum myz \\ -\sum mzx & -\sum mzy & \sum m(x^2 + y^2) \end{bmatrix} $$ 式中\(m\)为系统中某个质点的质量，\(x,y,z\)分别为改质点在空间中的坐标。我们称\(I_{xx}, I_{yy}, I_{zz}\)为刚体对三个坐标轴的转动惯量， \(I_{xy}, I_{yz}, I_{zx}\)为惯性积。我们分别给基座和扫描器定义惯性数据如下：
|
|
 |
| 图2. Gazebo下模型"velodyne_hdl-32"的惯性数据 |
3 描述关节
至此，我们已经有了一个模型，描述了Hdl32的外观、碰撞以及惯性属性。下面我们要为这个模型添加关节(joint)，让它动起来。
Joint描述了两个Link之间的约束关系，在机器人中最常用的Joint就是转动关节(revolute joint)。它定义了两个Link之间可以相对转动的自由度。 在SDF官网上列举了所有可用的关节类型。我们在</model>标签前添加一个<joint>标签，定义一个revolute类型的关节。
<joint type="revolute" name="joint">
<!-- Position the joint at the bottom of the top link -->
<pose>0 0 -0.036785 0 0 0</pose>
<parent>base</parent>
<child>top</child>
<!-- The axis defines the joint's degree of freedom -->
<axis>
<!-- Revolve around the z-axis -->
<xyz>0 0 1</xyz>
<!-- Limit refers to the range of motion of the joint -->
<limit>
<!-- Use a very large number to indicate a continuous revolution -->
<lower>-10000000000000000</lower>
<upper>10000000000000000</upper>
</limit>
</axis>
</joint>
其中<parent>和<child>两个标签描述了"top"通过"joint"连接到了"base"上，连杆"top"在空间中的位置通过"base"的位置以及"joint"的转角决定。
<pose>则指明了连接处，其坐标是相对于<child>原点的，这里就是"top"的底部。<axis>详细描述了关节的约束，这里通过标签<xyz>说明"top"是绕着z轴旋转的。
Joint只是描述了两个部件之间的连接关系和约束，所以它是不存在视图、碰撞、质量等等信息的。我们仍然通过指令$ gazebo velodyne.world -u来加载模型，
直接是看不到关节的。需要右击模型，然后选择View->Joints和View->Transparent，就可以看到在"top"的底部有一个坐标系。这个坐标系就是joint，
在它的z轴上有一个圆圈，表示"top"绕着z轴旋转。如下图3所示。
 |
| 图3. Gazebo下模型"velodyne_hdl-32"的Joint |
 |
4 添加传感器
现在让我们来为激光雷达模型添加一个"ray"的传感器，它主要用于模拟激光测距。 首先，我们需要在标签<link name="top">后面添加如下的代码，来描述激光传感器的位置姿态、更新频率等信息。
<!-- Add a ray sensor, and give it a name -->
<sensor type="ray" name="sensor">
<!-- Position the ray sensor based on the specification. Also rotate
it by 90 degrees around the X-axis so that the rays
become vertical -->
<pose>0 0 -0.004645 1.5707 0 0</pose>
<!-- Enable visualization to see the rays in the GUI -->
<visualize>true</visualize>
<!-- Set the update rate of the sensor -->
<update_rate>30</update_rate>
</sensor>
然后，在标签<update_rate>30</update_rate>之后添加如下的代码，描述"ray"传感器的具体属性。它主要有两个部分：
<scan>描述了激光束的数量、扫描的角度、分辨率等信息。<range>则定义了每个激光束的特性。
<ray>
<!-- The scan element contains the horizontal and vertical beams.
We are leaving out the vertical beams for this tutorial. -->
<scan>
<!-- The horizontal beams -->
<horizontal>
<!-- The velodyne has 32 beams(samples) -->
<samples>32</samples>
<!-- Resolution is multiplied by samples to determine number of
simulated beams vs interpolated beams. See:
http://sdformat.org/spec?ver=1.6&elem=sensor#horizontal_resolution-->
<resolution>1</resolution>
<!-- Minimum angle in radians -->
<min_angle>-0.53529248</min_angle>
<!-- Maximum angle in radians -->
<max_angle>0.18622663</max_angle>
</horizontal>
</scan>
<!-- Range defines characteristics of an individual beam -->
<range>
<!-- Minimum distance of the beam -->
<min>0.05</min>
<!-- Maximum distance of the beam -->
<max>70</max>
<!-- Linear resolution of the beam -->
<resolution>0.02</resolution>
</range>
</ray>
如果我们通过指令$ gazebo velodyne.world加载模型，就可以看到类似下图4(a)所示的HDL32模型，它向外发出32道激光束。
为了查看传感器的数据，我们可以在光束的正前方放置一个立方体，可以看到所有的光束都变成了深蓝色，如图4(b)所示。选择菜单项Window->Topic Visualization，
在弹出的窗口中选择"/gazebo/default/velodyne_hdl-32/top/sensor/scan"点击OK后就可以看到图4(b)中左上角的窗口，它形象的显示了传感器测量的距离。
如果读者看到的是一片空白，可以通过鼠标的滚轮放大图像的，也可以点击鼠标左键拖动图像。在移动鼠标的过程中也可以看到传感器实际测量的数据。
 |
 |
| 图4(a). Gazebo下模型"velodyne_hdl-32"在发光 | 图4(b). Gazebo下模型"velodyne_hdl-32"传感器数据 |
|
 |
5. 添加Mesh文件
虽然我们已经用SDF描述了Hdl32，但它只有两个圆柱体，跟图片还是差了很远。Gazebo支持通过Mesh文件的形式加载模型，这样可以很大程度上提高视觉效果以及仿真的真实度。 所谓的Mesh文件就是一个描述物体3D模型的文件，它是一种相对比较通用的格式，在不同的绘图软件下都能打开。
Velodyne公司已经为我们提供了STEP格式的Mesh文件。但是Gazebo目前只支持STL, OBJ和Collada格式的文件， 所以我们还需要对其进行一下格式转换。在Gazebo的官方教程中，使用了freecad和Blender两个软件对其进行了转换， 仅使用freecad就已经可以将STEP格式转换为Collada格式，但是转换出来的模型参数上存在一些问题，主要是尺度单位等方面的问题，所以教程就又使用Blender进一步做了修改。 这里不再对转换过程进行介绍，我们直接使用转换完成后的velodyne_base.dae 和velodyne_top.dae分别作为基座和传感器的Mesh文件。
在使用Mesh文件之前，我们先为HDL32专门建一个目录用于组织模型描述文件、代码文件、以及各种资源文件。 在官方教程中推荐了模型目录的组织形式。下面我们在目录"~/.gazebol/models"中创建一个"velodyne_hdl32"的目录， 并创建model.config文件描述模型的基本信息。
$ mkdir ~/.gazebo/models/velodyne_hdl32
$ vim ~/.gazebol/models/velodyne_hdl32/model.config
将下面的xml内容拷贝到这个配置文件中，这里主要记录了模型的名称、版本、描述模型的sdf文件、作者信息以及一些附加的描述信息。
<?xml version="1.0"?>
<model>
<name>Velodyne HDL-32</name>
<version>1.0</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Optional: YOUR NAME</name>
<email>Optional: YOUR EMAIL</email>
</author>
<description>
A model of a Velodyne HDL-32 LiDAR sensor.
</description>
</model>
我们需要再创建一个"model.sdf"文件，并把原来"velodyne.world"中的内容拷贝过来，删掉其中用于添加sun和ground的<include>标签，以及<world>的opening和closing标签。
得到一个只有<model>标签的文件。此时，
我们我们就可以直接在Gazebo的左侧边栏的Insert选项卡中找到Velodyne HDL-32并选中添加到仿真场景中了。
下面我们将实际添加mesh文件。首先在"~/.gazebo/models/velodyne_hdl32"中创建"meshes"子目录，并将已经转换好格式的velodyne_base.dae 和velodyne_top.dae拷贝到该目录下：
$ mkdir ~/.gazebo/models/velodyne_hdl32/meshes
$ cp velodyne_base.dae ~/.gazebo/models/velodyne_hdl32/meshes
$ cp velodyne_top.dae ~/.gazebo/models/velodyne_hdl32/meshes
然后编辑"model.sdf"文件，将其中描述"top"部件的视图模型<visual name="top_visual">标签中的内容用如下的代码替换。然后通过左侧边栏插入模型的时候，我们就会看到下面右图所示的样子。
传感器部分用Mesh文件渲染了之后就与真实的传感器很像了。
|
 |
但是它的位置有些不对，激光不是从开的那个槽中射出的，而且没有与底座向量。这是因为转换Mesh文件格式的时候，没有正确的调整3D模型的参数。 我们有两种方式校正这一问题。第一种就是用Blender打开Mesh文件，直接修改模型的参数。另一种方式是在model.sdf文件中做适当的修改来使它达到想要的效果。 这里我们使用第二种方式，在<visual name="top_visual">标签中添加一个<pose>的标签来调整视图模型的位置和姿态，代码和效果如下所示。
|
 |
采用类似的方式我们还可以为底座添加Mesh文件，这样我们的激光雷达模型就想点样子了。此外，我们还可以给Mesh文件添加纹理、或者使用OGRE material script进一步装饰模型，这里就不再细说了。
|
 |
4. 总结
通过本文我们会发现SDF与URDF之间存在着很多相似之处。它们都是基于xml语言的描述方法，都通过link和joint来描述机器人的， joint也是以父子关系描述约束的两个link。并不好对比两者谁好谁坏，它们各自有自己的优点和使用场景。SDF在Gazebo这类仿真软件中用的比较多，URDF在ROS系统中用的比较多。 当然URDF也可以在Gazebo中使用，但需要额外做些工作。真心希望它们能够统一起来，也希望机械绘图的软件能够更好的支持SDF和URDF，以后建模仿真、机械图纸等等最好可以通过一个环境来搞定。