Gazebo相关内容学习

什么是Gazebo和ros,以及二者的关系？

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

为了达到自如切换的效果，怎样在ROS系统中控制Gazebo以及其仿真的机器人模型？

答：需要3个方面的要素:
(一) Gazebo的运行与world文件的加载
(二) 正确的处理URDF和SDF文件描述的机器人模型
(三) Gazebo与ros之间的交互接口。

对于要素(一)和要素(二)的操作？

        Gazebo可以通过rosrun或者roslaunch运行。但主要以roslaunch运行。world文件也主要以roslaunch加载。
下面用一个例子，演示1个简单的虚拟3D环境并在其中加入机器人，以及机器人从环境中获取数据给ros，这个仿真平台的搭建过程：
步骤1：使用roslaunch加载world模型
        gazebo_ros给我们提供了很多种世界模型，并且我们可以简单的使用一条语句打开任一个世界。
        举例说明：

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

从上面4种模型中，打开mud_world模型，即打开mud_world.launch进行查看，从下面可看出它只有一个标签用来加载gazebo_ros的empty_world.launch文件，通过修改"world_name"参数来指定需要加载的世界文件。

        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包
          我们从步骤1已学会gazebo如何启动，为了实现1个完整ros工程中ROS与Gazebo之间的交互，要学会此工程下代码的编写，故我们先创建一个package包，按照ROS的惯例组织目录。
          我们用catkin_create_pkg命令创建一个robot_gazebo的package包(我们可以用自己的机器人名称替代这里的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
  然后，在robot_gazebo的目录中新建一个名为"worlds"的目录，用于存放需要仿真的世界模型文件(此文件类似于mud.world)。并在该目录下创建一个"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"文件复制如下内容。参考步骤1中的套路，在launch文件中重用gazebo_ros的empty_world.launch， 只是修改世界文件名称为我们刚刚建立的"robot.world"。

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

步骤3：把URDF描述的机器人添加到Gazebo(即世界地图)中
        官方文档中提到了2种添加URDF描述的机器人的方法：
        方法1：ROS Service Call Spawn Method–这种方法具有更高的可移植性，并且可以保持ROS的相对路径，但是需要调用一个python脚本请求一个ROS服务。推荐使用
        方法2：Model Database Methos–这种方法需要把机器人模型添加到".world"文件中，这看起来更简洁也更方便。但是需要设定一个环境变量把机器人添加到Gazebo模型数据库中。
        方法1用一个称为"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

注意：很多URDF文件不能直接添加到Gazebo中还需做进一步修改。这里我们对刚刚提到的在URDF解析器中已设计好的机器人模型robot.urdf做进行一些修改，具体修改方法我们在下文步骤4中予以介绍，这里可以下载修改后的文件。我们将之保存在robot_description/urdf中并命名为GRobot.gazebo.urdf。运行下面指令，就可以看到机器人添加到world中并趴在地上，之所以不能站起来是因为我们没有在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_world.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"/>
        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="true"/>
        <arg name="gui" value="true"/>
        <arg name="headless" value="false"/>
        <arg name="recording" value="false"/>
        <arg name="debug" value="false"/>
    </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_world.launch

步骤4：URDF添加到Gazebo世界之前的洗礼
        虽然URDF是ROS系统中标准的文件格式，但是对于仿真而言还是缺少很多特性。它只能够描述一个机器人的运动学和动力学特性，但不能够描述机器人在世界中的位置，而且也缺少摩擦、阻尼等仿真参数的定义。为了解决这些各种问题，Gazebo就创建了SDF(Simulation Description Format)。SDF本身也是用XML格式的文件，可以使用工具简单方便地从URDF迁移到SDF上。
        让URDF描述的文件正常的在Gazebo中工作，我们需要额外添加一些标签。首先，必须为每一个link标签添加一个inertial标签并合理的描述惯性数据。
        inertia定义了link的质量和惯性矩，它们是进行仿真分析所必须的参数。此外，URDF中有专门的gazebo标签(步骤5详介)，它是URDF的一种扩展，用于添加在Gazebo环境中进行仿真的属性。这些属性都不是必须的，Gazebo会自动的为这些属性赋予默认值，但是如果我们提供了足够的属性描述，仿真的效果就会更好，更接近于真实。
        在GRobot.gazebo.urdf(链接地址://gaoyichao.com/Xiaotu/ros/src/GRobot.gazebo.urdf)中，我们描述了每一个link和joint，为了能够使其在Gazebo中显示出来，我们为每一个link都设置了视图模型、碰撞模型和惯性模型。这里的惯性模型是URDF文件能在Gazebo世界中生存的必要条件，我们可以尝试删掉其中任意一个link的惯性模型，重新运行程序会看到没有机器人趴在地上。
        以我的理解，Gazebo本质上还是使用的SDF格式进行的仿真，我们之所以可以导入URDF格式的模型，是因为gazebo_ros的工具spawn_model做了格式的转换。
Gazebo还提供了一个方便把URDF文件转换到SDF格式的工具，我们可以通过它来检验URDF文件是否可以被Gazebo接受：

$ gz sdf -p MODEL.urdf

其中,MODEL为需要检测的目标模型，可以根据需要替换之。如果格式没有问题，这条指令会成功输出转换后的SDF格式描述。如果存在问题，将不能够输出正常的描述。
步骤5：SDF格式中的gazebo标签
        URDF专门有一个gazebo标签用来描述在SDF格式中定义但未在URDF中定义的属性。它有三个等级，对应着robot、link和joint，即机器人整体层面、零件层面、关节层面。定义时需要使用属性"reference"指定修饰的对象。
        在视图模型中我们还为每个link添加了颜色，但是这些颜色配置只在rviz中有作用，虽然我们为不同的link设置了不同的颜色但是在Gazebo中看来都是白白的一片。
实际上在文件中还为每一个link设置了一个gazebo的属性来描述它们在Gazebo中的颜色，只是被注释掉了。现在将注释去掉，就可以看到一个蓝色的机器人趴在地上，如图所示。

下面是base_link(即零件层面中的一个零件对象base_link)的gazebo标签，通过reference指定其中定义的颜色是对base_link的附加描述。

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

        注意：如果在URDF中没有使用gazebo标签进行指定，则默认是一个标签(即机器人整体层面而言)，是对整个机器人的描述。
三个层面下各自的参数：
a) 针对robot的的属性：

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

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

b) 针对Link的的属性列表:

