ROS学习笔记十二：使用gazebo在ROS中仿真

想要在ROS系统中对我们的机器人进行仿真，需要使用gazebo。

gazebo是一种适用于复杂室内多机器人和室外环境的仿真环境。它能够在三维环境中对多个机器人、传感器及物体进行仿真，产生实际传感器反馈和物体之间的物理响应。

在本文中，你会学习如何使用之前创建的机器人模型，如何加载一个激光雷达传感器和摄像头，并使机器人模型像真的机器人一样移动。

一、在 gazebo 中使用 URDF 3D 模型

在 gazebo 工作前，我们需要安装ROS功能包与 gazebo 交互：

$ sudo apt-get install ros-indigo-gazebo-ros-pkgs ros-indigo-ros-control

为了在 gazebo 中导入机器人模型，你需要先完成 URDF 模型。在这里我们将使用.xacro文件，虽然这可能更复杂，但是对于代码开发来说其功能非常强大。你能够在 chapter7_tutorials/robot1_description/urdf/robot1_base_01.xacro 找到修改后的文件：（由于代码过多，所以并未完全贴出，下面只是很少的一部分代码）

<link name="base_link">
	<visual>
		<geometry>
			<box size="0.2 .3 .1"/>
		</geometry>
		<origin rpy="0 0 1.54" xyz="0 0 0.05"/>
		<material name="white">
			<color rgba="1 1 1 1"/>
		</material>
	</visual>
	<collision>
		<geometry>
			<box size="0.2 .3 0.1"/>
		</geometry>
	</collision>
	<xacro:default_inertial mass="10"/>
</link>

这是机器人底盘 base_link 的新代码。请注意 collision 和 inertial 部分对于在 gazebo 中运行模型是必需的，这样才能计算机器人的物理响应。

想要启动这一切，我们创建一个名为 gazebo.launch 的启动文件到 chapter7_tutorials/robot1_gazebo/launch 文件夹下，并添加以下代码：

<?xml version="1.0"?>
<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="true"/>
  <arg name="use_sim_time" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="true"/>

  <!-- 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 robot1_gazebo)/worlds/robot.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <arg name="model" />
  <param name="robot_description"
	 command="$(find xacro)/xacro.py $(arg model)" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	args="-urdf -model robot1 -param robot_description -z 0.05"/> 

</launch>

想要启动文件，需要使用以下命令：

$ roslaunch robot1_gazebo gazebo.launch model:="`rospack find robot1_description`/urdf/robot1_base_01.xacro"

然后可以在 gazebo 中看到机器人。仿真初始状态是暂停的，你可以点击 paly 来运行它。（不过机器人不会移动）

正如你所见，模型并没有任何纹理渲染。在rviz中你能看到在URDF文件中声明的纹理。但是在 gazebo 中，你看不到它们。为了在 gazebo 中添加可见的纹理，需要在你的 .gazebo 模型文件中创建robot.gazebo文件。将 robot1_description/urdf/robot1_base_01.xacro 文件另存为 robot1_base_02.xacro，并添加如下代码，即可创建robot.gazebo文件：

<xacro:include filename="$(find robot1_description)/urdf/robot.gazebo" />

启动这个新文件，你看到的机器人虽然还是同一个，但已经带有纹理了：

$ roslaunch robot1_gazebo gazebo.launch model:="`rospack find robot1_description`/urdf/robot1_base_02.xacro"

你将会看到以下输出：

二、在 gazebo 中添加传感器

在gazebo中，你能够对机器人的物理运动进行仿真。你同样能仿真它的传感器。

在本节中我们将会向模型中添加一个摄像头和激光雷达传感器。

我们会从 gazebo_ros_demos 功能包中调用激光雷达。这是ROS的神奇之处，你能从其他功能包中复用代码从而简化开发。

我们唯一需要做的是向 robot1_base_02.xacro 文件中增加这些行，来为机器人添加 Hokuyo 激光雷达3D模型，并另存为 robot1_base_03.xacro 文件：

  <!-- 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>

在 robot.gazebo 文件里，我们将添加 libgazebo_ros_laser 插件，这样就可以模拟 Hokuyo 激光测距雷达的行为。

  <!-- 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>

