用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服务器根据这个世界文件来初始化一次仿真。
model://sun
model://ground_plane
如左面代码所示，这里我们初步建立一个有地面和光照的世界。
其中
在
在Gazebo中好像很强调模型的复用，以至于向大地、阳光等一些比较通用的设置都专门通过模型的形式发布。 Gazebo鼓励大家把自己创建的新模型发布到社区中，这样人们就可以根据自己的需要下载修改自己的模型，毕竟站在巨人的肩膀上往往可以少走一些弯路。
下面我们就要在这个叫做"default"的世界中创建一个模型来描述HDL32。让我们把下面的代码拷贝到标签之前。虽然已经做了比较详细的注释， 这里我们还是稍微解释一下这段代码。这里我们创建了一个叫做"velodyne_hdl-32"的模型，它由"base"和"top"两个link构成，分别对应着HDL32的基座和扫描器两个部分。 在每一个link中我们都在
0 0 0.029335 0 0 0
.04267
.05867
.04267
.05867
0 0 0.095455 0 0 0
0.04267
0.07357
0.04267
0.07357
我们可以通过指令运行Gazebo加载我们新建的带有"velodyne_hdl-32"模型的world文件。
$ gazebo velodyne.world -u
下图显示的是hdl32的碰撞模型。在Gazebo中默认显示的是视图模型，即
| 转存失败重新上传取消 |
| 图2. Gazebo下模型"velodyne_hdl-32"的碰撞模型 |
2 添加惯性数据
现在我们的模型还缺少向惯性数据这样的动力学属性。在物理引擎中都是根据惯性信息来计算当一个力施加在模型上的行为。一个错误的惯性数据将产生很奇怪的现象， 所以我们需要提供一组比较准确的惯性数据。一般我们说的惯性数据包括质量(mess)和惯性张量(inertia matrix)两个部分。
质量很好理解，中学我们就在接触不再讲了。惯性张量主要是对物体旋转状态的惯性描述，它是一个3×3的对称矩阵：
I=⎡⎣⎢⎢⎢IxxIxyIzxIxyIyyIyzIzxIyzIzz⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢∑m(y2+z2)−∑myz−∑mzx−∑mxy∑m(z2+x2)−∑mzy−∑mxz−∑myz∑m(x2+y2)⎤⎦⎥⎥⎥
式中m为系统中某个质点的质量，x,y,z分别为改质点在空间中的坐标。我们称Ixx,Iyy,Izz为刚体对三个坐标轴的转动惯量， Ixy,Iyz,Izx为惯性积。我们分别给基座和扫描器定义惯性数据如下：
|
|
我们可以在Gazebo中通过右击模型，然后选择View->Inertia查看添加了惯性数据的模型。如下图3所示，我也不太清楚为什么是一个立方体的形式，既然官方例程说是这个样子， 我们就姑且这么认为吧，以后再深究这个问题。
| 转存失败重新上传取消 |
| 图2. Gazebo下模型"velodyne_hdl-32"的惯性数据 |
3 描述关节
至此，我们已经有了一个模型，描述了Hdl32的外观、碰撞以及惯性属性。下面我们要为这个模型添加关节(joint)，让它动起来。
Joint描述了两个Link之间的约束关系，在机器人中最常用的Joint就是转动关节(revolute joint)。它定义了两个Link之间可以相对转动的自由度。 在SDF官网上列举了所有可用的关节类型。我们在标签前添加一个
0 0 -0.036785 0 0 0
base
top
0 0 1
-10000000000000000
10000000000000000
其中
Joint只是描述了两个部件之间的连接关系和约束，所以它是不存在视图、碰撞、质量等等信息的。我们仍然通过指令$ gazebo velodyne.world -u来加载模型， 直接是看不到关节的。需要右击模型，然后选择View->Joints和View->Transparent，就可以看到在"top"的底部有一个坐标系。这个坐标系就是joint， 在它的z轴上有一个圆圈，表示"top"绕着z轴旋转。如下图3所示。
| 转存失败重新上传取消 |
| 图3. Gazebo下模型"velodyne_hdl-32"的Joint |
我们可以拖拽打开右边栏，然后点击选中模型，在右边栏中就会多出joint的配置选项。我们在Force选项卡中个这个关节施加0.001的作用力，如下图所示，然后开启仿真，我们就可以看到joint的坐标系在不断的旋转。
 |
4 添加传感器
现在让我们来为激光雷达模型添加一个"ray"的传感器，它主要用于模拟激光测距。 首先，我们需要在标签后面添加如下的代码，来描述激光传感器的位置姿态、更新频率等信息。
0 0 -0.004645 1.5707 0 0
true
30
然后，在标签
32
1
-0.53529248
0.18622663
0.05
70
0.02
如果我们通过指令$ 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"传感器数据 |
在实际的测量过程中，因为各种各样不确定性的因素，得到的数据总是存在一定的噪声的，这点在Gazebo中也是可以模拟的。我们可以在
|
 |
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文件、作者信息以及一些附加的描述信息。
Velodyne HDL-32
1.0
model.sdf
Optional: YOUR NAME
Optional: YOUR EMAIL
A model of a Velodyne HDL-32 LiDAR sensor.
我们需要再创建一个"model.sdf"文件，并把原来"velodyne.world"中的内容拷贝过来，删掉其中用于添加sun和ground的
下面我们将实际添加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"部件的视图模型
|
转存失败重新上传取消 |
但是它的位置有些不对，激光不是从开的那个槽中射出的，而且没有与底座向量。这是因为转换Mesh文件格式的时候，没有正确的调整3D模型的参数。 我们有两种方式校正这一问题。第一种就是用Blender打开Mesh文件，直接修改模型的参数。另一种方式是在model.sdf文件中做适当的修改来使它达到想要的效果。 这里我们使用第二种方式，在
|
转存失败重新上传取消 |
采用类似的方式我们还可以为底座添加Mesh文件，这样我们的激光雷达模型就想点样子了。此外，我们还可以给Mesh文件添加纹理、或者使用OGRE material script进一步装饰模型，这里就不再细说了。
|
转存失败重新上传取消 |
4. 总结
通过本文我们会发现SDF与URDF之间存在着很多相似之处。它们都是基于xml语言的描述方法，都通过link和joint来描述机器人的， joint也是以父子关系描述约束的两个link。并不好对比两者谁好谁坏，它们各自有自己的优点和使用场景。SDF在Gazebo这类仿真软件中用的比较多，URDF在ROS系统中用的比较多。 当然URDF也可以在Gazebo中使用，但需要额外做些工作。真心希望它们能够统一起来，也希望机械绘图的软件能够更好的支持SDF和URDF，以后建模仿真、机械图纸等等最好可以通过一个环境来搞定。