【ROS2】机器人建模与仿真（1）——建模入门（URDF+rviz2）

refer：小鱼

一. 机器人建模

采用URDF（Unified Robot Description Format）统一机器人描述格式，URDF使用XML格式描述机器人文件。

通过代码的形式描述机器人的各个组件，以及组件之间的联动关系。

1. URDF的组成

1.1 申明信息

xml的申明信息

<?xml version="1.0"?>

机器人的申明信息

<robot name="fishbot">
     
</robot>

1.2 两种关键组件

1. LINK：
   • LINK 是机器人模型中的基本构建块之一，用于描述机器人的刚性部件或链接。
   • 每个 LINK 都有自己的坐标系，用于定义其位置和姿态。
   • LINK 可以包含几何形状（用于可视化）、质量信息、惯性信息等。
   • LINK 之间可以通过 JOINT 连接起来，形成机器人的链式结构。
2. JOINT：
   • JOINT 是用于连接 LINK 的关键组件，允许 LINK 之间相对运动。
   • JOINT 定义了两个 LINK 之间的相对运动方式，比如旋转、平移等。
   • JOINT 有不同类型，包括旋转关节、平移关节、固定关节等，每种类型定义了不同的运动方式。
   • JOINT 通常具有限制条件，如最大/最小角度、最大/最小位移等。

Link & Joint 分别表示机器人的各个组件（Link）以及组件与组件间的相对关系：关节（Joint），通过这两种组件即可完成对一个机器人的描述。

1.2.1 Link组件

1. 标签详情

• visual 显示形状

  • <geometry> (几何形状)
    

    • <box> 长方体
      

      • 标签属性: size-长宽高
      • 举例：<box size="1 1 1" />

    • <cylinder> 圆柱体
      

      • 标签属性:radius -半径 length-高度
      • 举例：<cylinder radius="1" length="0.5"/>

    • sphere 球体
      

      • 属性：radius -半径
      • 举例：<sphere radius="0.015"/>

    • mesh 第三方导出的模型文件
      

      • 属性：filename
      • 举例: <mesh filename="package://robot_description/meshes/base_link.DAE"/>

  • origin (可选：默认在物体几何中心)

    • 属性 xyz默认为零矢量 rpy弧度表示的翻滚、俯仰、偏航
    • 举例：<origin xyz="0 0 0" rpy="0 0 0" />

  • material 材质

    • 属性 name 名字
      • color
        • 属性 rgba a代表透明度
        • 举例：<material name="white"><color rgba="1.0 1.0 1.0 0.5" /> </material>

• collision 碰撞属性，仿真章节中讲解

• inertial 惯性参数 质量等，仿真章节中讲解

2. 示例代码

  <!-- base link -->
  <link name="base_link">
      <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.10"/>
      </geometry>
    </visual>
  </link>

• <link> 元素定义了一个链接，它有一个名为 “base_link” 的属性。
• <visual> 元素指定了这个链接的可视化外观。
• <origin> 元素指定了链接的原点位置和旋转角度。在这个例子中，xyz 属性表示链接原点的坐标为 (0, 0, 0)，rpy 属性表示链接的旋转角度为 (0, 0, 0)。
• <geometry> 元素定义了链接的几何形状。
• <cylinder> 元素指定了链接的几何形状为一个圆柱体，具有长度（length）为 0.12，半径（radius）为 0.1

1.2.2 Joint组件

描述Link之间的关系，包括：

• 父子之间的连接类型，包括是否固定的，可以旋转的等
• 父部件名字
• 子部件名字
• 父子之间相对位置
• 父子之间的旋转轴，绕哪个轴转

1. 标签详情

Joint属性

• name 关节的名称
• type 关节的类型
  • revolute: 旋转关节，绕单轴旋转,角度有上下限,比如舵机0-180
  • continuous: 旋转关节，可以绕单轴无限旋转,比如自行车的前后轮
  • fixed: 固定关节，不允许运动的特殊关节
  • prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
  • planer: 平面关节，允许在x/y/z，rx/ry/rz六个方向运动
  • floating: 浮动关节，允许进行平移、旋转运动

Joint子标签

• parent 父link名称
  

  • <parent link="base_link" />

• child 子link名称
  

  • <child link="laser_link" />

• origin 父子之间的关系xyz rpy
  

  • <origin xyz="0 0 0.014" />

• axis 围绕旋转的关节轴
  

  • <axis xyz="0 0 1" />

2. 示例代码