ODE文档
ODE使用的是erp和cfm
从erp/cfm到stiffness/damping的映射。
c) 针对Joint的的属性列表:

        综上而言，SDF与URDF之间存在着很多相似之处。它们都是基于xml语言的描述方法，都通过link和joint来描述机器人的， joint也是以父子关系描述约束的两个link。并不好对比两者谁好谁坏，它们各自有自己的优点和使用场景。SDF在Gazebo这类仿真软件中用的比较多，URDF在ROS系统中用的比较多。 当然URDF也可以在Gazebo中使用，但需要额外做些工作(即添加gazebo标签及其内容)。
步骤6：Gazobo中添加传感器
在Gazebo中，你能够对机器人的物理运动进行仿真。你同样能仿真它的传感器。下面以添加一个摄像头和激光雷达传感器为例。
首先，我们需要做的是向 之前已经写好的URDF文件中增加下面的代码，来为机器人添加 Hokuyo 激光雷达3D模型：

  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://robot1_description/meshes/hokuyo.dae"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <!-- hokuyo -->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.570796</min_angle>
            <max_angle>1.570796</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

其次，在 robot.gazebo文件里，我们将添加 libgazebo_ros_laser 插件，这样就可以模拟 Hokuyo 激光测距雷达的行为。
最后，可以启动模型文件了：
采用同样的方法，我们向 robot.gazebo 和URDF模型文件中添加几行代码以增加另一个传感器（一个摄像头）。
注意，这个激光雷达会像真实的雷达一样产生“真实"的传感器数据。你能够通过 rostopic echo 命令看到这些数据：

$ rostopic echo /robot/laser/scan

我们可以向摄像头发出相同命令，但如果你想观察摄像头看到的 gazebo 仿真图像，你可以在终端中写入以下指令：

$ rosrun image_view image_view image:=/robot/camera1/image_raw

步骤7：控制机器人在gazebo中移动
        滑移转向（skid-steer）机器人是一种对机身两侧*分别进行驱动的移动机器人。它通过将两侧*控制在不同的转速（所产生的转速差）进行转向，而不需要*有任何转向行动。
        和前面的激光雷达一样，gazebo 也已经有了 skid 驱动的实现，我们能够直接使用它移动机器人。使用此行驶控制器，只需要在模型文件中增加以下代码：

<!-- Drive controller -->
<gazebo>
  <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
    <updateRate>100.0</updateRate>
    <robotNamespace>/</robotNamespace>
    <leftFrontJoint>base_to_wheel1</leftFrontJoint>
    <rightFrontJoint>base_to_wheel3</rightFrontJoint>
    <leftRearJoint>base_to_wheel2</leftRearJoint>
    <rightRearJoint>base_to_wheel4</rightRearJoint>
    <wheelSeparation>4</wheelSeparation>
    <wheelDiameter>0.1</wheelDiameter>
    <robotBaseFrame>base_link</robotBaseFrame>
    <torque>1</torque>
    <topicName>cmd_vel</topicName>
    <broadcastTF>0</broadcastTF>
  </plugin>
