ROS 2边学边练（44）-- 从头开始构建一个视觉机器人模型

前言

        从此篇开始我们就开始接触URDF(Unified Robot Description Format，统一机器人描述格式)，并利用其语法格式搭建我们自己的机器人模型。

动动手

        开始之前我们需要确认是否安装joint_state_publisher功能包，如果有安装过二进制版本的urdf_tutorial,也是可以的（教程里提到的所有机器人模型都可以在urdf_turial包里面找见，运行示例也是直接调用里面这些），否则需要更新下源并下载安装。

        如果没有安装，参考下面的命令。

安装依赖

$sudo apt install ros-iron-joint-state-publisher-gui ros-iron-joint-state-publisher
$sudo apt install ros-iron-xacro

安装urdf_tutorial 

$sudo apt update
$sudo apt install ros-iron-urdf-tutorial

 注意：还有其他方法可以下载urdf_tutorial功能包源工程，但均有问题。如git clone -b ros2 https://github.com/ros/urdf_tutorial.git,下载不了，再比如git clone https://github.com/ros/urdf_tutorial.git，可以下载，通过浏览器进入查看会发现最近的更新时间都是3年前了，且用的是catkin，最后colcon build --package-select urdf_tutorial会报错，构建不了。

如果大家用的是虚拟机，但是Ubuntu网络没有对应的ipv4，无法ping目标网络，那可能是虚拟机的网络配置问题，可以通过虚拟机->设置->网络适配器（桥接自动），选择桥接+复制物理网络连接状态。 

        关于urdf里面的机器人，一般由关节（joint，起连接作用）和连杆（link，多个link可由joint相衔接）构成，比如一个简单的机械臂，其组成如下。

单个形状

        我们先从单独一个几何形状开始逐步组装成R2D2机器人。[原文件：01-myfirst.urdf]

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

        这段urdf代码定义了一个叫myfirst的机器人，它只包含一个连接base_link，其视觉外观是一个长为0.6米半径为0.2米的圆柱体（cylinder）。是不是一目了然，毫无压力。

        我们来试试启动一个launch文件，看看这个圆柱体实际的效果如何：

$ros2 launch urdf_tutorial display.launch.py model:=urdf/01-myfirst.urdf

上面的语句实际上做了三件事：

• 加载特定的模型文件（01-myfirst.urdf）并且将此文件保存为robot_state_publisher节点的一个参数；
• 运行节点，发布sensor_msgs/msg/JointState数据类型消息，并转换之；
• 开启Rviz（读取配置文件中的参数加载）。

 最终效果如下：

注意点：

• 固定坐标系是网格中心所在的变换坐标系。在这里，它是由我们的一个连接（或称为部分）base_link所定义的坐标系；
• 视觉元素（即圆柱体）的默认原点位于其几何中心。因此，圆柱体的一半位于网格之下。

多个形状

        加大一点难度，我们在上述圆柱体的基础上再增加一个形状部件。在开头也提到了关节（joint），如果要在一个模块上添加另外一个模块，我们必须确定好joint，解析器才能知道第二个模块所放的位置，对于这个joint，大体上有两种，活动的（比如能旋转）和固定的，我们先来个固定的joint。[原文件：02-multipleshapes.urdf]

<?xml version="1.0"?>
<robot name="multipleshapes">
  <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>

        机器人multipleshapes中，第一个link还是我们的圆柱体base_link，第二个link是需要组装到base_link上的right_leg（长方体box，长宽高0.6m*0.1m*0.2m），在最后我们看到有加了个joint，名字为base_to_right_leg，类型是固定（fixed），其父组件为base_link,子组件为right_leg，也即right_leg是通过base_to_right_leg这个joint连接组装到base_link上的。子组件的位置取决于父组件的位置。

$ros2 launch urdf_tutorial display.launch.py model:=urdf/02-multipleshapes.urdf

        base_link和right_leg重叠在一块了，这是由于它们共用了同样的原点（默认），如果不想它们重叠，那我们就需要定义多个原点。

