【ROS教程 007】3D建模与仿真

所有开始之前请安装erratic_robot功能包集。我们需要在Gazebo中使用 erratic_robot功能包集中的差分不定式机器人驱动来移动机器人模型。

$ sudo apt-get install ros-fuerte-erratic-robot

没有机会真正接触到一个机器人时，仿真不失为最好的选择。本节我们将学习创建机器人3D模型、为机器人提供运动 物理控制 惯性 和其他物理响应、为机器人3D模型添加仿真传感器、在仿真环境中使用该模型。
（1）自定义机器人在ROS中的3D模型
在ROS中使用机器人3D模型或部分结构模型主要是用于仿真机器人或者是为了帮助开发者简化他们的日常工作，当然具体的方法都是通过URDF文件。标准化机器人描述格式（URDF）是一种用于描述机器人、其部分结构、关节、自由度等的XML格式文件。每次在ROS系统中看到3D的机器人都会有对应的URDF文件与之对应，例如PR2(Willow Garage)或者是Robonaut(NASA)。
（2）创建第一个URDF文件
我们将在下面建立的机器人是一种常见的移动机器人，它有四个轮子、一个带有抓取器的手臂。为了打好基础我们先创建一个带有四个轮子的机器人底座。在chapter5_tutorials/urdf文件夹下创建一个新文件robot1.urdf，内容如下：

<robot name=" Robot1 ">
<link name="base_link">
  <visual>
  <geometry>
  <box size="0.2 .3 .1"/>
  geometry>
<origin rpy="0 0 0" 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>
<inertial>
<mass value="100"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>

  link>

  <link name="wheel_1">
  <visual>
  <geometry>
  <cylinder length="0.05" radius="0.05"/>
  geometry>
<origin rpy="0 1.5 0" xyz="0.1 0.1 0"/>
  <material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<geometry>
  <cylinder length="0.05" radius="0.05"/>
geometry>
collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

  <link name="wheel_2">
  <visual>
  <geometry>
  <cylinder length="0.05" radius="0.05"/>
  geometry>
<origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/>
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="0.05" radius="0.05"/>
geometry>
collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>

  link>

  <link name="wheel_3">
  <visual>
  <geometry>
  <cylinder length="0.05" radius="0.05"/>
  geometry>
<origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/>
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="0.05" radius="0.05"/>
geometry>
collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

  <link name="wheel_4">
  <visual>
  <geometry>
  <cylinder length="0.05" radius="0.05"/>
  geometry>
<origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/>
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="0.05" radius="0.05"/>
geometry>
collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>

  link>

  <joint name="base_to_wheel1" type="fixed">
  <parent link="base_link"/>
  <child link="wheel_1"/>
  <origin xyz="0 0 0"/>
  joint>

  <joint name="base_to_wheel2" type="fixed">
  <parent link="base_link"/>
  <child link="wheel_2"/>
  <origin xyz="0 0 0"/>
  joint>

  <joint name="base_to_wheel3" type="fixed">
  <parent link="base_link"/>
  <child link="wheel_3"/>
  <origin xyz="0 0 0"/>
  joint>

  <joint name="base_to_wheel4" type="fixed">
  <parent link="base_link"/>
  <child link="wheel_4"/>
  <origin xyz="0 0 0"/>
  joint>

<link name="arm_base">
  <visual>
  <geometry>
  <box size="0.1 .1 .1"/>
  geometry>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<material name="white">
<color rgba="1 1 1 1"/>
material>
  visual>
<collision>
<geometry>
  <box size="0.1 .1 .1"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

<joint name="base_to_arm_base" type="continuous">
<parent link="base_link"/>
<child link="arm_base"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0"/>
joint>

<link name="arm_1">
  <visual>
  <geometry>
  <box size="0.05 .05 0.5"/>
  geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<material name="white">
<color rgba="1 1 1 1"/>
material>
  visual>
<collision>
<geometry>
  <box size="0.05 .05 0.5"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

<joint name="arm_1_to_arm_base" type="revolute">
<parent link="arm_base"/>
<child link="arm_1"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.15"/>
<limit effort ="1000.0" lower="-1.0" upper="1.0" velocity="0.5"/>
joint>

<link name="arm_2">
  <visual>
  <geometry>
  <box size="0.05 0.05 0.5"/>
  geometry>
<origin rpy="0 0 0" xyz="0.06 0 0.15"/>
<material name="white">
<color rgba="1 1 1 1"/>
material>
  visual>
<collision>
<geometry>
  <box size="0.05 .05 0.5"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

<joint name="arm_2_to_arm_1" type="revolute">
<parent link="arm_1"/>
<child link="arm_2"/>
<axis xyz="1 0 0"/>
<origin xyz="0.0 0 0.45"/>
<limit effort ="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
joint>

  <joint name="left_gripper_joint" type="revolute">
  <axis xyz="0 0 1"/>
  <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  <origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
  <parent link="arm_2"/>
  <child link="left_gripper"/>
  joint>

  <link name="left_gripper">
  <visual>
  <origin rpy="0 0 0" xyz="0 0 0"/>
  <geometry>
  <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
  geometry>
  visual>