</gazebo>

你在代码中能够看到的参数都是一些简单的配置，以便这个控制器能够支持4个*的机器人工作。例如，我们选择 base_to_wheel1、base_to_wheel2、base_to_wheel3、base_to_wheel4 关节作为机器人的驱动轮。另外一个有趣的参数是 topicName。我们需要以这个参数为名发布命令来控制机器人。正确配置*关节的方向非常重要，如以下代码中的 base link 和 wheel1 关节：

<joint name="base_to_wheel1" type="continuous">
   <parent link="base_link"/>
   <child link="wheel_1"/>
   <origin rpy="-1.5707 0 0" xyz="0.1 0.15 0"/>
   <axis xyz="0 0 1" />
</joint>

现在我们使用以下命令启动带有控制器、机器人和地图的模型：

$ roslaunch robot1_gazebo gazebo_wg.launch

你将会在 gazebo 屏幕中看到机器人在地图上。现在我们将使用键盘来移动地图中的机器人。这个节点在 teleop_twist_keyboard 功能包中，发布 /cmd_vel 主题。运行以下命令以按照此功能包：

$ sudo apt-get install ros-indigo-teleop-twist-keyboard
$ rosstack profile
$ rospack profile

然后，运行节点如下:

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

现在你会看到一个有很多说明的新命令行窗口，你可以使用（u，i，o，j，k，l，m，“,”，“.”）按键来移动机器人并设置最大速度。（目前已知u键是加速，j键是左转，k键是减速，l键是右转）

在URDF文件中如何对link(即零件)设置视图模型、碰撞模型和惯性模型？

答：从GRobot.gazebo.urdf文件中的代码查看设置方法。

如何使用URDF描述一个机器人，并通过工具rviz来查看机器人的模型?

        URDF不仅提供了球、圆柱、长方体等基本的形状外，还可以加载mesh文件来展示复杂的3D模型。
        准备工作是已经安装好’joint_state_publisher’包。如果我们使用apt-get来安装完整版的ROS系统或者urdf_tutorial，那么就应该已经安装了。我们可以用rosdep工具来检查相关的依赖是否都已经安装了。

步骤1：使用简单的几何体创建一个可视化机器人模型
提到的所有的机器人模型都可以在’urdf_tutorial’包中找到。
首先，研究一个简单的形状，下面是一个描述这一形状的urdf文件内容，它是一个XML文档：

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>
</robot>

这个XML文档描述了一个叫做"myfirst"的机器人，它只有一个link(或者说是部分)，它的visual部分只是描述了一个长0.6米，直径为0.2米的圆柱。我们可以进入到urdf_tutorial包中，以用display.launch来查看一下这个模型：

$ roscd urdf_tutorial
$ roslaunch urdf_tutorial display.launch model:=urdf/01-myfirst.urdf

这个display.launch文件就在urdf_tutorial下的launch子目录中，其内容如下：

<launch>
    <arg name="model" />
    <arg name="gui" default="False" />
    <param name="robot_description" textfile="$(arg model)" />
    <param name="use_gui" value="$(arg gui)"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
</launch>

上述代码完成了3件事：

• 把urdf文件中描述的模型导入参数服务器。
• 运行节点发布sensor_msgs/JointSate。(运行了两个节点joint_state_publisher和robot_state_publisher，以后将会细讲)
• 运行一个有配置文件的Rviz。(Starts Rviz with a configuration file)
  这将打开一个RVIZ的窗口软件，其中显示了一个红色的圆柱体。
  
  在图中我们主要关注两个对象：Grid和RobotModel。Grid是一个用来描述世界坐标系的网格，它就位于世界坐标系的原点处，其offset为(0;0;0)。 在RobotModel中有一个Link，也是位于世界坐标系的原点处，其Position为(0;0;0)。实际上我们在URDF文档中没有指定圆柱体的位置，所以rviz就默认将其渲染到了坐标系的原点上。
  其次，研究一个稍微复杂的多个形状模型，现在我们创建两个几何体，这两个几何体之间存在一定的位置关系，所以我们除了在URDF文件中添加一个Link来描述第二个几何体之外，还需要添加一个Joint来描述两个几何体之间的连接关系。 下面是新的URDF文档内容：

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
    </visual>
  </link>
  
  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
  </joint>
</robot>

在这个URDF文档中，我们新增加了一个长宽高分别为0.6, 0.1和0.2米的立方体。通过一个固定(fixed)的Joint把两个Link连接在一起。Joint有几种不同的形式，fixed是其中不能转动或者移动的一种。Joint连接两个Link，他们之间是一种树型的结构，具有父子关系。这里的例子是说零件right_leg通过关节base_to_right_leg固连到基座零件base_link上。这种父子关系还说明子Link的位置和姿态是基于父Link的。 运行如下的指令，我们可以在打开的RVIZ中看到这两个几何体。