<?xml version="1.0"?>
<robot name="fishbot">
    
  <!-- base link -->
  <link name="base_link">
      <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.10"/>
      </geometry>
    </visual>
  </link>
    
  <!-- laser link -->
  <link name="laser_link">
      <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.02"/>
      </geometry>
      <material name="black">
          <color rgba="0.0 0.0 0.0 0.5" /> 
      </material>
    </visual>
  </link>
    
  <!-- laser joint -->
    <joint name="laser_joint" type="fixed">
        <parent link="base_link" />
        <child link="laser_link" />
        <origin xyz="0 0 0.075" />
    </joint>

</robot>

二. RVIZ2模型可视化

基于URDF的描述，通过RVIZ2显示出来可视化模型。

大致步骤：

• 建立工作空间，建立机器人描述功能包
• 建立urdf文件夹编写urdf文件
• 建立launch文件夹，编写launch文件
• 修改setup.py配置，编译测试

1. 建立工作空间与功能包

# 创建工作空间
mkdir fishbot_ws
mkdir src
cd src

# 创建功能包
ros2 pkg create fishbot_description --build-type ament_python

2. 建立并编写URDF文件

# 建立URDF文件夹，创建URDF文件
cd fishbot_description && mkdir urdf
cd urdf

touch fishbot_base.urdf

完成后的src文件结构：

├── fishbot_description
│   ├── __init__.py
├── package.xml
├── setup.cfg
├── setup.py
└── urdf
    └── fishbot_base.urdf

编辑fishbot_base.urdf，把刚才的代码复制进取就行

<?xml version="1.0"?>
<robot name="fishbot">
    
  <!-- base link -->
  <link name="base_link">
      <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.10"/>
      </geometry>
    </visual>
  </link>
    
  <!-- laser link -->
  <link name="laser_link">
      <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.02"/>
      </geometry>
      <material name="black">
          <color rgba="0.0 0.0 0.0 0.8" /> 
      </material>
    </visual>
  </link>
    
  <!-- laser joint -->
    <joint name="laser_joint" type="fixed">
        <parent link="base_link" />
        <child link="laser_link" />
        <origin xyz="0 0 0.075" />
    </joint>

</robot>

3. 建立并编写launch文件

在目录src/fishbot_description下创建launch文件夹并在其下新建display_rviz2.launch.py文件。

mkdir launch
touch display_rviz2.launch.py

完成后的目录结构：

├── fishbot_description
│   ├── __init__.py
├── launch
│   └── display_rviz2.launch.py
├── package.xml
├── setup.cfg
├── setup.py
└── urdf
    └── fishbot_base.urdf

编辑launch文件

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_name = 'fishbot_description'
    urdf_name = "fishbot_base.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
        )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        arguments=[urdf_model_path]
        )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        )

    ld.add_action(robot_state_publisher_node)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(rviz2_node)

    return ld

• generate_launch_description() 函数是入口点，用于生成 LaunchDescription 对象，描述了启动的节点及其参数。
• FindPackageShare 类用于查找指定包的共享目录，以便获取该包中的资源文件。在这里，它用于获取机器人描述包（fishbot_description）的共享目录路径。
• LaunchDescription 对象用于存储要启动的节点及其参数。

其中涉及三个节点

• joint_state_publisher_gui 负责发布机器人关节数据信息，通过joint_states话题发布
• robot_state_publisher_node负责发布机器人模型信息robot_description，并将joint_states数据转换tf信息发布
• rviz2_node负责显示机器人的信息

最终，通过调用 ld.add_action() 将这些节点添加到 LaunchDescription 对象中，并将其返回，以便由 ROS 2 的启动系统加载并执行。

attention:

这里我们用到了joint_state_publisher_gui和robot_state_publisher两个包，如果你的系统没有安装这两个包，可以手动安装:

sudo apt install ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-robot-state-publisherCopy to clipboardErrorCopied

joint_state_publisher_gui，还有一个兄弟叫做joint_state_publisher

两者区别在于joint_state_publisher_gui运行起来会跳出一个界面，通过界面可以操作URDF中能动的关节

4.修改setup.py

导入头文件

from glob import glob
import os

更改data_file列表:

data_files 列表包含了要安装到系统中的其他文件和目录，以及它们的安装路径。在这里需要包含了 resource_index/packages、package.xml、launch 目录下的所有 .launch.py 文件、urdf 目录下的所有文件等。

完整setup.py

from setuptools import setup
from glob import glob
import os

package_name = 'fishbot_description'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/**')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='root@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

5.编译测试

回到工作空间，编译

colcon build

运行测试

source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py

运行结果：

发现啥都没有，需要进行一些配置。

1. 设置Global Options -> Fixed Frame
   

2. 设置Grid->Reference Frame
   

3. 添加Robotmode模块
   

设置Description Topic 为 /robot_description

然后放大中间的那一个小红点，可以看到URDF所描述的机器人了

可以通过tf查看link之间的坐标关系。

坐标关系可视化：