通过ROS开启Gazebo的世界

Gazebo是一个不错的仿真工具，它使用物理引擎模拟真实的世界，使得我们可以通过仿真的方式从原理上验证算法，计算负载和受力情况，进而指引我们做结构和算法的设计。 ROS则是一个方便的系统集成工具，可以轻松的监听传感器的数据，发布执行器的控制指令。如果将两者结合在一起，就可以自如的在真实世界和仿真世界之间来回切换。

为了达到自如切换的效果，我们需要研究一下如何在ROS系统中控制Gazebo以及其仿真的机器人模型。这主要涉及到三个方便的要素:(1) Gazebo的运行与world文件的加载 (2) 合理地处理URDF和SDF文件描述的机器人模型 (3) ROS与Gazebo之间的交互接口。本文关注第一个要素，通过rosrun或者roslaunch运行Gazebo。

官方提供了一系列的ROS包称为gazebo_ros_pkgs，提供了用ROS的消息、服务以及动态配置参数的机制在Gazebo环境中模拟一个机器人的接口。它们可以通过Ubuntu原生的apt进行查找和安装:

        $ sudo apt search gazebo-ros

1. 使用roslaunch打开World模型

打开Gazebo并装载世界模型的方式有很多种，roslaunch是ROS系统中标准的启动多个ROS节点的方法，我们这里就先讨论一下如何使用roslaunch来打开一个Gazebo的世界模型。我们可以简单的通过一条语句打开一个各种各样的世界：

        $ roslaunch gazebo_ros empty_world.launch
        $ roslaunch gazebo_ros mud_world.launch
        $ roslaunch gazebo_ros shapes_world.launch
        $ roslaunch gazebo_ros rubble_world.launch

我们来看一下mud_world.launch的文件，它只有一个<include>标签用来加载gazebo_ros的empty_world.launch文件，通过修改参数"world_name"指定需要加载的世界文件。

        <launch>
            <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
            <include file="$(find gazebo_ros)/launch/empty_world.launch">
                <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
                <arg name="world_name" value="worlds/mud.world"/>
                <arg name="paused" value="false"/>
                <arg name="use_sim_time" value="true"/>
                <arg name="gui" value="true"/>
                <arg name="headless" value="false"/> <!-- Inert - see gazebo_ros_pkgs issue #491 -->
                <arg name="recording" value="false"/>
                <arg name="debug" value="false"/>
            </include>
        </launch>

empty_world.launch中只是开启了Gazebo的服务器和客户端两个节点而已，它先定义了如下的一些参数，然后依据这些参数的具体取值开启服务器和客户端。以后需要通过ROS的launch文件打开一个世界模型的时候，就可以像这里的mud_world.launch一样，把empty_world.launch文件include进来，直接修改其中的世界模型的文件名称。

paused: 开启Gazebo后进入暂停模式。
use_sim_time: 告知ROS节点，通过ROS主题/clock来获取Gazebo发布的模拟时间。
gui: 开启一个带有用户界面的Gazebo
headless: (deprecated)，开启gazebo的日志记录
debug: 开启一个debug模式下的gzserver
verbose: 打印errors和warnings到终端

2. 创建一个关于Gazebo的ROS包

我们已经可以用roslaunch开启Gazebo环境了，下面为了更方便的解释ROS与Gazebo之间的交互过程，我们先创建一个gazebo的包，按照ROS的惯例组织目录。我们用catkin_create_pkg创建一个robot_gazebo的包(我们可以用自己的机器人名称替代这里的robot)，并添加依赖关系std_msgs rospy roscpp gazebo_ros和gazebo_plugins。

        $ cd ~/catkin_ws/src
        $  catkin_create_pkg robot_gazebo std_msgs rospy roscpp gazebo_ros gazebo_plugins
        Created file robot_gazebo/CMakeLists.txt
        Created file robot_gazebo/package.xml
        Created folder robot_gazebo/include/robot_gazebo
        Created folder robot_gazebo/src
        Successfully created files in /home/gyc/catkin_ws/src/robot_gazebo. Please adjust the values in package.xml.

我们在robot_gazebo的根目录下建立一个"worlds"的目录，用于存放需要仿真的世界文件。并在该目录下创建一个"robot.world"并复制如下内容。在该世界文件中添加了地面、阳光和一个加油站的模型。

        <?xml version="1.0" ?>
        <sdf version="1.4">
            <world name="default">
                <include>
                    <uri>model://ground_plane</uri>
                </include>
                <include>
                    <uri>model://sun</uri>
                </include>
                <include>
                    <uri>model://gas_station</uri>
                    <name>gas_station</name>
                    <pose>-2.0 7.0 0 0 0 0</pose>
                </include>
            </world>
        </sdf>

然后在robot_gazebo的根目录下创建一个"launch"的目录。并创建"robot.launch"文件复制如下内容。参考刚刚提到的套路，在launch文件中重用gazebo_ros的empty_world.launch，只是修改世界文件名称为我们刚刚建立的"robot.world"。

        <launch>
            <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
            <include file="$(find gazebo_ros)/launch/empty_world.launch">
                <arg name="world_name" value="$(find robot_gazebo)/worlds/robot.world"/>
                <!-- more default parameters can be changed here -->
            </include>
        </launch>