$ roscd urdf_tutorial
$ roslaunch urdf_tutorial display.launch model:=urdf/02-multipleshapes.urdf

显示的图像中，他们是重叠在一起的，这是因为我们只是定义了两个几何体的形状和大小， Joint的添加也只是说明了两者具有连接关系，我们都还没有描述他们在空间中的位置和关系。
下面我们在"base_to_right_leg"的描述项中添加一个origin的标签。这个origin标签有两个属性xyz和rpy，它们分别描述了子Link坐标系相对于父Link坐标系的位置和姿态关系。 xyz就是我们熟悉的笛卡尔坐标系下空间点的坐标描述，而rpy则是三个欧拉角——滚转角(roll)、俯仰角(pitch)和偏航角(yaw)。 它们表示从父Link的坐标系先后绕z,y,x轴转动yaw,pitch,roll角度就可以得到一个与子Link坐标系平行的坐标系。

<joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -1.22 0.25" rpy="0 0 0"/>
</joint>

这里xyz="0 -1.22 0.25"表示子Link相对于父Link右移了1.22米，上移了0.25米；rpy="0 0 0"则说明两者之间没有相对转动。通过urdf_tutorial中的display.launch我们可以查看两个Link在空间中位置发生了变化， 如下图所示(为了方便显示我们修改了"right_leg"的尺寸为长宽高分别是0.3,0.1和0.2米)。图中的长方体在圆柱体的左上方，其中有红、绿、蓝三种颜色的线段，它们实际上是长方体的坐标系，红色对应x坐标轴， 绿色和蓝色则分别对应y轴和z轴。

我们还可以为每个Link添加坐标描述，比如下面关于right_leg的描述，我们在visual标签下添加一个子标签origin，它描述了视图模型相对于此几何体原点的相对位置和姿态关系。

<link name="right_leg">
    <visual>
      <geometry><box size="0.3 0.1 0.2"/></geometry>
      <origin xyz="-0.15 0 0" rpy="0 0 0"/>
    </visual>
</link>

下面是不同xyz和rpy下的视图效果

步骤2：给机器人外表上颜色
在前面的例子中，所有的物体都被渲染成了红色，这是应为我们没有说明物体的颜色。下面我们通过material标签来为每个几何体添加颜色说明。

<?xml version="1.0"?>
<robot name="origins">
  <material name="blue"><color rgba="0 0 0.8 1"/></material>
  <material name="white"><color rgba="1 1 1 1"/></material>
        
  <link name="base_link">
    <visual>
      <geometry><cylinder length="0.6" radius="0.2"/></geometry>
      <material name="blue"/>
    </visual>
  </link>
        
  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.3 0.1 0.2"/>
        <origin xyz="-0.15 0 0" rpy="0 0 0"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
        
  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -1.22 0.25" rpy="0 0 0"/>
  </joint>
</robot>

在这个URDF文档中，我们先定义了两个material标签，其中只描述了材料的颜色，有需要的话我们还可以定义一些其它关于材料的属性。在每个link中我们也添加了material标签， base_link采用blue的材料，而right_leg则采用white的材料。我们可以看到URDF中显示的3D图形：

步骤3：机器人上添加更加复杂的形状
URDF不仅可以定义和描述简单的几何体，它还可以通过一些mesh文件来描述复杂的形状。比如我们查看urdf_tutorial下的05-visual.urdf模型，既可以看到一个完整的R2D2机器人， 它的手抓部分就是用mesh文件描述的复杂结构。

$ roscd urdf_tutorial
$ roslaunch urdf_tutorial display.launch model:=urdf/05-visual.urdf

我们需要在mesh标签中给出mesh文件的路径，比如说这里我们借用了PR2机器人的手抓。

<link name="left_gripper">
    <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry><mesh filename="package://pr2_description/meshes/l_finger.dae"/></geometry>
    </visual>
</link>

mesh文件可以有几种不同的格式。STL是一种比较常用的格式，RVIZ的渲染引擎也支持dae格式。dae是一种可以描述颜色的格式，这样我们就不用在针对它声明一些关于颜色的材料特性了。
至此，前3步我们只关注了如何描述机器人的视图模型，也就是URDF中的visual标签。已经了解了URDF的基本功能，使用RVIZ工具简单的查看了机器人模型。URDF文件本质上就只是一个简单的xml文档，它用link表示机器人中的各个部分，用joint表示各个部分之间的连接关系。joint连接的两个link具有父子关系，整个机器人可以用一棵树来描述。 各个link之间存在一些相对的位置和姿态关系，可以通过在link或者joint中加入origin标签来描述。
步骤4：赋予机器人运动的能力
之前的机器人模型仅仅是一个视图模型，虽然也描述了机器人的Link和Joint，但Joint是固定的(fixed)，整个机器人是不能动的。但另外三种Joint，它们可以赋予机器人运动的能力。
        现在，我们来查看一个具有运动功能的机器人。仍然通过urdf_tutorial中的display.launch工具来查看模型06-flexible.urdf。我们可以将之与05-visual.urdf对比， 查看两者之间的不同。这里我们只关注其中的三个关节：head_swivel, left_gripper_joint, gripper_extension。