<collision>
<geometry>
  <box size="0.1 .1 .1"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

  <joint name="left_tip_joint" type="fixed">
  <parent link="left_gripper"/>
  <child link="left_tip"/>
  joint>

  <link name="left_tip">
  <visual>
  <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
  <geometry>
  <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
  geometry>
  visual>
<collision>
<geometry>
  <box size="0.1 .1 .1"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
  link>

  <joint name="right_gripper_joint" type="revolute">
  <axis xyz="0 0 -1"/>
  <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  <origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
  <parent link="arm_2"/>
  <child link="right_gripper"/>
  joint>

  <link name="right_gripper">
  <visual>
  <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
  <geometry>
  <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
  geometry>
  visual>
<collision>
<geometry>
  <box size="0.1 .1 .1"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>

  link>

  <joint name="right_tip_joint" type="fixed">
  <parent link="right_gripper"/>
  <child link="right_tip"/>
  joint>

  <link name="right_tip">
  <visual>
  <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
  <geometry>
  <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
  geometry>
  visual>
<collision>
<geometry>
  <box size="0.1 .1 .1"/>
geometry>
collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>

  link>
robot>

在代码中有两种用于描述机器人几何结构的基本字段：连接（link）和关节（joint）。检查我们的配置是否正确可以使用

$ rosrun urdf_parser check_urdf robot1.urdf

cd进入到chapter5_tutorials文件夹下( 注意路径问题) ， 以图形的方式来查看它， 可以看到如下图所示

$ rosrun urdf_parser urdf_to_graphiz "./urdf/robot1.urdf"

这个工具将会在Fuerte之后被弃用，如果使用的版本比较新，应该使用urdfdom功能包。
num1：在rivz里查看3D模型。
现在我们有了自己的机器人，使用rviz查看它的3D展示以及它关节的运行。在launch文件夹下创建display.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>
<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.vcg" />
launch>

cd进入到chapter5_tutorials文件夹下( 注意路径问题) ，使用以下命令启动它

$ roslaunch chapter5_tutorials display.launch model:="./urdf/robot1.urdf"

有时希望自己构建的模型更加真实，通过添加更多的元素使模型变得更加丰富和细致，这就需要加载我们自行创建的图形或者是使用其他机器人模型的图形。
（3）写机器人模型方法xacro
xacro可帮助我们减小URDF文件的尺寸，并增加文件的可读性和可维护性，我们可以方便地使用常量、数学方法、宏等进行编程。 我们可以使用 SketchUp之类的3D建模程序来创建模型。 SketchUp只能在Windows或Mac系统下运行。
（4）在ROS中仿真
要想在ROS系统中对我们的模型进行仿真，需要使用Gazebo。Gazebo是一种适用于机器人和室外环境的仿真环境。它能够在三维环境中对多个机器人、传感器及物体进行仿真，产生实际传感器反馈和物体之间的物理响应。

• num1:在Gazebo中使用URDF3D模型

在urdf/robot1_base_01.xacro找到修改后的文件：

<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
  xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
  xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
name="robot1_xacro">

<xacro:property name="length_wheel" value="0.05" />
<xacro:property name="radius_wheel" value="0.05" />
<xacro:macro name="default_inertial" params="mass">
  <inertial>
  <mass value="${mass}" />
  <inertia ixx="1.0" ixy="0.0" ixz="0.0"
  iyy="1.0" iyz="0.0"
  izz="1.0" />
  inertial>
xacro:macro>

<link name="base_footprint">
  <visual>
<geometry>
  <box size="0.001 0.001 0.001"/>
  geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
  visual>
<xacro:default_inertial mass="0.0001"/>
  link>

<gazebo reference="base_footprint">
<material>Gazebo/Greenmaterial>
<turnGravityOff>falseturnGravityOff>
gazebo>

<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" />
<parent link="base_footprint" />
<child link="base_link" />
joint>

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

  <link name="wheel_1">
  <visual>
  <geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
  geometry>

<origin rpy="0 0 0" xyz="0 0 0"/>
  <material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
geometry>
collision>
<xacro:default_inertial mass="1"/>
  link>

  <link name="wheel_2">
  <visual>
  <geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
  geometry>

<origin rpy="0 0 0" xyz="0 0 0"/>
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
geometry>
collision>
<xacro:default_inertial mass="1"/>

  link>

  <link name="wheel_3">
  <visual>
  <geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
  geometry>


<origin rpy="0 0 0" xyz="0 0 0"/>
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
geometry>
collision>
<xacro:default_inertial mass="1"/>
  link>

  <link name="wheel_4">
  <visual>
  <geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
  geometry>

<origin rpy="0 0 0" xyz="0 0 0" />
  <material name="black"/>
  visual>