至此，我们就可以通过roslaunch打开刚刚建立的世界模型了。首先进入ROS的工作空间根目录下进行一次编译，然后添加运行环境，最后运行damo，就可以看到如下的一个加油站。

        $ cd ~/catkin_ws
        $ catkin_make
        $ source devel/setup.bash
        $ roslaunch robot_gazebo robot.launch

3. 添加URDF描述的机器人到Gazebo

官方文档中提到了两种添加URDF描述的机器人的方法。推荐使用第一种方法"ROS Service Call Spawn Method"。

"ROS Service Call Spawn Method"这种方法具有更高的可移植性，并且可以保持ROS的相对路径，但是需要调用一个python脚本请求一个ROS服务。
"Model Database Methos"这种方法需要把机器人模型添加到".world"文件中，这看起来更简洁也更方便。但是需要设定一个环境变量把机器人添加到Gazebo模型数据库中。

ROS服务请求的这种方法用一个称为"spawn_model"的python脚本请求gazebo_ros的一个服务来添加URDF到gazebo中。它是gazebo_ros的一个脚本，我们可以用如下的指令进行。这里我们假设在'catkin_ws'下有一个'robot_description'的包，其中有一个存放着机器人描述文件robot.urdf的目录urdf。参数-x, -y和-z表示添加的模型在世界坐标系中的位置，-model则是添加的模型名称。

    $ rosrun gazebo_ros spawn_model -file `rospack find robot_description`/urdf/robot.urdf -urdf -x 0 -y 0 -z 1 -model robot

与ROS的各种原生工具类似，我们可以通过如下的指令查看spawn_model的各种参数及其使用方法：

    $ rosrun gazebo_ros spawn_model -h

不幸的是，很多URDF文件不能直接用Gazebo还需要做一些修改才可以。这里我们对URDF解析器中提到的机器人模型进行一些修改，具体修改方法我们在下文中予以介绍，这里可以下载修改后的文件。我们将之保存在robot_description/urdf命名为GRobot.gazebo.urdf。运行如下指令，就可以看到我们可怜的机器人趴在地上，之所以不能站起来是因为我们没有在URDF文件中给机器人添加任何控制约束。

    $ cd ~/catkin_ws/src
    $ catkin_create_pkg robot_description
    $ cd robot_description
    $ mkdir urdf
    $ cd urdf
    $ wget http://gaoyichao.com/Xiaotu//ros/src/GRobot.gazebo.urdf
    $ rosrun gazebo_ros spawn_model -file `rospack find robot_description`/urdf/GRobot.gazebo.urdf -urdf -x 0 -y 0 -z 1 -model robot

我们完全可以将这一指令添加到launch文件中，在创建世界的时候就把机器人添加进来。

        <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
            args="-file $(find robot_description)/urdf/GRobot.gazebo.urdf -urdf -x 0 -y 0 -z 1 -model robot" />

在robot_gazebo的launch目录下新建一个robot_gazebo.launch的文件，写入如下内容：

        <launch>
            <param name="robot_description" textfile="$(find robot_description)/urdf/GRobot.gazebo.urdf" />

            <include file="$(find gazebo_ros)/launch/empty_world.launch">
                <arg name="world_name" value="$(find robot_gazebo)/worlds/robot.world"/>
            </include>

            <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
                args="-param robot_description -urdf -x 0 -y 0 -z 1 -model robot" />
        </launch>

在该launch文件中，我们先用指定机器人模型到参数robot_description上，再通过gazebo_ros的empty_world.launch文件打开Gazebo，最后通过gazebo_ros的spawn_model导入机器人模型。通过如下指令就也可以看到可怜的机器人像刚才那样趴在地上。

    $ roslaunch robot_gazebo robot_gazebo.launch

4. URDF进入Gazebo世界的洗礼

虽然URDF是ROS系统中标准的文件格式，但是对于仿真而言还是缺少很多特性。它只能够描述一个机器人的运动学和动力学特性，但不能够描述机器人在世界中的位置，而且也缺少摩擦、阻尼等仿真参数的定义。为了解决这些各种问题，Gazebo就创建了SDF(Simulation Description Format)。 SDF本身也是用XML格式的文件，可以使用工具简单方便地从URDF迁移到SDF上。

让URDF描述的文件正常的在Gazebo中工作，我们需要额外添加一些标签。首先，必须为每一个<link>标签添加一个<inertia>的标签并合理的描述惯性数据。 inertia定义了link的质量和惯性矩，它们是进行仿真分析所必须的参数。此外，URDF专门为Gazebo定义了<gazebo>标签，它是URDF的一种扩展，用于添加在Gazebo环境中进行仿真的属性。这些属性都不是必须的，Gazebo会自动的为这些属性赋予默认值，但是如果我们提供了足够的属性描述，仿真的效果就会更好，更接近于真实。