$ roscd urdf_tutorial
$ roslaunch urdf_tutorial display.launch model:=urdf/06-flexible.urdf gui:=true

在运行display.launch的时候，我们增加了一个gui的参数，它就是用来打开一个GUI工具栏。拖动工具栏中的各个关节的滑动条，我们可以看到对应关节发生转动。

接下来，让我们来看一下关节"head_swivel"，它的描述如下：

head_swivel是连接头和躯干的关节，其类型被定义为continuous，这意味着它的值可以从负无穷变化到正无穷，是一种旋转的关节。一般*也会定义称这种关节， 这样它就可以正向或者反向无限的转动下去。但GUI工具栏不能描述[−∞,+∞]的区间，它只是在[−π,π]转动。
        在描述关节转动时，我们还需要指定一个转轴，它是通过axis标签来描述的。axis描述的转轴的方向向量，这里R2D2的脑袋是绕着Z轴旋转的，所以我们定义xyz=“0 0 1”。 因为通常我们对一个*的期望就是一直转，所以一般不用给continuous类型的关节添加什么约束。
接下来，让我们来看一下关节"left_gripper_joint"，left_gripper_joint也是一种旋转的关节，它被定义为revolute。revolute与continuous相比就是一定要有一个约束项，它通过limit标签来描述。 这里的约束是指该关节可以转动的位置最小为0弧度，最大为0.548弧度，最大转速为0.5弧度每秒，作用力小于1000。它的描述如下：

接下来，让我们来看一下关节"grippe_extension"，grippe_extension则是一种直线运动的关节，其类型为prismatic。与revolute和continuous不同的是，prismatic并不是绕着指定轴转动，它是沿着指定轴进行直线的移动。 对其约束的描述也是用limit标签来实现的，但它的量纲为米。它的描述如下：

        综上，通过这三种关节的形式，我们可以描述任何类型的机械运动结构。但ROS还提供了一些其它类型的Joint，比如一个plannar关节可以在一个平面上移动，一个floating的关节则没有什么约束，可以在空间中随意移动。由于这些关节的状态都不能够通过一个变量来表示，而且理论上它们可以通过上述三种类型的关节组合得到，所以我们在这里不对它们做更多的介绍。

我们在移动GUI的滑动条的时候，Rviz中模型会跟着移动，这是怎么实现的呢？首先GUI的程序中有一个解析器用来解析URDF，并找出其中所有非固定的关节以及它们的约束。 接着，GUI发布一个sensor_msgs/JointState的消息。这个消息将被robot_state_publisher节点用于计算机器人各个部分的坐标变换关系。变换后的结果就被Rviz用来更新模型的显示。
步骤5：为机器人添加碰撞模型
到目前为止，我们只是描述了机器人模型的视图模型。但使用Gazebo做仿真的时候，我们还需要用到碰撞模型。下面是一个添加了碰撞模型描述的base_link零件:

碰撞模型是用collision标签来描述的，它是link标签下与visual同级的一个标签。在collision中，与visual一样建立了一个几何体来描述机器人的形状和尺寸。 实际上这个几何形状可以不与visual中的一样，我们完全可以建一个长方体，只要仿真的时候合理就可以了。同样我们也可以添加坐标系的描述。这些都是根据实际需要，仿真效果、计算量综合考量后的结果。
步骤6：为机器人添加惯性模型(包含物理属性)
到目前为止，除了视图模型和碰撞模型之外，使用Gazebo做仿真的时候，我们还需要用到惯性模型。该模型能够描述一些物理属性，下面是一个描述机器人质量的模型：

<inertial>
    <mass value="10"/>
    <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
</inertial>

惯性模型使用一个inertial标签，与碰撞模型和视图模型是一个级的。其中的mass描述了质量，inertia则描述了惯量矩阵。 由于惯量矩阵是一个对称矩阵所以这里只用了6个变量来描述,即下图所示。