原点

        我们来定义多个原点使得各个组件之间不再穿插。R2D2的腿部连接在其躯干的上半部分，位于侧面。所以我们指定关节的原点就在那里（躯干上半部）。同时，它（关节）并不是连接在腿部的中间，而是连接在腿部的上部，因此我们也必须调整腿部的原点位置。我们还需要旋转腿部，使其竖直站立。[原文件：03-origins.urdf]

<?xml version="1.0"?>
<robot name="origins">
  <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>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

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

</robot>

        我们先从joint属性看起，关节的原点是根据父参考坐标系来定义的。因此，我们在y方向上偏移了-0.22米（即向我们的左边，但相对于坐标轴是向右的），在z方向上偏移了0.25米（即向上）。这意味着无论子连接（child link）的视觉原点标签如何，子连接的原点都会向上并向右偏移。由于我们没有指定rpy（roll pitch yaw）属性，子坐标系将默认与父坐标系具有相同的方向。

        接着再看看right_leg,它的原点既有xyz偏移量也有rpy偏移量。由于我们希望腿部连接在顶部，我们将原点向下偏移，将z偏移量设置为-0.3米（right_leg的原点是相对于joint原点位置作变化，z值偏移-0.3,就能将长方体的顶部尽量接近圆柱体的顶部）。并且，由于我们希望腿部的长部分与z轴平行，我们围绕Y轴旋转视觉部分PI/2（即90度）。

$ros2 launch urdf_tutorial display.launch.py model:=urdf/03-origins.urdf

• 启动文件（launch）在运行包时会基于我们的URDF为每个link生成TF帧，而Rviz会利用这些TF帧信息计算并显示出各个形状体的对应所在。
• 如果给定的URDF link没有对应的TF帧，那么它将被放置在原点位置，并以白色显示（相关问题）。

物质属性

        原文标题为Material Girl，既幽默又如实，但我就翻译的严肃点了，莫怪。机器人上的link上面的效果都是红色，但是如果我们要自定义各个link的颜色（或其他属性）可不可以呢，当然了，我们可以在urdf文件里指定material标签即可。[原文件：04-materials.urdf]

<?xml version="1.0"?>
<robot name="materials">

  <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.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

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

  <link name="left_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="base_to_left_leg" type="fixed">
    <parent link="base_link"/>
    <child link="left_leg"/>
    <origin xyz="0 0.22 0.25"/>
  </joint>

</robot>

        我们在material标签里定义了“blue”(rgba通道)和“white”两种颜色，在link标签里面进行了引用，这会改变该link原先的颜色属性。material标签也可以定义在link属性的内部（其它的link也可以引用），甚至我们还可以使用贴图来渲染我们的机器人。

$ros2 launch urdf_tutorial display.launch.py model:=urdf/04-materials.urdf

补全模型

        最后，我们给机器人加上脚、轮子和头(增加了球体和一些mesh文件，后续我们还会用到)。[原文件：05-visual.urdf]

