devbox
花生_TL007
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS2 - URDF_ros2 urdf

作者：花生_TL007 | 2024-06-10 19:58:07

赞

踩

ros2 urdf

系列文章目录

 

前言

URDF（统一机器人描述格式）是一种文件格式，用于在 ROS 中指定机器人的几何形状和组织结构。本教程假定您知道如何编写格式规范的 XML 代码

 

一、从零开始构建可视机器人模型

        在本教程中，我们将建立一个机器人的可视化模型，这个模型看起来有点像 R2D2。在稍后的教程中，你将学习如何衔接模型、添加一些物理属性以及使用 xacro 生成更简洁的代码，但现在，我们将专注于获得正确的视觉几何形状。

        在继续之前，请确保已安装 joint_state_publisher 软件包。如果你安装了 urdf_tutorial 二进制文件，应该已经安装了。如果没有，请更新您的安装以包含该软件包（使用 rosdep 检查）。

本教程中提到的所有机器人模型（以及源文件）都可以在 urdf_tutorial 软件包中找到。

1.1 一个形状

首先，我们只探讨一个简单的形状。下面是你能做出的最简单的 urdf。

  1. <?xml version="1.0"?>
  2. <robot name="myfirst">
  3.   <link name="base_link">
  4.     <visual>
  5.       <geometry>
  6.         <cylinder length="0.6" radius="0.2"/>
  7.       </geometry>
  8.     </visual>
  9.   </link>
  10. </robot>

将 XML 翻译成英文，这是一个名为 myfirst 的机器人，只包含一个 link（又称部件），其视觉组件只是一个长 0.6 米、半径 0.2 米的圆柱体。对于一个简单的 "hello world"（你好世界）类型的示例来说，这可能看起来像很多外层标签，但它会变得更加复杂，相信我。

要检查模型，请启动 display.launch.py 文件：

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

 它有三个功能

  1. 加载指定模型，并将其保存为机器人状态发布（robot_state_publisher）节点的参数。
  2. 运行节点，以发布 sensor_msgs/msg/JointState 和变换（稍后将详细介绍）。
  3. 使用配置文件启动 Rviz

启动 display.launch.py 之后，RViz 将显示如下内容：

da57521d965b48a48b786322acb5a246.png

1.2 多个形状

现在我们来看看如何添加多个形状/部件。如果我们只是在 urdf 中添加更多部件元素，解析器将不知道把它们放在哪里。因此，我们必须添加关节。关节元素既可以指灵活的关节，也可以指不灵活的关节。我们先从不灵活或固定的关节开始。 

  1. <?xml version="1.0"?>
  2. <robot name="multipleshapes">
  3.   <link name="base_link">
  4.     <visual>
  5.       <geometry>
  6.         <cylinder length="0.6" radius="0.2"/>
  7.       </geometry>
  8.     </visual>
  9.   </link>
  10.  
  11.   <link name="right_leg">
  12.     <visual>
  13.       <geometry>
  14.         <box size="0.6 0.1 0.2"/>
  15.       </geometry>
  16.     </visual>
  17.   </link>
  18.  
  19.   <joint name="base_to_right_leg" type="fixed">
  20.     <parent link="base_link"/>
  21.     <child link="right_leg"/>
  22.   </joint>
  23.  
  24. </robot>
  • 注意我们是如何定义一个 0.6 米 x 0.1 米 x 0.2 米的方框的
  • 接合点是以父节点和子节点的形式定义的。URDF 最终是一个只有一个 root link 的树形结构。这意味着腿的位置取决于 base_link 的位置。
ros2 launch urdf_tutorial display.launch.py model:=urdf/02-multipleshapes.urdf

 

b3c0202a1f3446349e8946a1667b3b39.png

1.3 起源

R2D2 的腿连接到躯干上半部分的侧面。因此，我们要在这里指定 JOINT 的原点。此外，它并不是连接到腿的中间，而是连接到上半部分，因此我们也必须偏移腿的原点。我们还可以旋转腿部，使其直立。

  1. <?xml version="1.0"?>
  2. <robot name="origins">
  3.   <link name="base_link">
  4.     <visual>
  5.       <geometry>
  6.         <cylinder length="0.6" radius="0.2"/>
  7.       </geometry>
  8.     </visual>
  9.   </link>
  10.  
  11.   <link name="right_leg">
  12.     <visual>
  13.       <geometry>
  14.         <box size="0.6 0.1 0.2"/>
  15.       </geometry>
  16.       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  17.     </visual>
  18.   </link>
  19.  
  20.   <joint name="base_to_right_leg" type="fixed">
  21.     <parent link="base_link"/>
  22.     <child link="right_leg"/>
  23.     <origin xyz="0 -0.22 0.25"/>
  24.   </joint>
  25.  
  26. </robot>

我们先来看看关节的原点。它是根据母体的坐标系定义的。因此，我们在 y 方向的距离是-0.22 米（在我们的左侧，但相对于坐标轴是在右侧），在 z 方向的距离是 0.25 米（向上）。这意味着，无论子部件的视觉原点标签是什么，子部件的原点都将是向上和向右。由于我们没有指定 rpy（滚动俯仰偏航）属性，子坐标系将默认与父坐标系具有相同的方向。

现在，再看腿部的视觉原点，它有 xyz 和 rpy 偏移量。这定义了视觉元素相对于其原点的中心位置。由于我们希望支腿连接在顶部，因此我们将 Z 偏移量设置为-0.3 米，从而将原点向下偏移。由于我们希望腿的长部分与 Z 轴平行，因此我们将视觉部分绕 Y 轴旋转 PI/2。

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

9dce42db96984ecfb3d4c7f58d115598.png

 启动文件运行的软件包将根据 URDF 为模型中的每个部件创建 TF 坐标系。Rviz 会使用这些信息来确定每个形状的显示位置。

如果给定的 URDF 部件不存在 TF 坐标系，那么它将以白色显示在原点处

1.4 Material Girl 