此外，我们还可以在碰撞模型collision标签下添加一些子标签来描述表面的摩擦系数、阻尼系数、刚度等表面的物理特性。对于关节我们还可以添加摩擦和阻尼两个属性。 关于这些物理特性，我们在以后关于仿真的教程中如果用到的话再做详细的介绍。
步骤7：学会在程序中访问已有的机器人模型，用于实际的机器人控制当中？
        使用URDF已经描述好的机器人模型，要在程序中使用该模型，我们还需要能够从描述文件中解析出机器人的各种参数。
        我们在进行机器人软件的开发的时候会频繁的查询机器人模型，来完成预期的功能。
下面举例说明一个机器人模型的访问：
首先我们在工作空间下创建一个"learning_urdf"的package:

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_urdf rospy roscpp urdf
$ cd learning_urdf

在新建的package的目录中，创建一个urdf的子目录用于保存教程中需要用到的urdf文件。我们可以把GRobot从这里下载下来，保存到这个子目录中。

$ mkdir urdf
$ cd urdf
$ wget http://gaoyichao.com/Xiaotu//ros/src/GRobot.urdf
$ cd ..
$ mkdir src
$ cd src

然后创建一个src子目录用于保存用到的cpp源代码，把下面的示例代码保存到这个子目录中并命名为parser.cpp。这段代码作用是从指定的urdf文件中解析出了机器人的模型，并在标准输出中打印了一些关于机器人的信息。 我们会在本文的后续内容中详细介绍urdf相关的API。

#include <urdf/model.h>
#include <ros/ros.h>
#include <iostream>
        
int main(int argc, char *argv[]) {
    ros::init(argc, argv, "parser");
        
    if (2 != argc) {
        ROS_ERROR("Need a urdf file as argument");
        return -1;
    }
    std::string urdf_file_name = argv[1];
        
    urdf::Model model;
    if (!model.initFile(urdf_file_name)) {
        ROS_ERROR("Failed to parse urdf file");
        return -1;
    }
        
    std::cout << "Successfully parsed urdf file" << std::endl;
    std::cout << "机器人名称: " << model.getName() << std::endl;
    std::cout << "根节点: " << model.getRoot()->name << std::endl;
    std::cout << "根节点下有 " << model.getRoot()->child_joints.size() << "个关节" << std::endl;
    std::cout << "分别是:" << std::endl;
    for (int i = 0; i < model.getRoot()->child_joints.size(); i++)
        std::cout << model.getRoot()->child_joints[i]->name << std::endl;
       
    return 0;
}

接下来修改CMakeLists.txt文件，添加对例程的编译规则:
add_executable(parser src/parser.cpp)
target_link_libraries(parser ${catkin_LIBRARIES})
切换到工作空间的根目录下，编译运行就可以看到如下的结果，说明我们已经可以通过程序访问机器人模型了。

$ cd ~/catkin_ws
$ catkin_make
$ source ./devel/sertup.bash
$ roscd learning_urdf
$ rosrun learning_urdf parser urdf/GRobot.urdf

输出结果：

下面对parser.cpp文中代码进行讲解，以理解机器人模型是如何被访问的。
        parser.cpp中，我们包含了头文件"urdf/model.h"，并在main函数中构建了一个urdf::Model的对象，已达到能够访问urdf接口的作用。
        从model.h中知道ModelInterface中派生出了urdf::Model这个类，urdf::Model实现了各种对象的初始化函数。下面列出了各个初始化函数。

namespace urdf{
    class Model: public ModelInterface {
    public:
        /// \brief Load Model from TiXMLElement
        bool initXml(TiXmlElement *xml);
        /// \brief Load Model from TiXMLDocument
        bool initXml(TiXmlDocument *xml);
        /// \brief Load Model given a filename
        bool initFile(const std::string& filename);
        /// \brief Load Model given the name of a parameter on the parameter server
        bool initParam(const std::string& param);
        /// \brief Load Model given the name of a parameter on the parameter server using provided nodehandle
        bool initParamWithNodeHandle(const std::string& param, const ros::NodeHandle& nh = ros::NodeHandle());
        /// \brief Load Model from a XML-string
        bool initString(const std::string& xmlstring);
    };
    // shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for std::shared_ptrs in latest version
}

        从parser.cpp中可看出，我们通过initFile()进行初始化，这个函数首先从指定的文件中把urdf文件内容读到一个字符串std::String对象中，然后调用initString()完成机器人模型的创建。其它的函数都是类似的操作。
        ModelInterface还挺供了很多函数，用来访问机器人模型的数据和信息。例如，
getRoot()函数可以获得机器人模型的根节点：

LinkConstSharedPtr getRoot(void) const;

通过名称访问机器人的特定Link或者Joint：

LinkConstSharedPtr getLink(const std::string& name) const;
JointConstSharedPtr getJoint(const std::string& name) const;

获知URDF文件中定义的材料(Material):