使用以下命令启动新的模型：

$ roslaunch robot1_gazebo gazebo.launch model:="`rospack find robot1_description`/urdf/robot1_base_03.xacro"

你将会看到附带激光雷达模块的机器人。

采用同样的方法，我们向 robot.gazebo 和 robot1_base_03.xacro 添加几行代码以增加另一个传感器（一个摄像头）。

再使用以下命令启动添加摄像头后的模型：

$ roslaunch robot1_gazebo gazebo.launch model:="`rospack find robot1_description`/urdf/robot1_base_04.xacro"

注意，这个激光雷达会像真实的雷达一样产生“真实"的传感器数据。你能够通过 rostopic echo 命令看到这些数据：

$ rostopic echo /robot/laser/scan

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

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

下面是点击"play"允许后摄像头拍摄的图像。

三、在 gazebo 中加载和使用地图

在本小节中我们将会使用一张柳树车库公司（Willow Garage）办公室的地图、这张地图在我们的ROS软件中应该默认安装了，保存在 gazebo_worlds 功能包中。

为了检查模型，可以使用以下命令启动 .launch 文件：

$ roslaunch gazebo_ros willowgarage_world.launch

你会在 gazebo 中看到3D的办公室。这个办公室只有墙壁，你可以添加桌子、椅子和其他你想添加的物体。

我们现在要做的是创建一个新的 .launch 文件来同时加载地图和机器人。为实现这个功能，我们在 robot1_gazebo/launch 文件夹下创建一个名为 gazebo_wg.launch 的文件，并添加以下代码：

<?xml version="1.0"?>
<launch>
  <include file="$(find gazebo_ros)/launch/willowgarage_world.launch">
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
         command="$(find xacro)/xacro.py '$(find robot1_description)/urdf/robot1_base_04.xacro'" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model robot1 -param robot_description -z 0.05"/>

</launch>

现在运行带有机器人的地图模型文件（注意：有些时候会出现软件崩溃，多尝试运行几次）：

$ roslaunch robot1_gazebo gazebo_wg.launch

你会在 gazebo 中看到机器人和地图。下一步是在仿真虚拟环境中命令机器人移动和接受其传感器的仿真读数。

四、在 gazebo 中移动机器人

滑移转向（skid-steer）机器人是一种对机身两侧轮子分别进行驱动的移动机器人。它通过将两侧轮子控制在不同的转速（所产生的转速差）进行转向，而不需要轮子有任何转向行动。

正如前面所说，在 gazebo 中我们需要对机器人、关节、传感器等设备的行为进行编程。和前面的激光雷达一样，gazeb 也已经有了 skid 驱动的实现，我们能够直接使用它移动机器人。使用此行驶控制器，只需要在模型文件（robot1_base_04.xacro）中增加以下代码：

  <!-- 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。我们需要以这个参数为名发布命令来控制机器人。此时，当你发布一个 sensor_msgs/Twist 主题调用 /cmd_vel 时，机器人将会移动。正确配置轮子关节的方向非常重要，按照xacro文件中的当前方向，机器人将上下移动，因此有必要修改四个轮子的初始rpy，如以下代码中的 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>

所有修改都保存在 chapter7_tutorials/robot1_description/urdf/robot1_base_04.xacro 文件中。现在我们使用以下命令启动带有控制器、机器人和地图的模型：

$ 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键是右转）

如果一切顺利的话，你可以驾驶机器人穿越 Willow Garage 的办公室。你可以观察到激光雷达数据和摄像头显示的图像。

五、小节

在本文中，你学会了如何创建一个属于自己的3D机器人模型。这包括对添加纹理、创建关节和说明如何使用节点来移动机器人等工作的详细解释。

概况来讲，我们已经看到了如何复用其他机器人的部件来设计我们自己的机器人，特别是我们已经包含了抓取器并添加了传感器，例如一个激光雷达和一个摄像头。

因此，在仿真中不需要完全从无到有来建造一个机器人，社区里已经开发了大量的机器人，你可以下载代码并在 ROS 和 gazebo 中运行它们，如有必要也可以修改它们。