"好吧，"我听到你说。"这很可爱，但不是每个人都拥有 B21。我的机器人和 R2D2 都不是红色的！" 说得好。让我们来看看材料标签。

  1. <?xml version="1.0"?>
  2. <robot name="materials">
  3.  
  4.   <material name="blue">
  5.     <color rgba="0 0 0.8 1"/>
  6.   </material>
  7.  
  8.   <material name="white">
  9.     <color rgba="1 1 1 1"/>
  10.   </material>
  11.  
  12.   <link name="base_link">
  13.     <visual>
  14.       <geometry>
  15.         <cylinder length="0.6" radius="0.2"/>
  16.       </geometry>
  17.       <material name="blue"/>
  18.     </visual>
  19.   </link>
  20.  
  21.   <link name="right_leg">
  22.     <visual>
  23.       <geometry>
  24.         <box size="0.6 0.1 0.2"/>
  25.       </geometry>
  26.       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  27.       <material name="white"/>
  28.     </visual>
  29.   </link>
  30.  
  31.   <joint name="base_to_right_leg" type="fixed">
  32.     <parent link="base_link"/>
  33.     <child link="right_leg"/>
  34.     <origin xyz="0 -0.22 0.25"/>
  35.   </joint>
  36.  
  37.   <link name="left_leg">
  38.     <visual>
  39.       <geometry>
  40.         <box size="0.6 0.1 0.2"/>
  41.       </geometry>
  42.       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  43.       <material name="white"/>
  44.     </visual>
  45.   </link>
  46.  
  47.   <joint name="base_to_left_leg" type="fixed">
  48.     <parent link="base_link"/>
  49.     <child link="left_leg"/>
  50.     <origin xyz="0 0.22 0.25"/>
  51.   </joint>
  52.  
  53. </robot>
  • 身体现在是蓝色的。我们定义了一个名为 "蓝色 "的新材质，红色、绿色、蓝色和 alpha 通道分别定义为 0、0、0.8 和 1。所有值的范围都是 [0,1]。然后，base_link 的视觉元素就会引用该材质。白色材质的定义与此类似。
  • 你也可以在可视化元素中定义材质标签，甚至在其他部件中引用它。不过，如果你重新定义了它，也不会有人抱怨。
  • 您还可以使用纹理来指定用于为对象着色的图像文件
ros2 launch urdf_tutorial display.launch.py model:=urdf/04-materials.urdf

b086a73b8c3a4984bd2918ecc780ca38.png

1.5 完成模型

现在，我们再为模型添加一些形状：脚、轮子和头。最值得注意的是，我们添加了一个球体和一些网格。我们还将添加一些稍后会用到的其他部件。 

  1. <?xml version="1.0"?>
  2. <robot name="visual">
  3.  
  4.   <material name="blue">
  5.     <color rgba="0 0 0.8 1"/>
  6.   </material>
  7.   <material name="black">
  8.     <color rgba="0 0 0 1"/>
  9.   </material>
  10.   <material name="white">
  11.     <color rgba="1 1 1 1"/>
  12.   </material>
  13.  
  14.   <link name="base_link">
  15.     <visual>
  16.       <geometry>
  17.         <cylinder length="0.6" radius="0.2"/>
  18.       </geometry>
  19.       <material name="blue"/>
  20.     </visual>
  21.   </link>
  22.  
  23.   <link name="right_leg">
  24.     <visual>
  25.       <geometry>
  26.         <box size="0.6 0.1 0.2"/>
  27.       </geometry>
  28.       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  29.       <material name="white"/>
  30.     </visual>
  31.   </link>
  32.  
  33.   <joint name="base_to_right_leg" type="fixed">
  34.     <parent link="base_link"/>
  35.     <child link="right_leg"/>
  36.     <origin xyz="0 -0.22 0.25"/>
  37.   </joint>
  38.  
  39.   <link name="right_base">
  40.     <visual>
  41.       <geometry>
  42.         <box size="0.4 0.1 0.1"/>
  43.       </geometry>
  44.       <material name="white"/>
  45.     </visual>
  46.   </link>
  47.  
  48.   <joint name="right_base_joint" type="fixed">
  49.     <parent link="right_leg"/>
  50.     <child link="right_base"/>
  51.     <origin xyz="0 0 -0.6"/>
  52.   </joint>
  53.  
  54.   <link name="right_front_wheel">
  55.     <visual>
  56.       <origin rpy="1.57075 0 0" xyz="0 0 0"/>
  57.       <geometry>
  58.         <cylinder length="0.1" radius="0.035"/>
  59.       </geometry>
  60.       <material name="black"/>
  61.     </visual>
  62.   </link>
  63.   <joint name="right_front_wheel_joint" type="fixed">
  64.     <parent link="right_base"/>
  65.     <child link="right_front_wheel"/>
  66.     <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
  67.   </joint>
  68.  
  69.   <link name="right_back_wheel">
  70.     <visual>
  71.       <origin rpy="1.57075 0 0" xyz="0 0 0"/>
  72.       <geometry>
  73.         <cylinder length="0.1" radius="0.035"/>
  74.       </geometry>
  75.       <material name="black"/>
  76.     </visual>
  77.   </link>
  78.   <joint name="right_back_wheel_joint" type="fixed">
  79.     <parent link="right_base"/>
  80.     <child link="right_back_wheel"/>
  81.     <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
  82.   </joint>
  83.  
  84.   <link name="left_leg">
  85.     <visual>
  86.       <geometry>
  87.         <box size="0.6 0.1 0.2"/>
  88.       </geometry>
  89.       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  90.       <material name="white"/>
  91.     </visual>
  92.   </link>
  93.  
  94.   <joint name="base_to_left_leg" type="fixed">
  95.     <parent link="base_link"/>
  96.     <child link="left_leg"/>
  97.     <origin xyz="0 0.22 0.25"/>
  98.   </joint>
  99.  
  100.   <link name="left_base">
  101.     <visual>
  102.       <geometry>
  103.         <box size="0.4 0.1 0.1"/>
  104.       </geometry>
  105.       <material name="white"/>
  106.     </visual>
  107.   </link>
  108.  
  109.   <joint name="left_base_joint" type="fixed">
  110.     <parent link="left_leg"/>
  111.     <child link="left_base"/>
  112.     <origin xyz="0 0 -0.6"/>
  113.   </joint>
  114.  
  115.   <link name="left_front_wheel">
  116.     <visual>
  117.       <origin rpy="1.57075 0 0" xyz="0 0 0"/>
  118.       <geometry>
  119.         <cylinder length="0.1" radius="0.035"/>
  120.       </geometry>
  121.       <material name="black"/>
  122.     </visual>
  123.   </link>
  124.   <joint name="left_front_wheel_joint" type="fixed">
  125.     <parent link="left_base"/>
  126.     <child link="left_front_wheel"/>
  127.     <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
  128.   </joint>
  129.  
  130.   <link name="left_back_wheel">
  131.     <visual>
  132.       <origin rpy="1.57075 0 0" xyz="0 0 0"/>
  133.       <geometry>
  134.         <cylinder length="0.1" radius="0.035"/>
  135.       </geometry>
  136.       <material name="black"/>
  137.     </visual>
  138.   </link>
  139.   <joint name="left_back_wheel_joint" type="fixed">
  140.     <parent link="left_base"/>
  141.     <child link="left_back_wheel"/>
  142.     <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
  143.   </joint>
  144.  
  145.   <joint name="gripper_extension" type="fixed">
  146.     <parent link="base_link"/>
  147.     <child link="gripper_pole"/>
  148.     <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
  149.   </joint>
  150.  
  151.   <link name="gripper_pole">
  152.     <visual>
  153.       <geometry>
  154.         <cylinder length="0.2" radius="0.01"/>
  155.       </geometry>
  156.       <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
  157.     </visual>
  158.   </link>
  159.  
  160.   <joint name="left_gripper_joint" type="fixed">
  161.     <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
  162.     <parent link="gripper_pole"/>
  163.     <child link="left_gripper"/>
  164.   </joint>
  165.  
  166.   <link name="left_gripper">
  167.     <visual>
  168.       <origin rpy="0.0 0 0" xyz="0 0 0"/>
  169.       <geometry>
  170.         <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
  171.       </geometry>
  172.     </visual>
  173.   </link>
  174.  
  175.   <joint name="left_tip_joint" type="fixed">
  176.     <parent link="left_gripper"/>
  177.     <child link="left_tip"/>
  178.   </joint>
  179.  
  180.   <link name="left_tip">
  181.     <visual>
  182.       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
  183.       <geometry>
  184.         <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
  185.       </geometry>
  186.     </visual>
  187.   </link>
  188.   <joint name="right_gripper_joint" type="fixed">
  189.     <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
  190.     <parent link="gripper_pole"/>
  191.     <child link="right_gripper"/>
  192.   </joint>
  193.  
  194.   <link name="right_gripper">
  195.     <visual>
  196.       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
  197.       <geometry>
  198.         <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
  199.       </geometry>
  200.     </visual>
  201.   </link>
  202.  
  203.   <joint name="right_tip_joint" type="fixed">
  204.     <parent link="right_gripper"/>
  205.     <child link="right_tip"/>
  206.   </joint>
  207.  
  208.   <link name="right_tip">
  209.     <visual>
  210.       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
  211.       <geometry>
  212.         <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
  213.       </geometry>
  214.     </visual>
  215.   </link>
  216.  
  217.   <link name="head">
  218.     <visual>
  219.       <geometry>
  220.         <sphere radius="0.2"/>
  221.       </geometry>
  222.       <material name="white"/>
  223.     </visual>
  224.   </link>
  225.   <joint name="head_swivel" type="fixed">
  226.     <parent link="base_link"/>
  227.     <child link="head"/>
  228.     <origin xyz="0 0 0.3"/>
  229.   </joint>
  230.  
  231.   <link name="box">
  232.     <visual>
  233.       <geometry>
  234.         <box size="0.08 0.08 0.08"/>
  235.       </geometry>
  236.       <material name="blue"/>
  237.     </visual>
  238.   </link>
  239.  
  240.   <joint name="tobox" type="fixed">
  241.     <parent link="head"/>
  242.     <child link="box"/>
  243.     <origin xyz="0.1814 0 0.1414"/>
  244.   </joint>
  245. </robot>