MaterialSharedPtr getMaterial(const std::string& name) const;

获取Link、Joint和Material的另外一种方式是使用指针，因为它们在ModelInterface中都是由public的std::map容器保存的，我们也可以直接访问这些容器：

std::map<std::string, LinkSharedPtr> links_;
std::map<std::string, JointSharedPtr> joints_;
std::map<std::string, MaterialSharedPtr> materials_;

其中LinkSharedPtr, JointSharedPtr, MaterialSharedPtr分别是指向Link, Joint和Material对象的智能指针。
        综上而言，包含了头文件"urdf/model.h"并创建了urdf::Model对象，就可以通过该对象获得关于机器人模型的所有数据。urdf::Model只是定义了一些初始化对象的函数，它派生自ModelInterface。ModelInterface则提供了访问机器人模型的各种接口，我们可以通过get函数来获取指定的Link, Joint和Material，也可以通过智能指针来访问。
步骤8：让机器人在rviz中舞动起来
        准备工作，假定在工作空间下有一个叫做learning_urdf的package，其中有一个urdf的子目录，里面保存了一个叫做GRobot.urdf的机器人文件。 这个urdf文件描述了一个有23个*度的双足机器人。我们使用urdf_tutorial中的display.launch来查看一下这个模型：

$ roscd learning_urdf
$ roslaunch urdf_tutorial display.launch model:=urdf/GRobot.urdf gui:=true

        右下角中的joint_state_publisher工具栏中，对应每个*度都有一个滑块。拖动滑块可以控制对应的关节转动，如果我们写对应类似控制滑块的代码，那么就可以实际控制它运动了。 所以我们要先研究一下display.launch都做了一些什么工作，然后实现一个我们自己的控制器。
display.launch文件就在urdf_tutorial下的launch子目录中，其内容如下：

<launch>
    <arg name="model" />
    <arg name="gui" default="False" />
    <param name="robot_description" textfile="$(arg model)" />
    <param name="use_gui" value="$(arg gui)"/>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
</launch>

第2,3行声明了两个参数model和gui，我们在运行该launch文件的时候通过这两个参数来指定机器人模型，和控制是否打开工具栏(默认是不打开工具栏)。我们在启动launch时，传入的参数分别是model:=urdf/GRobot.urdf 、gui:=true。
第4行中通过textfile将文件内容加载到参数服务器中的"robot_description"字段下。
第5行中，把参数gui:=true保存到参数服务器的"use_gui"字段下，它用于指引joint_state_publisher节点是否创建一个GUI窗口。
第6行中运行了一个叫做"joint_state_publisher"的节点， 它是一个python的程序。 这个程序发布了一个叫做"joint_states"的主题，其数据类型是"sensor_msgs/JointState"。这是一个描述机器人各个关节状态的主题。
第7行中运行的"robot_state_publisher"节点，订阅了第6行节点发布的"joint_states"主题，时刻接收关节的数据信息，"robot_state_publisher"就是一个tf广播器， 它是一个C++的程序，时刻计算各个坐标系之间的变换关系，并将之广播出去。

第8行中运行的rviz则是一个tf监听器，它接受tf的变换广播，并更新视图。

综上，通过对display.launch的分析，我们知道从拖动工具栏中的滑块，到rviz中机器人的模型发生运动，中间经过了如下的几个过程：

1. 节点joint_state_publisher发布一条sensor_msgs/JointState的消息到主题joint_states;
2. 节点robot_state_publisher接收到主题joint_states的消息，根据消息内容更新GRobot各个坐标系状态，并通过tf广播器机制发布;
3. rviz监听到GRobot坐标系发生变化，更新视图显示。

可以说，这一系列动作都是由主题"joint_states"触发的。我们可以通过rostopic和rosmsg工具查看主题对应消息内容的格式：

可以看到主题的发布者只有joint_state_publisher，订阅者是robot_state_publisher。 消息类型是sensor_msgs/JointState，它由header, name, position, velocity和effort五个部分构成。 其中header是一个复合的结构，它的stamp字段就是一个时间戳，对于本例而言是重要的。剩下的四个字段都是数组，在C++语言中都是以std::vector的形式实现的，所以我们可以直接roscpp的接口中获取数组长度。name是关节(joint)的名称;position是关节的位置消息，量纲为弧度(rad)或者米(m);velocity是关节的速度信息，量纲为rad/s或者m/s;effor是关节的力矩或者力信息，量纲为Nm或者N。 它们都是一一对应的，也就是说name[i], position[i]描述的是同一个关节的名字和位置信息。
下面，我们自己写一个joint_state_listener节点，来监听该主题并获取关节数据查看一下。

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <iostream>
        