<collision>
<geometry>
  <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
geometry>
collision>
<xacro:default_inertial mass="1"/>

  link>

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

 <joint name="base_to_wheel2" type="continuous">
  <axis xyz="0 0 1" />
  <anchor xyz="0 0 0" />
  <limit effort="100" velocity="100" />
  <parent link="base_link"/>
  <child link="wheel_2"/>
  <origin rpy="1.5707 0 0" xyz="-0.1 0.15 0"/>
joint>

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

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

这是机器人底盘base_link的新代码。请注意 collision和 inertial部分对于在Gazebo中运行模型是必需的，这样才能计算机器人的物理响应。
想要启动这一切，我们需要创建一个名为gazebo.launch

<launch>
  

  <param name="/use_sim_time" value="true" />

  
  <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>

  <arg name="model" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >node>
  
  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" >
  <param name="publish_frequency" type="double" value="50.0" />
  node>

  <node name="spawn_robot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.1 -model robot_model" respawn="false" output="screen" />

launch>

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

$ roslaunch chapter5_tutorials gazebo.launch model:="./urdf/robot1_base_01.xacro"

现在在Gazebo中看到机器人，祝贺，已在虚拟世界中迈出第一步！

正如上图所见，模型并没有任何的纹理渲染。在rviz中能看到URDF文件中声明的纹理，但是在Gazebo中却看不到。将robot1_base_01.xacro文件另存为robot1_base_02.xacro并添加以下代码：

 <gazebo reference="base_link">
  <material>Erratic/BlueBrushedAluminummaterial>
 gazebo>

 <gazebo reference="wheel_1">
  <material>Erratic/Blackmaterial>
 gazebo>

 <gazebo reference="wheel_2">
  <material>Erratic/Blackmaterial>
 gazebo>

 <gazebo reference="wheel_3">
  <material>Erratic/Blackmaterial>
 gazebo>

 <gazebo reference="wheel_4">
  <material>Erratic/Blackmaterial>
 gazebo>

启动这个新文件将会看到一个带有纹理的机器人。

• num2:在Gazebo中添加传感器

在 Gazebo中能够对机器人的物理运行进行仿真，同样还能仿真它的传感器，本节我们将会向模型中添加一个激光雷达传感器。这个传感器将会成为机器人你的一个新部件，你要选好它的安装位置。在Gazebo中你会看到一个新的3D模型，它很像Hokuyo激光雷达。我们会从erratic_gazebo_plugins功能包中调用激光雷达。唯一需要做的就是向.xacro文件中增加这些行变为robot1_base_03.xacro：

<include filename="$(find erratic_description)/urdf/erratic_hokuyo_laser.xacro" />

  
  <erratic_hokuyo_laser parent="base_link">
  <origin xyz="0.18 0 0.11" rpy="0 0 0" />
  erratic_hokuyo_laser>

使用以下命令启动新的模型将会看到附带有雷达模块的机器人：

 $ roslaunch chapter5_tutorials gazebo.launch model:="./urdf/robot1_base_03.xacro"

num3：在Gazebo中加载和使用地图
在本小节中我们将会使用一张柳树车库公司办公室地图，这张图在我们的ROS软件中默认安装了。这个3D模型保存在gazebo_words功能包中，为了检查模型，需要使用以下命令启动.launch文件

$ roslaunch gazebo_worlds wg_collada_world.launch

我们需要创建一个新的.launch文件来同时加载地图和机器人。在launch文件夹下创建gazebo_map_robot.launch，内容如下：

<launch>
  

  <param name="/use_sim_time" value="true" />

  
  <include file="$(find gazebo_worlds)/launch/wg_collada_world.launch"/>

  <arg name="model" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >node>
  
  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" >
  <param name="publish_frequency" type="double" value="50.0" />
  node>

  <node name="spawn_robot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.1 -model robot_model" respawn="false" output="screen" />

launch>

现在运行带有激光雷达的模型文件，你会在Gazebo GUI中看到机器人和地图。

$ roslaunch chapter5_tutorials gazebo_map_robot.launch model:="./urdf/robot1_base_03.xacro"

num4: 在Gazebo中移动机器人
差分轮式机器人是一种对机身两侧轮子分别进行驱动的移动机器人。它通过将两侧的轮子控制在不同的转速进行转向，而不需要轮子有任何转向运动。差分驱动通常意味着机器人有2个驱动轮，而我们的机器人有4个轮子，因此在运动中会出现问题，所以要进行一些修改。修改后的文件为robot1_base_04.xacro，内容如下：
现在我们使用以下命令启动带有控制器和地图的模型：
现在我们将使用节点来移动那个机器人，这个节点在erratic_teleop功能包中，运行以下命令

Refrences:
[1]. Aaron Martinez Enrique Fern andez, ROS机器人程序设计[B], P104-125, 2014.
https://answers.ros.org/question/68203/robot_state_publisher-can-find-robot-element/