ros2 launch urdf_tutorial display.launch.py model:=urdf/05-visual.urdf

 

f8eaa4bc2f56479790ff3a9241191726.png

如何添加球体应该不难解释：

  1. <link name="head">
  2.   <visual>
  3.     <geometry>
  4.       <sphere radius="0.2"/>
  5.     </geometry>
  6.     <material name="white"/>
  7.   </visual>
  8. </link>

 这里的网格是从 PR2 中借用的。它们是独立的文件，需要指定路径。应使用 package://NAME_OF_PACKAGE/path 符号。本教程的网格位于 urdf_tutorial 包中名为 meshes 的文件夹中。

  1. <link name="left_gripper">
  2.   <visual>
  3.     <origin rpy="0.0 0 0" xyz="0 0 0"/>
  4.     <geometry>
  5.       <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
  6.     </geometry>
  7.   </visual>
  8. </link>
  •  网格可以多种不同格式导入。STL 是比较常见的格式，但引擎也支持 DAE，它可以有自己的颜色数据，这意味着你不必指定颜色/材料。通常情况下，这些都是单独的文件。这些网格参考网格文件夹中的 .tif 文件。
  • 网格也可以使用相对缩放参数或边界框尺寸来确定大小。
  • 我们也可以参考完全不同软件包中的网格。

就是这样。一个类似 R2D2 的 URDF 模型。现在你可以继续下一步，让它移动起来。

 二、建立可移动的机器人模型

        在本教程中，我们将修改上一教程中制作的 R2D2 模型，使其具有可移动的关节。在之前的模型中，所有的关节都是固定的。现在我们将探索另外三种重要的关节类型：连续关节、旋转关节和棱柱关节。

        在继续之前，请确保您已经安装了所有先决条件。有关所需信息，请参阅前面的教程。

        同样，本教程中提到的所有机器人模型都可以在 urdf_tutorial 包中找到。

        下面是带有柔性关节的新版 urdf。您可以将其与之前的版本进行比较，以了解所有变化，但我们将只关注三个关节示例。

        要可视化并控制该模型，请运行与上一教程相同的命令：

ros2 launch urdf_tutorial display.launch.py model:=urdf/06-flexible.urdf

不过，现在这也会弹出一个图形用户界面，允许你控制所有非固定关节的值。玩一下模型，看看它是如何移动的。然后，我们可以看看我们是如何做到这一点的。

2.1 头部

  1. <joint name="head_swivel" type="continuous">
  2.   <parent link="base_link"/>
  3.   <child link="head"/>
  4.   <axis xyz="0 0 1"/>
  5.   <origin xyz="0 0 0.3"/>
  6. </joint>

车身和车头之间的连接是一个连续的关节，这意味着它可以从负无穷大到正无穷大任意角度旋转。车轮也是这样建模的，因此它们可以永远向两个方向滚动。

我们唯一需要添加的额外信息是旋转轴，这里由 xyz 三元组指定，它指定了头部将围绕其旋转的矢量。由于我们希望它围绕 Z 轴旋转，所以我们指定了 "0 0 1 "这个向量。

 2.2 夹具

  1. <joint name="left_gripper_joint" type="revolute">
  2.   <axis xyz="0 0 1"/>
  3.   <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  4.   <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
  5.   <parent link="gripper_pole"/>
  6.   <child link="left_gripper"/>
  7. </joint>

 左右两个抓手关节都被建模为旋转关节。这意味着它们的旋转方式与连续关节相同，但有严格的限制。因此，我们必须在极限标签中指定关节的上限和下限（以弧度为单位）。我们还必须指定该关节的最大速度和力度，但实际值在这里并不重要。

2.3 夹持臂

  1. <joint name="gripper_extension" type="prismatic">
  2.   <parent link="base_link"/>
  3.   <child link="gripper_pole"/>
  4.   <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
  5.   <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
  6. </joint>

夹臂是一种不同的关节，即棱柱关节。这意味着它是沿轴线运动，而不是绕轴线运动。这种平移运动使我们的机器人模型能够伸出和缩回抓臂。

除了单位是米而不是弧度外，棱柱臂的极限指定方式与旋转接头相同。

2.4 其他类型的关节

还有两种关节可以在空间中移动。棱形关节只能沿一个维度移动，而平面关节可以在一个平面或两个维度内移动。此外，浮动关节不受约束，可以在三个维度中的任意维度移动。这些关节不能只用一个数字指定，因此不包括在本教程中。

2.5 指定姿势