void callback(const sensor_msgs::JointState::ConstPtr &msg) {
    std::cout << "===========================================================================" << std::endl;
    std::cout << msg->name.size() << "\t" << msg->position.size() << "\t";
    std::cout << msg->velocity.size() << "\t" << msg->effort.size() << std::endl;
    int n = msg->name.size();
    for (int i = 0; i < n; i++) {
        std::cout << msg->name[i] << ":" << msg->position[i] << std::endl;
    }
}
        
int main(int argc, char *argv[]) {
    ros::init(argc, argv, "joint_state_listener");
    ros::NodeHandle n;
        
    ros::Subscriber sub = n.subscribe("joint_states", 1000, callback);
    ros::spin();
}

在该joint_state_listener中，我们订阅了"joint_states"并在接收到消息后的回调函数中把消息的数据内容都打印出来了，运行效果如下：

在打印的数据中，第一行依次列出了接收消息中name, position, velocity, effort的元素数量，其中name和position有23个，其余为0。这23正好对应着我们机器人的23个*度， 没有速度和力的信息。其余各行中的数据表示对应关节的位置都为0，如果拖动了工具栏我们会看到对应行也会发生变化。
这就说明我们只需要发布一个joint_states的主题，通过sensor_msgs/JointState列出各个关节的位置、速度、力的信息就可以通过ROS的工具控制一台机器人。
下面我们写一个joint_state_publisher来控制GRobot在rviz中不停地扭动：
joint_state_publisher的完整代码可以在这里下载。首先包含必要的头文件，ros/ros.h提供了访问ROS系统内核的接口，sensor_msgs/JointState.h则是我们要发布的joint_states主题的消息类型描述，urdf/model.h提供了访问urdf模型的接口，iostream则是C++中的标准输入输出流，程序中可能用到的std::cout就是通过该头文件访问的。

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <urdf/model.h>
#include <iostream>

在main函数中，我们先完成ros节点的初始化并注册发布"joint_states"的主题。

int main(int argc, char *argv[]) {
ros::init(argc, argv, "joint_state_publisher");
ros::NodeHandle n;
        
ros::Publisher pub = n.advertise("joint_states", 1000);
ros::Rate loop_rate(10);

接着，我们从参数服务器中获取GRobot的模型，并创建urdf模型对象。

std::string urdf_file_name;
    urdf::Model model;
    if (n.getParam("robot_description", urdf_file_name)) {
        std::cout << "urdf_file_name:" << urdf_file_name << std::endl;
        if (!model.initString(urdf_file_name)) {
            ROS_ERROR("Failed to parse urdf file");
            return -1;
        }
}

        从urdf模型中把不是固定连接的关节找出来，因为我们在建立GRobot的时候只用了Fixed和Continuous两种类型的Joint，所以这里不再做更细的区分。

std::vector joints;
for (auto it = model.joints_.begin(); it != model.joints_.end(); it++) {
    urdf::JointSharedPtr joint = it->second;
    if (urdf::Joint::FIXED != joint->type) {
        joints.push_back(joint);
    }
}

        在一个while循环中不断的发布消息，这里和官方教程一样不考虑速度和力的因素，只是单纯的控制所有的关节转动相同的角度。

    double position = 0.0;
    while (ros::ok()) {
        sensor_msgs::JointState msg;
        msg.header.stamp = ros::Time::now();
        for (auto it = joints.begin(); it != joints.end(); it++) {
            msg.name.push_back((*it)->name);
            msg.position.push_back(position);
        }
        position = (position > 6.28) ? 0 : position + 0.01;
        
        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

        这个程序依赖于urdf和sensor_msgs两个包，所以我们需要修改learning_urdf的package.xml文件，添加相应的依赖项:

<build_depend>urdf</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>urdf</run_depend>
<run_depend>sensor_msgs</run_depend>

并在CMakeLists.txt中添加编译规则。

find_package(catkin REQUIRED COMPONENTS roscpp rospy urdf sensor_msgs)
...
add_executable(joint_state_publisher src/joint_state_publisher.cpp)
target_link_libraries(joint_state_publisher ${catkin_LIBRARIES})

然后回到工作空间根目录下进行编译:

$ cd ~/catkin_ws/
$ catkin_make
$ source ./devel/setup.bash
$ roscd learning_urdf

我们从urdf_tutorial中把display.launch拷到learning_urdf的launch目录下，做如下修改，用我们新建的joint_state_publisher替换官方例程的:

<launch>
    <arg name="model" />
    <param name="robot_description" textfile="$(arg model)" />
    <node name="joint_state_publisher" pkg="learning_urdf" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
</launch>

然后运行该launch文件，就可以看到GRobot在rviz中的魔鬼身姿，不停地扭动…

$ roslaunch learning_urdf display.launch model:=urdf/GRobot.urdf