<?xml version="1.0"?>
<robot name="visual">

  <material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 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.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

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

  <link name="right_base">
    <visual>
      <geometry>
        <box size="0.4 0.1 0.1"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>

  <joint name="right_base_joint" type="fixed">
    <parent link="right_leg"/>
    <child link="right_base"/>
    <origin xyz="0 0 -0.6"/>
  </joint>

  <link name="right_front_wheel">
    <visual>
      <origin rpy="1.57075 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.035"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <joint name="right_front_wheel_joint" type="fixed">
    <parent link="right_base"/>
    <child link="right_front_wheel"/>
    <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
  </joint>

  <link name="right_back_wheel">
    <visual>
      <origin rpy="1.57075 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.035"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <joint name="right_back_wheel_joint" type="fixed">
    <parent link="right_base"/>
    <child link="right_back_wheel"/>
    <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
  </joint>

  <link name="left_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="base_to_left_leg" type="fixed">
    <parent link="base_link"/>
    <child link="left_leg"/>
    <origin xyz="0 0.22 0.25"/>
  </joint>

  <link name="left_base">
    <visual>
      <geometry>
        <box size="0.4 0.1 0.1"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>

  <joint name="left_base_joint" type="fixed">
    <parent link="left_leg"/>
    <child link="left_base"/>
    <origin xyz="0 0 -0.6"/>
  </joint>

  <link name="left_front_wheel">
    <visual>
      <origin rpy="1.57075 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.035"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <joint name="left_front_wheel_joint" type="fixed">
    <parent link="left_base"/>
    <child link="left_front_wheel"/>
    <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
  </joint>

  <link name="left_back_wheel">
    <visual>
      <origin rpy="1.57075 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.035"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <joint name="left_back_wheel_joint" type="fixed">
    <parent link="left_base"/>
    <child link="left_back_wheel"/>
    <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
  </joint>

  <joint name="gripper_extension" type="fixed">
    <parent link="base_link"/>
    <child link="gripper_pole"/>
    <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
  </joint>

  <link name="gripper_pole">
    <visual>
      <geometry>
        <cylinder length="0.2" radius="0.01"/>
      </geometry>
      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
    </visual>
  </link>

  <joint name="left_gripper_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
    <parent link="gripper_pole"/>
    <child link="left_gripper"/>
  </joint>

  <link name="left_gripper">
    <visual>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
      </geometry>
    </visual>
  </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://urdf_tutorial/meshes/l_finger_tip.dae"/>
      </geometry>
    </visual>
  </link>
  <joint name="right_gripper_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
    <parent link="gripper_pole"/>
    <child link="right_gripper"/>
  </joint>

  <link name="right_gripper">
    <visual>
      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
      </geometry>
    </visual>
  </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://urdf_tutorial/meshes/l_finger_tip.dae"/>
      </geometry>
    </visual>
  </link>

  <link name="head">
    <visual>
      <geometry>
        <sphere radius="0.2"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <joint name="head_swivel" type="fixed">
    <parent link="base_link"/>
    <child link="head"/>
    <origin xyz="0 0 0.3"/>
  </joint>

  <link name="box">
    <visual>
      <geometry>
        <box size="0.08 0.08 0.08"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>

  <joint name="tobox" type="fixed">
    <parent link="head"/>
    <child link="box"/>
    <origin xyz="0.1814 0 0.1414"/>
  </joint>
</robot>

$ros2 launch urdf_tutorial display.launch.py model:=urdf/05-visual.urdf

头部（球体）的添加如下：

<link name="head">
  <visual>
    <geometry>
      <sphere radius="0.2"/>
    </geometry>
    <material name="white"/>
  </visual>
</link>

        此教程中的mesh文件（组件模型）来自于PR2机器人，每个模型都有一个单独的mesh文件，我们可以通过指定模型对应的路径（package://NAME_OF_PACKAGE/path）来使用它们。

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

• mesh（网格）可以以多种不同的格式导入。STL格式相当常见，但引擎还支持DAE格式，DAE格式可以包含其自身的颜色数据，这意味着你不需要指定颜色/材质。通常这些是在单独的文件中。这些网格还引用了位于网格文件夹中的.tif文件（注意：这里可能有一个小错误，.tif 文件通常不是用于3D网格的颜色或纹理数据。更常见的是使用如 .png、.jpg 或 .dds 等格式的图片文件作为纹理。可能是这里提到的 .tif 是个特例或者是一个错误。在3D建模和渲染中，.tif 文件不如其他格式常见，但在某些情况下可能被使用。）。
• mesh（网格）也可以使用相对缩放参数或边界框大小来进行尺寸调整。
• 我们也可以在完全不同的包中引用mesh（网格）。

        到此我们的R2D2机器人就组装好了，下一步，我们挑战一下，洒点灵魂，让它动起来。