当您在图形用户界面中移动滑块时，模型也会在 Rviz 中移动。 这是如何实现的？首先，图形用户界面会解析 URDF，找到所有非固定关节及其限制。然后，它使用滑块的值来发布 sensor_msgs/msg/JointState 消息。然后，机器人状态发布器会使用这些信息来计算不同部件之间的所有变换。然后，生成的变换树将用于在 Rviz 中显示所有形状。

三、添加物理和碰撞属性 

 在本教程中，我们将了解如何为 URDF 模型添加一些基本的物理属性，以及如何指定其碰撞属性。

3.1 碰撞

到目前为止，我们只用一个子元素视觉（visual）指定了我们的部件，它定义了（毫不奇怪）机器人的外观。但是，为了让碰撞检测发挥作用或模拟机器人，我们还需要定义一个碰撞元素。下面是带有碰撞和物理属性的新 urdf。

下面是新基础部件的代码。

  1. <link name="base_link">
  2.     <visual>
  3.       <geometry>
  4.         <cylinder length="0.6" radius="0.2"/>
  5.       </geometry>
  6.       <material name="blue">
  7.         <color rgba="0 0 .8 1"/>
  8.       </material>
  9.     </visual>
  10.     <collision>
  11.       <geometry>
  12.         <cylinder length="0.6" radius="0.2"/>
  13.       </geometry>
  14.     </collision>
  15.   </link>
  • 碰撞元素是链接对象的直接子元素，与视觉标记处于同一级别。
  • 碰撞元素定义形状的方式与视觉元素相同，都使用几何体标签。几何标记的格式与视觉标记完全相同。
  • 您还可以以与视觉元素相同的方式，将原点作为碰撞标记的子元素来指定。

在很多情况下，您都希望碰撞的几何体和原点与视觉的几何体和原点完全相同。不过，有两种主要情况是不需要的：

  • 更快的处理速度。对两个网格进行碰撞检测要比对两个简单几何体进行碰撞检测复杂得多。因此，您可能希望在碰撞元素中用更简单的几何图形替换网格。
  • 安全区。您可能希望限制靠近敏感设备的移动。例如，如果我们不想让任何东西与 R2D2 的头部发生碰撞，我们可以将碰撞几何体定义为一个包裹着他头部的圆柱体，以防止任何东西太靠近他的头部。

3.2 物理特性

为了让模型正常模拟，您需要定义机器人的几个物理属性，即 Gazebo 等物理引擎需要的属性。

3.2.1 惯性

每个被模拟的部件都需要一个惯性标签。下面是一个简单的例子。

    1. <link name="base_link">
    2.   <visual>
    3.     <geometry>
    4.       <cylinder length="0.6" radius="0.2"/>
    5.     </geometry>
    6.     <material name="blue">
    7.       <color rgba="0 0 .8 1"/>
    8.     </material>
    9.   </visual>
    10.   <collision>
    11.     <geometry>
    12.       <cylinder length="0.6" radius="0.2"/>
    13.     </geometry>
    14.   </collision>
    15.   <inertial>
    16.     <mass value="10"/>
    17.     <inertia ixx="1e-3" ixy="0.0" ixz="0.0" iyy="1e-3" iyz="0.0" izz="1e-3"/>
    18.   </inertial>
    19. </link>
  •  该部件也是 link 对象的子部件。
  • 质量以千克为单位。
  • 惯性元素指定了 3x3 旋转惯性矩阵。由于该矩阵是对称的，因此只需 6 个元素即可表示。

ixx

ixy

ixz

ixy

iyy

iyz

ixz

iyz

izz

  • 建模程序（如 MeshLab）可以提供这些信息。几何基元（圆柱体、方盒子、球体）的惯性可以通过维基百科上的惯性力矩张量列表计算出来（上例中使用的就是惯性力矩张量）。
  • 惯性张量取决于物体的质量和质量分布。第一种好的近似方法是假设物体体积内的质量分布相等，然后根据物体的形状计算惯性张量，如上所述。
  • 如果不确定放什么，对于一个中等大小的部件来说，ixx/iyy/izz=1e-3 或更小的矩阵通常是一个合理的默认值（它相当于一个边长为 0.1 米、质量为 0.6 千克的盒子）。单位矩阵是一个特别糟糕的选择，因为它往往过高（相当于边长 0.1 米、质量 600 千克的盒子！）。
  • 您还可以指定一个原点标签来指定重心和惯性参考系（相对于部件的参考系）。
  • 在使用实时控制器时，惯性部件为零（或几乎为零）会导致机器人模型在没有警告的情况下坍塌，所有链接的原点都将与世界原点重合。

3.3 接触系数

您还可以定义部件相互接触时的行为方式。这可以通过碰撞标记的子元素 contact_coefficients 来实现。有三个属性可以指定：

  • mu - 摩擦系数
  • kp - 刚度系数
  • kd - 阻尼系数

3.4 关节动力学

关节的运动方式由关节的动力学标签定义。这里有两个属性：

  • friction - 物理静摩擦力。对于棱柱关节，单位是牛顿。对于旋转接头，单位为牛顿米。
  • damping - 物理阻尼值。对于棱柱关节，单位为牛顿秒/米。对于旋转接头，单位为牛顿米秒/弧度。

如果未指定，这些系数默认为零。

3.5 其他标签

在纯 URDF 领域（即不包括 Gazebo 特有的标签），还有两个标签可以帮助定义关节：校准和安全控制器。请查看规范，因为本教程不包含这两个标签。

四、使用 Xacro 清理代码

现在，如果你正在家里按照这些步骤设计自己的机器人，你可能已经厌倦了为了让非常简单的机器人描述正确解析而做各种数学运算。幸运的是，您可以使用 xacro 软件包来简化您的生活。它有三个非常有用的功能。

  • 常量
  • 简单数学
  • 宏

在本教程中，我们将介绍所有这些快捷方式，以帮助减小 URDF 文件的整体大小，并使其更易于阅读和维护。

4.1 使用 Xacro

顾名思义，xacro 是一种 XML 宏语言。xacro 程序运行所有宏并输出结果。典型的用法如下

xacro model.xacro > model.urdf

您还可以在启动文件中自动生成 urdf。这很方便，因为它可以保持更新，而且不会占用硬盘空间。不过，生成需要一定时间，因此请注意启动文件可能需要更长时间才能启动。

要在启动文件中运行 xacro，需要将 Command 替换作为 robot_state_publisher 的参数。

  1. path_to_urdf = get_package_share_path('turtlebot3_description') / 'urdf' / 'turtlebot3_burger.urdf'
  2. robot_state_publisher_node = launch_ros.actions.Node(
  3.     package='robot_state_publisher',
  4.     executable='robot_state_publisher',
  5.     parameters=[{
  6.         'robot_description': ParameterValue(
  7.             Command(['xacro ', str(path_to_urdf)]), value_type=str
  8.         )
  9.     }]
  10. )