在GRobot.gazebo.urdf中，我们描述了每一个link和joint，为了能够让其在Gazebo中显示出来，我们为每一个link都设置了视图模型、碰撞模型和惯性模型。这里的惯性模型是URDF文件能在Gazebo世界中生存的必要条件，我们可以尝试删掉其中任意一个link的惯性模型，重新运行程序会看到没有机器人趴在地上。以我的理解，Gazebo本质上还是使用的SDF格式进行的仿真，我们之所以可以导入URDF格式的模型，是因为gazebo_ros的工具spawn_model做了格式的转换。这点我没有查证过，以后有时间了就看下这个python脚本确认一下。Gazebo还提供了一个方便的工具把URDF文件转换到SDF格式，我们可以通过它来检验URDF文件是否可以被Gazebo接受：

    $ gz sdf -p MODEL.urdf

其中，MODEL为需要检测的目标模型，可以根据需要替换之。如果格式没有问题，这条指令会输出成功转换后的SDF格式描述。如果存在问题，将不能够输出正常的描述。

5. <gazebo>标签

URDF专门定义了一个<gazebo>标签用来描述在SDF格式中定义但未在URDF中定义的属性。它有三个等级，对应着<robot>、<link>和<joint>。定义时需要使用属性"reference"指定修饰的对象。

在视图模型中我们还为每个link添加了颜色，但是这些颜色配置只在rviz中有作用，虽然我们为不同的link设置了不同的颜色但是在Gazebo中看来都是白白的一片。实际上为文件中还为每一个link设置了一个gazebo的属性来描述它们在Gazebo中的颜色，只是被注释掉了。现在将注释去掉，就可以看到一个蓝色的机器人趴在地上，如右图所示。下面是base_link的<gazebo>标签，通过reference指定其中定义的颜色是对base_link的附加描述。

        <link name="base_link">
            ...
        </link>
        <gazebo reference="base_link">
            <material>Gazebo/Blue</material>
        </gazebo>

如果没有指定，那么就默认是一个<robot>标签，是对整个机器人的描述。下表列出了针对<robot>的所有<gazebo>元素:

Name	Type	Description
static	bool	如果设定为真，那么模型将是不可移动的。否则将参与物理引擎的运算。

我们可以试着在刚才的GRobot.gazebo.urdf中，添加如下的语句后重新运行刚才的指令，就会看到机器人吊在半空中，一动不动。

        <gazebo><static>true</static></gazebo>

下表列出了针对Link的<gazebo>的属性列表:

Name	Type	Description
material	value	视觉模型的材料。
gravity	bool	是否考虑重力。
dampingFactor	double	阻尼系数。
maxVel	double	maximum contact correction velocity truncation term
minDepth	double	应用contact correction impulse之前所允许的最小深度
mu1	double	物理引擎Open Dynamics Engine(ODE)定义的摩擦系数，参见ODE文档。
mu2	double	物理引擎Open Dynamics Engine(ODE)定义的摩擦系数，参见ODE文档。
fdir1	string	mu1的方向向量
kp	double	ODE定义的碰撞刚度kp和阻尼系数kd。ODE使用的是erp和cfm，但存在一个从erp/cfm到stiffness/damping的映射。
kd	double
selfCollide	bool	如果为真，那么对应link将可以与模型中的其它link碰撞。
maxContacts	int	允许接触的最大物体数量。这个值将覆盖物理引擎中定义的max_contacts。
laserRetro	double	雷达传感器返回的密度值。

下表列出了针对Joint的<gazebo>的属性列表:

Name	Type	Description
stopCfm	double	ODE中定义的Joint的cfm和erp。
stopErp	double	ODE中定义的Joint的cfm和erp。
provideFeedback	bool	允许joint通过Gazebo插件发布它们的受力数据(包括力-力矩)。
implicitSpringDamper	bool	如果这些对象值为真，ODE将使用ERP和CFM模拟阻尼。这是一种比默认阻尼更稳定的数值方法。 cfmDamping标签已经启用了，应当改用implicitSpringDamper
cfmDamping	bool
fudgeFactor	double	Scale the excess for in a joint motor at joint limits. 取值在0到1之间。

关于这些<gazebo>的标签描述原文档写的很混乱，暂时写成这样，后续再修改。

6. 总结

为了把Gazebo与ROS环境整合在一起，官方提供了gazebo_ros的包可以方便的使用ros的各种工具访问Gazebo。在本文中我们介绍了使用roslaunch打开一个Gazebo仿真世界的方法，在launch文件中通过include把gazebo_ros中的empty_world.launch包含进来，然后通过修改参数world_name的值为需要仿真的世界文件名称。

我们通过gazebo_ros中的一个python脚本spawn_model发送一个添加模型的请求，就可以把我们用URDF写的机器人模型添加到仿真环境中。若要在Gazebo中正常使用URDF格式，就必须为每一个link标签添加质量和惯性张量的描述，因为它们是做仿真所必须的数据。此外，URDF格式中专门定义了一个<gazebo>的标签，用来描述在SDF中定义的属性。