加载机器人模型的更简便方法是使用 urdf_launch 软件包自动加载 xacro/urdf。

  1. from launch import LaunchDescription
  2. from launch.actions import IncludeLaunchDescription
  3. from launch.substitutions import PathJoinSubstitution
  4. from launch_ros.substitutions import FindPackageShare
  5.  
  6.  
  7. def generate_launch_description():
  8.     ld = LaunchDescription()
  9.  
  10.     ld.add_action(IncludeLaunchDescription(
  11.         PathJoinSubstitution([FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']),
  12.         launch_arguments={
  13.             'urdf_package': 'turtlebot3_description',
  14.             'urdf_package_path': PathJoinSubstitution(['urdf', 'turtlebot3_burger.urdf'])}.items()
  15.     ))
  16.     return ld

在 URDF 文件的顶部，您必须指定一个命名空间，以便正确解析文件。例如，这是一个有效 xacro 文件的前两行：

  1. <?xml version="1.0"?>
  2. <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">

4.2 常量

让我们快速浏览一下 R2D2 中的 base_link。 

  1. <link name="base_link">
  2.   <visual>
  3.     <geometry>
  4.       <cylinder length="0.6" radius="0.2"/>
  5.     </geometry>
  6.     <material name="blue"/>
  7.   </visual>
  8.   <collision>
  9.     <geometry>
  10.       <cylinder length="0.6" radius="0.2"/>
  11.     </geometry>
  12.   </collision>
  13. </link>

这里的信息有点多余。我们两次指定了圆柱体的长度和半径。更糟糕的是，如果我们想要更改，需要在两个不同的地方进行更改。

幸运的是，xacro 允许您指定作为常量的属性。我们可以写这样的代码来代替上述代码。

  1. <xacro:property name="width" value="0.2" />
  2. <xacro:property name="bodylen" value="0.6" />
  3. <link name="base_link">
  4.     <visual>
  5.         <geometry>
  6.             <cylinder radius="${width}" length="${bodylen}"/>
  7.         </geometry>
  8.         <material name="blue"/>
  9.     </visual>
  10.     <collision>
  11.         <geometry>
  12.             <cylinder radius="${width}" length="${bodylen}"/>
  13.         </geometry>
  14.     </collision>
  15. </link>
  •  前两行指定了两个值。它们可以定义在任何地方（假设 XML 有效）、任何层级、使用之前或之后。通常它们放在最上面。
  • 我们没有在几何元素中指定实际半径，而是使用美元符号和大括号来表示该值。
  • 这段代码将生成与上图相同的代码。

然后，${} 结构的内容值将用于替换 ${}。这意味着您可以将其与属性中的其他文本结合起来。

  1. <xacro:property name=”robotname” value=”marvin” />
  2. <link name=”${robotname}s_leg” />

 这将生成

<link name=”marvins_leg” />

然而，${}中的内容不一定只能是一个属性，这就引出了我们的下一个问题...

4.3 数学

在 ${} 结构中，你可以使用四种基本运算（+,-,*,/）、一元减号和括号，建立任意复杂的表达式。例如

  1. <cylinder radius="${wheeldiam/2}" length="0.1"/>
  2. <origin xyz="${reflect*(width+.02)} 0 0.25" />

除 sin 和 cos 等基本数学运算外，您还可以使用更多运算。

4.4 宏

这是 xacro 软件包中最大、最有用的组件。

4.4.1 简单宏

让我们来看看一个简单无用的宏。

  1. <xacro:macro name="default_origin">
  2.     <origin xyz="0 0 0" rpy="0 0 0"/>
  3. </xacro:macro>
  4. <xacro:default_origin />

(这是无用的，因为如果不指定原点，它的值与此相同）。这段代码将生成以下内容。

<origin rpy="0 0 0" xyz="0 0 0"/>
  • 从技术上讲，名称并非必备元素，但您需要指定名称才能使用它。
  • <xacro:$NAME /> 的每个实例都会被替换为 xacro:macro 标记的内容。
  • 请注意，尽管内容不完全相同（两个属性的顺序互换了），但生成的 XML 是等价的。
  • 如果未找到指定名称的 xacro，它将不会被展开，也不会产生错误。

4.4.2 参数化宏

您还可以对宏进行参数化处理，这样它们就不会每次都生成完全相同的文本。如果与数学功能相结合，功能会更加强大。

首先，让我们以 R2D2 中使用的一个简单宏为例。

  1. <xacro:macro name="default_inertial" params="mass">
  2.     <inertial>
  3.             <mass value="${mass}" />
  4.             <inertia ixx="1e-3" ixy="0.0" ixz="0.0"
  5.                  iyy="1e-3" iyz="0.0"
  6.                  izz="1e-3" />
  7.     </inertial>
  8. </xacro:macro>

可与代码一起使用

<xacro:default_inertial mass="10"/>

参数的作用与属性相同，可以在表达式中使用。

您也可以将整个程序块用作参数。

  1. <xacro:macro name="blue_shape" params="name *shape">
  2.     <link name="${name}">
  3.         <visual>
  4.             <geometry>
  5.                 <xacro:insert_block name="shape" />
  6.             </geometry>
  7.             <material name="blue"/>
  8.         </visual>
  9.         <collision>
  10.             <geometry>
  11.                 <xacro:insert_block name="shape" />
  12.             </geometry>
  13.         </collision>
  14.     </link>
  15. </xacro:macro>
  16.  
  17. <xacro:blue_shape name="base_link">
  18.     <cylinder radius=".42" length=".01" />
  19. </xacro:blue_shape>
  •  要指定程序块参数，请在参数名称前加上星号。
  • 可以使用 insert_block 命令插入程序块
  • 插入程序块的次数不限。

4.4.3 实际使用

xacro 语言的使用非常灵活。除了上面显示的默认惯性宏之外，下面是 xacro 在 R2D2 模型中的几种实用方法。

要查看 xacro 文件生成的模型，请运行与之前教程相同的命令：

ros2 launch urdf_tutorial display.launch.py model:=urdf/08-macroed.urdf.xacro

(启动文件一直在运行 xacro 命令，但由于没有宏要展开，所以这并不重要）。

4.4.4 腿部宏

您经常需要在不同位置创建多个外观相似的对象。您可以使用宏和一些简单的数学计算来减少您需要编写的代码量，就像我们在 R2 的两条腿上所做的那样。

  1. <xacro:macro name="leg" params="prefix reflect">
  2.     <link name="${prefix}_leg">
  3.         <visual>
  4.             <geometry>
  5.                 <box size="${leglen} 0.1 0.2"/>
  6.             </geometry>
  7.             <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
  8.             <material name="white"/>
  9.         </visual>
  10.         <collision>
  11.             <geometry>
  12.                 <box size="${leglen} 0.1 0.2"/>
  13.             </geometry>
  14.             <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
  15.         </collision>
  16.         <xacro:default_inertial mass="10"/>
  17.     </link>
  18.  
  19.     <joint name="base_to_${prefix}_leg" type="fixed">
  20.         <parent link="base_link"/>
  21.         <child link="${prefix}_leg"/>
  22.         <origin xyz="0 ${reflect*(width+.02)} 0.25" />
  23.     </joint>
  24.     <!-- A bunch of stuff cut -->
  25. </xacro:macro>
  26. <xacro:leg prefix="right" reflect="1" />
  27. <xacro:leg prefix="left" reflect="-1" />
  • 常用窍门 1：使用名称前缀得到两个名称相似的物体。
  • 常用窍门 2：使用数学计算关节原点。在改变机器人尺寸的情况下，使用数学计算来计算关节偏移量，可以省去很多麻烦。
  • 常用技巧 3： 使用反射参数，并将其设置为 1 或-1。请看我们是如何使用反射参数，将身体两侧的腿置于 base_to_${prefix}_leg 原点的。

五、将 URDF 与机器人状态发布器结合使用 

5.1 背景

本教程将向您展示如何为行走机器人建模、将状态发布为 tf2 消息并在 Rviz 中查看模拟结果。 首先，我们创建描述机器人装配的 URDF 模型。接下来，我们编写一个节点，模拟运动并发布 JointState 和变换。然后，我们使用 robot_state_publisher 将整个机器人状态发布到 /tf2。

b7918206d0ac4b0481e46ce1c6c658ee.gif

5.2 任务

5.2.1 创建软件包

创建目录：

mkdir -p second_ros2_ws/src

然后创建软件包：

  1. cd second_ros2_ws/src
  2. ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_r2d2 --dependencies rclpy
  3. cd urdf_tutorial_r2d2

现在你应该看到一个 urdf_tutorial_r2d2 文件夹。接下来，您将对其进行一些更改。

5.2.2 创建 URDF 文件

创建一个目录，我们将在其中存储一些资产：

mkdir -p urdf

下载 URDF 文件并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml。下载 Rviz 配置文件并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz。

  1. <robot name="r2d2">
  2.  
  3.   <link name="axis">
  4.     <visual>
  5.       <origin xyz="0 0 0" rpy="1.57 0 0" />
  6.       <geometry>
  7.         <cylinder radius="0.01" length=".5" />
  8.       </geometry>
  9.       <material name="gray">
  10.         <color rgba=".2 .2 .2 1" />
  11.       </material>
  12.     </visual>
  13.  
  14.     <collision>
  15.       <origin xyz="0 0 0" rpy="1.57 0 0" />
  16.       <geometry>
  17.         <cylinder radius="0.01" length=".5" />
  18.       </geometry>
  19.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  20.     </collision>
  21.   </link>
  22.  
  23.   <link name="leg1">
  24.     <inertial>
  25.       <mass value="1"/>
  26.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  27.       <origin/>
  28.     </inertial>
  29.  
  30.     <visual>
  31.       <origin xyz="0 0 -.3" />
  32.       <geometry>
  33.         <box size=".20 .10 .8" />
  34.       </geometry>
  35.       <material name="white">
  36.         <color rgba="1 1 1 1"/>
  37.       </material>
  38.     </visual>
  39.  
  40.     <collision>
  41.       <origin xyz="0 0 -.3" />
  42.       <geometry>
  43.         <box size=".20 .10 .8" />
  44.       </geometry>
  45.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  46.     </collision>
  47.   </link>
  48.  
  49.   <joint name="leg1connect" type="fixed">
  50.     <origin xyz="0 .30 0" />
  51.     <parent link="axis"/>
  52.     <child link="leg1"/>
  53.   </joint>
  54.  
  55.   <link name="leg2">
  56.     <inertial>
  57.       <mass value="1"/>
  58.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  59.       <origin/>
  60.     </inertial>
  61.  
  62.     <visual>
  63.       <origin xyz="0 0 -.3" />
  64.       <geometry>
  65.         <box size=".20 .10 .8" />
  66.       </geometry>
  67.       <material name="white">
  68.         <color rgba="1 1 1 1"/>
  69.       </material>
  70.     </visual>
  71.  
  72.     <collision>
  73.       <origin xyz="0 0 -.3" />
  74.       <geometry>
  75.         <box size=".20 .10 .8" />
  76.       </geometry>
  77.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  78.     </collision>
  79.   </link>
  80.  
  81.   <joint name="leg2connect" type="fixed">
  82.     <origin xyz="0 -.30 0" />
  83.     <parent link="axis"/>
  84.     <child link="leg2"/>
  85.   </joint>
  86.  
  87.   <link name="body">
  88.     <inertial>
  89.       <mass value="1"/>
  90.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  91.       <origin/>
  92.     </inertial>
  93.  
  94.     <visual>
  95.       <origin xyz="0 0 -0.2" />
  96.       <geometry>
  97.         <cylinder radius=".20" length=".6"/>
  98.       </geometry>
  99.       <material name="white"/>
  100.     </visual>
  101.  
  102.     <collision>
  103.       <origin xyz="0 0 0.2" />
  104.       <geometry>
  105.         <cylinder radius=".20" length=".6"/>
  106.       </geometry>
  107.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  108.     </collision>
  109.   </link>
  110.  
  111.   <joint name="tilt" type="revolute">
  112.     <parent link="axis"/>
  113.     <child link="body"/>
  114.     <origin xyz="0 0 0" rpy="0 0 0" />
  115.     <axis xyz="0 1 0" />
  116.     <limit upper="0" lower="-.5" effort="10" velocity="10" />
  117.   </joint>
  118.  
  119.   <link name="head">
  120.     <inertial>
  121.       <mass value="1"/>
  122.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  123.       <origin/>
  124.     </inertial>
  125.  
  126.     <visual>
  127.       <geometry>
  128.         <sphere radius=".4" />
  129.       </geometry>
  130.       <material name="white" />
  131.     </visual>
  132.  
  133.     <collision>
  134.       <origin/>
  135.       <geometry>
  136.         <sphere radius=".4" />
  137.       </geometry>
  138.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  139.     </collision>
  140.   </link>
  141.  
  142.   <joint name="swivel" type="continuous">
  143.     <origin xyz="0 0 0.1" />
  144.     <axis xyz="0 0 1" />
  145.     <parent link="body"/>
  146.     <child link="head"/>
  147.   </joint>
  148.  
  149.   <link name="rod">
  150.     <inertial>
  151.       <mass value="1"/>
  152.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  153.       <origin/>
  154.     </inertial>
  155.  
  156.     <visual>
  157.       <origin xyz="0 0 -.1" />
  158.       <geometry>
  159.         <cylinder radius=".02" length=".2" />
  160.       </geometry>
  161.       <material name="gray" />
  162.  
  163.     </visual>
  164.  
  165.     <collision>
  166.       <origin/>
  167.       <geometry>
  168.         <cylinder radius=".02" length=".2" />
  169.       </geometry>
  170.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  171.     </collision>
  172.   </link>
  173.  
  174.   <joint name="periscope" type="prismatic">
  175.     <origin xyz=".12 0 .15" />
  176.     <axis xyz="0 0 1" />
  177.     <limit upper="0" lower="-.5" effort="10" velocity="10" />
  178.     <parent link="head"/>
  179.     <child link="rod"/>
  180.   </joint>
  181.  
  182.   <link name="box">
  183.     <inertial>
  184.       <mass value="1"/>
  185.       <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
  186.       <origin/>
  187.     </inertial>
  188.  
  189.     <visual>
  190.       <geometry>
  191.         <box size=".05 .05 .05" />
  192.       </geometry>
  193.       <material name="blue" >
  194.         <color rgba="0 0 1 1" />
  195.       </material>
  196.     </visual>
  197.  
  198.     <collision>
  199.       <origin/>
  200.       <geometry>
  201.         <box size=".05 .05 .05" />
  202.       </geometry>
  203.       <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  204.     </collision>
  205.   </link>
  206.  
  207.   <joint name="boxconnect" type="fixed">
  208.     <origin xyz="0 0 0" />
  209.     <parent link="rod"/>
  210.     <child link="box"/>
  211.   </joint>
  212.  
  213. </robot>
  1. Panels:
  2.   - Class: rviz_common/Displays
  3.     Help Height: 78
  4.     Name: Displays
  5.     Property Tree Widget:
  6.       Expanded:
  7.         - /Global Options1
  8.         - /Status1
  9.         - /TF1
  10.         - /RobotModel1
  11.         - /RobotModel1/Description Topic1
  12.       Splitter Ratio: 0.5
  13.     Tree Height: 617
  14.   - Class: rviz_common/Selection
  15.     Name: Selection
  16.   - Class: rviz_common/Tool Properties
  17.     Expanded:
  18.       - /2D Goal Pose1
  19.       - /Publish Point1
  20.     Name: Tool Properties
  21.     Splitter Ratio: 0.5886790156364441
  22.   - Class: rviz_common/Views
  23.     Expanded:
  24.       - /Current View1
  25.     Name: Views
  26.     Splitter Ratio: 0.5
  27. Visualization Manager:
  28.   Class: ""
  29.   Displays:
  30.     - Alpha: 0.5
  31.       Cell Size: 1
  32.       Class: rviz_default_plugins/Grid
  33.       Color: 160; 160; 164
  34.       Enabled: true
  35.       Line Style:
  36.         Line Width: 0.029999999329447746
  37.         Value: Lines
  38.       Name: Grid
  39.       Normal Cell Count: 0
  40.       Offset:
  41.         X: 0
  42.         Y: 0
  43.         Z: 0
  44.       Plane: XY
  45.       Plane Cell Count: 10
  46.       Reference Frame: <Fixed Frame>
  47.       Value: true
  48.     - Class: rviz_default_plugins/TF
  49.       Enabled: true
  50.       Frame Timeout: 15
  51.       Frames:
  52.         All Enabled: true
  53.         axis:
  54.           Value: true
  55.         body:
  56.           Value: true
  57.         box:
  58.           Value: true
  59.         head:
  60.           Value: true
  61.         leg1:
  62.           Value: true
  63.         leg2:
  64.           Value: true
  65.         odom:
  66.           Value: true
  67.         rod:
  68.           Value: true
  69.       Marker Scale: 1
  70.       Name: TF
  71.       Show Arrows: true
  72.       Show Axes: true
  73.       Show Names: false
  74.       Tree:
  75.         odom:
  76.           axis:
  77.             body:
  78.               head:
  79.                 rod:
  80.                   box:
  81.                     {}
  82.             leg1:
  83.               {}
  84.             leg2:
  85.               {}
  86.       Update Interval: 0
  87.       Value: true
  88.     - Alpha: 1
  89.       Class: rviz_default_plugins/RobotModel
  90.       Collision Enabled: false
  91.       Description File: ""
  92.       Description Source: Topic
  93.       Description Topic:
  94.         Depth: 5
  95.         Durability Policy: Volatile
  96.         History Policy: Keep Last
  97.         Reliability Policy: Reliable
  98.         Value: /robot_description
  99.       Enabled: true
  100.       Links:
  101.         All Links Enabled: true
  102.         Expand Joint Details: false
  103.         Expand Link Details: false
  104.         Expand Tree: false
  105.         Link Tree Style: Links in Alphabetic Order
  106.         axis:
  107.           Alpha: 1
  108.           Show Axes: false
  109.           Show Trail: false
  110.           Value: true
  111.         body:
  112.           Alpha: 1
  113.           Show Axes: false
  114.           Show Trail: false
  115.           Value: true
  116.         box:
  117.           Alpha: 1
  118.           Show Axes: false
  119.           Show Trail: false
  120.           Value: true
  121.         head:
  122.           Alpha: 1
  123.           Show Axes: false
  124.           Show Trail: false
  125.           Value: true
  126.         leg1:
  127.           Alpha: 1
  128.           Show Axes: false
  129.           Show Trail: false
  130.           Value: true
  131.         leg2:
  132.           Alpha: 1
  133.           Show Axes: false
  134.           Show Trail: false
  135.           Value: true
  136.         rod:
  137.           Alpha: 1
  138.           Show Axes: false
  139.           Show Trail: false
  140.           Value: true
  141.       Name: RobotModel
  142.       TF Prefix: ""
  143.       Update Interval: 0
  144.       Value: true
  145.       Visual Enabled: true
  146.   Enabled: true
  147.   Global Options:
  148.     Background Color: 48; 48; 48
  149.     Fixed Frame: odom
  150.     Frame Rate: 30
  151.   Name: root
  152.   Tools:
  153.     - Class: rviz_default_plugins/Interact
  154.       Hide Inactive Objects: true
  155.     - Class: rviz_default_plugins/MoveCamera
  156.     - Class: rviz_default_plugins/Select
  157.     - Class: rviz_default_plugins/FocusCamera
  158.     - Class: rviz_default_plugins/Measure
  159.       Line color: 128; 128; 0
  160.     - Class: rviz_default_plugins/SetInitialPose
  161.       Topic:
  162.         Depth: 5
  163.         Durability Policy: Volatile
  164.         History Policy: Keep Last
  165.         Reliability Policy: Reliable
  166.         Value: /initialpose
  167.     - Class: rviz_default_plugins/SetGoal
  168.       Topic:
  169.         Depth: 5
  170.         Durability Policy: Volatile
  171.         History Policy: Keep Last
  172.         Reliability Policy: Reliable
  173.         Value: /goal_pose
  174.     - Class: rviz_default_plugins/PublishPoint
  175.       Single click: true
  176.       Topic:
  177.         Depth: 5
  178.         Durability Policy: Volatile
  179.         History Policy: Keep Last
  180.         Reliability Policy: Reliable
  181.         Value: /clicked_point
  182.   Transformation:
  183.     Current:
  184.       Class: rviz_default_plugins/TF
  185.   Value: true
  186.   Views:
  187.     Current:
  188.       Class: rviz_default_plugins/Orbit
  189.       Distance: 10
  190.       Enable Stereo Rendering:
  191.         Stereo Eye Separation: 0.05999999865889549
  192.         Stereo Focal Distance: 1
  193.         Swap Stereo Eyes: false
  194.         Value: false
  195.       Focal Point:
  196.         X: 0
  197.         Y: 0
  198.         Z: 0
  199.       Focal Shape Fixed Size: true
  200.       Focal Shape Size: 0.05000000074505806
  201.       Invert Z Axis: false
  202.       Name: Current View
  203.       Near Clip Distance: 0.009999999776482582
  204.       Pitch: 0.459797203540802
  205.       Target Frame: <Fixed Frame>
  206.       Value: Orbit (rviz)
  207.       Yaw: 4.618575572967529
  208.     Saved: ~
  209. Window Geometry:
  210.   Displays:
  211.     collapsed: false
  212.   Height: 846
  213.   Hide Left Dock: false
  214.   Hide Right Dock: false
  215.   QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cb000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  216.   Selection:
  217.     collapsed: false
  218.   Tool Properties:
  219.     collapsed: false
  220.   Views:
  221.     collapsed: false
  222.   Width: 1340
  223.   X: 72
  224.   Y: 60

5.2.3 发布状态

现在，我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整体运动轨迹。

启动您最喜欢的编辑器，将以下代码粘贴到 second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py 中

  1. from math import sin, cos, pi
  2. import rclpy
  3. from rclpy.node import Node
  4. from rclpy.qos import QoSProfile
  5. from geometry_msgs.msg import Quaternion
  6. from sensor_msgs.msg import JointState
  7. from tf2_ros import TransformBroadcaster, TransformStamped
  8.  
  9. class StatePublisher(Node):
  10.  
  11.     def __init__(self):
  12.         rclpy.init()
  13.         super().__init__('state_publisher')
  14.  
  15.         qos_profile = QoSProfile(depth=10)
  16.         self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
  17.         self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
  18.         self.nodeName = self.get_name()
  19.         self.get_logger().info("{0} started".format(self.nodeName))
  20.  
  21.         degree = pi / 180.0
  22.         loop_rate = self.create_rate(30)
  23.  
  24.         # robot state
  25.         tilt = 0.
  26.         tinc = degree
  27.         swivel = 0.
  28.         angle = 0.
  29.         height = 0.
  30.         hinc = 0.005
  31.  
  32.         # message declarations
  33.         odom_trans = TransformStamped()
  34.         odom_trans.header.frame_id = 'odom'
  35.         odom_trans.child_frame_id = 'axis'
  36.         joint_state = JointState()
  37.  
  38.         try:
  39.             while rclpy.ok():
  40.                 rclpy.spin_once(self)
  41.  
  42.                 # update joint_state
  43.                 now = self.get_clock().now()
  44.                 joint_state.header.stamp = now.to_msg()
  45.                 joint_state.name = ['swivel', 'tilt', 'periscope']
  46.                 joint_state.position = [swivel, tilt, height]
  47.  
  48.                 # update transform
  49.                 # (moving in a circle with radius=2)
  50.                 odom_trans.header.stamp = now.to_msg()
  51.                 odom_trans.transform.translation.x = cos(angle)*2
  52.                 odom_trans.transform.translation.y = sin(angle)*2
  53.                 odom_trans.transform.translation.z = 0.7
  54.                 odom_trans.transform.rotation = \
  55.                     euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
  56.  
  57.                 # send the joint state and transform
  58.                 self.joint_pub.publish(joint_state)
  59.                 self.broadcaster.sendTransform(odom_trans)
  60.  
  61.                 # Create new robot state
  62.                 tilt += tinc
  63.                 if tilt < -0.5 or tilt > 0.0:
  64.                     tinc *= -1
  65.                 height += hinc
  66.                 if height > 0.2 or height < 0.0:
  67.                     hinc *= -1
  68.                 swivel += degree
  69.                 angle += degree/4
  70.  
  71.                 # This will adjust as needed per iteration
  72.                 loop_rate.sleep()
  73.  
  74.         except KeyboardInterrupt:
  75.             pass
  76.  
  77. def euler_to_quaternion(roll, pitch, yaw):
  78.     qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
  79.     qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
  80.     qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
  81.     qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
  82.     return Quaternion(x=qx, y=qy, z=qz, w=qw)
  83.  
  84. def main():
  85.     node = StatePublisher()
  86.  
  87. if __name__ == '__main__':
  88.     main()

 5.2.4 创建启动文件

新建一个 second_ros2_ws/src/urdf_tutorial_r2d2/launch 文件夹。打开编辑器并粘贴以下代码，保存为 second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo_launch.py

  1. import os
  2. from ament_index_python.packages import get_package_share_directory
  3. from launch import LaunchDescription
  4. from launch.actions import DeclareLaunchArgument
  5. from launch.substitutions import LaunchConfiguration
  6. from launch_ros.actions import Node
  7.  
  8. def generate_launch_description():
  9.  
  10.     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
  11.  
  12.     urdf_file_name = 'r2d2.urdf.xml'
  13.     urdf = os.path.join(
  14.         get_package_share_directory('urdf_tutorial_r2d2'),
  15.         urdf_file_name)
  16.     with open(urdf, 'r') as infp:
  17.         robot_desc = infp.read()
  18.  
  19.     return LaunchDescription([
  20.         DeclareLaunchArgument(
  21.             'use_sim_time',
  22.             default_value='false',
  23.             description='Use simulation (Gazebo) clock if true'),
  24.         Node(
  25.             package='robot_state_publisher',
  26.             executable='robot_state_publisher',
  27.             name='robot_state_publisher',
  28.             output='screen',
  29.             parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
  30.             arguments=[urdf]),
  31.         Node(
  32.             package='urdf_tutorial_r2d2',
  33.             executable='state_publisher',
  34.             name='state_publisher',
  35.             output='screen'),
  36.     ])

 

5.2.5 编辑 setup.py 文件

您必须告诉 colcon 编译工具如何安装您的 Python 软件包。编辑 second_ros2_ws/src/urdf_tutorial_r2d2/setup.py 文件如下：

  • 包含这些导入语句
  1. import os
  2. from glob import glob
  3. from setuptools import setup
  4. from setuptools import find_packages
  •  将这两行添加到 data_files 中
  1. data_files=[
  2.   ...
  3.   (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
  4.   (os.path.join('share', package_name), glob('urdf/*')),
  5. ],

 修改 entry_points 表，以便以后从控制台运行 "state_publisher

  1. 'console_scripts': [
  2.     'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
  3. ],

保存更改后的 setup.py 文件。

5.2.6 安装软件包

  1. cd second_ros2_ws
  2. colcon build --symlink-install --packages-select urdf_tutorial_r2d2

 源设置文件：

source install/setup.bash

5.2.7 查看结果

启动软件包

ros2 launch urdf_tutorial_r2d2 demo_launch.py

打开一个新终端，使用以下命令运行 Rviz

rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

 

 

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/花生_TL007/article/detail/700129
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号