4.RVIZ2可视化URDF_rviz2可视化移动机器人教程

4.RVIZ2可视化URDF

一、本节介绍：

在之前，我们讲了机器人建模语言URDF与可视化工具RVIZ2，在本节，我们将使用这两个工具来可视化一个移动机器人模型。具体步骤如下：

1. 建立机器人描述功能包
2. 建立URDF文件夹，编写机器人的URDF描述文件
3. 建立launch文件夹，编写launch文件
4. 修改setup.py配置

（注：建立文件夹的目的是为了便于文件管理，区分各文件功能。）

创建了一个名称为ros2_car的工作空间

mkdir -p ros2_car/src
cd ros2_car/src
1
2

建立功能包

以python为编程语言创建一个功能包，这个功能包用作对机器人的URDF信息进行描述，存在于src目录下，功能包创建代码如下：

ros2 pkg create ros_car_description --build-type ament_python
1

创建URDF文件

在工作空间的机器人的URDF描述功能包中创建urdf文件夹用以存放urdf文件

在urdf文件夹下创建一个urdf文件：

cd ros_car_description
mkdir urdf
vim ros_car.urdf
1
2
3

在这里我使用的是vim文本编辑器进行文本的创建与编辑，这个工具需要提前安装，若没有安装过这个文本编辑器，则需要输入以下指令进行安装

sudo apt install vim
1

在网上有很多与VIM相关的使用教程，大家可自行探索其使用方法，这里暂不作进一步讲述。

在这里创建了一个四轮小车模型。

ros_car.urdf：

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

  <!-- 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>
      <material name="blue">
          <color rgba="0.1 0.1 1.0 0.5" />
      </material>
    </visual>
  </link>

  <!-- laser_link -->
  <link name="laser_link">
      <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.03" 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>
  
  <!-- imu_link -->
  <link name="imu_link">
      <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
            <box size="0.02 0.02 0.02"/>
      </geometry>
    </visual>
  </link>

  <!-- imu_joint -->
  <joint name="imu_joint" type="fixed">
      <parent link="base_link" />
      <child link="imu_link" />
      <origin xyz="0 0 0.04" />
  </joint>

  <!-- left_wheel1_link -->
  <link name="left_wheel1_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.03" radius="0.024"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" />
          </material>
      </visual>
  </link>
  
  <!-- left_wheel1_joint -->
  <joint name="left_wheel1_joint" type="continuous">
      <parent link="base_link" />
      <child link="left_wheel1_link" />
      <origin xyz="-0.08 0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>
  
   <!-- left_wheel2_link -->
  <link name="left_wheel2_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.03" radius="0.024"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" />
          </material>
      </visual>
  </link>

  <!-- left_wheel2_joint -->
  <joint name="left_wheel2_joint" type="continuous">
      <parent link="base_link" />
      <child link="left_wheel2_link" />
      <origin xyz="0.08 0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>

  <!-- right_wheel1_link -->
  <link name="right_wheel1_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.03" radius="0.024"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" />
          </material>
      </visual>
  </link>

  <!-- right_wheel1_joint -->
  <joint name="right_wheel1_joint" type="continuous">
      <parent link="base_link" />
      <child link="right_wheel1_link" />
      <origin xyz="-0.08 -0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>

  <!-- right_wheel2_link -->
  <link name="right_wheel2_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.03" radius="0.024"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" />
          </material>
      </visual>
  </link>

  <!-- bright_wheel2_joint -->
  <joint name="right_wheel2_joint" type="continuous">
      <parent link="base_link" />
      <child link="right_wheel2_link" />
      <origin xyz="0.08 -0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>

  <!-- Robot Footprint -->
  <!--默认情况下模型是以base_link作为参照的，不着地，这里添加一个joint来让模型着地 -->
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.072" rpy="0 0 0"/>
  </joint>

</robot>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148

创建launch文件

使用 launch 来一键启动可视化URDF所需的相关结点，这些结点被封装进了特定的功能包内，这些功能包包括

joint_state_publisher：通过 joint_states 话题来负责发布机器人关节数据信息。

joint_state_publisher_gui：通过 joint_states 话题来负责发布机器人关节数据信息。这个功能包还会提供一个可视化界面供我们操作关节。

robot_state_publisher_node：通过 robot_description 话题来发布机器人的模型信息，并将joint_states数据转换成 tf 信息发布出去。（发布urdf的最终整合处）

先通过以下两个指令来安装这两个功能包：

sudo apt install ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-robot-state-publisher
1

RVIZ2通过这两个话题，就能够将我们的机器人模型可视化显示出来，现在来创建启动这两个结点的launch文件,在ros_car_description文件夹下创建一个launch文件夹，并launch文件夹下创建一个名为urdf_node.launch.py：

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 = 'ros_car_description'
    urdf_name = "ros_car.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name)  # 寻找 share 目录路径
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')  # 字符串拼接，最后一部分是格式化字符串，输出应为urdf/ros_car.urdf

    robot_state_publisher_node = Node(  # 通过功能包来调用机器人模型发布结点
        package='robot_state_publisher',  # 功能包名称
        executable='robot_state_publisher',  # 功能包下对应的可执行文件
        arguments=[urdf_model_path]  # 传递urdf文件的路径作为参数
        )

    joint_state_publisher_node = Node(  # 通过功能包来调用机器人模型关节的发布结点
        package='joint_state_publisher_gui',  # 功能包名称
        executable='joint_state_publisher_gui',  # 功能包下对应的可执行文件
        name='joint_state_publisher_gui',  # 为结点起一个名称，以区分其他同类型结点
        arguments=[urdf_model_path]  # 传递urdf文件的路径作为参数
        )

    ld.add_action(robot_state_publisher_node)  # 该结点的ROS2通信形式是动作，将结点动作加入launch执行器中
    ld.add_action(joint_state_publisher_node)  # 该结点的ROS2通信形式是动作，将结点动作加入launch执行器中

    return ld

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

在这里解释下为什么urdf文件存放在了share文件夹下，share目录用于存储与代码无关的静态数据，如URDF、配置文件和资源文件。将这些文件放在share目录下可以确保它们在包安装后被正确找到。许多ROS工具和库期望描述文件和其他资源位于share目录下。这样可以简化工具的设计，使得它们能够一致地查找和加载所需的资源。在代码中，使用 FindPackageShare 来查找包的位置，然后构建URDF文件的路径，这是标准的ROS 2做法。该命令会查找安装包中的 share 目录，并确保URDF文件路径的正确性。总的来说, share 目录统一存放各种数据及资源，以方便ROS2中的其他资源调用。

修改setup.py配置

在ROS2中使用 Python 语言编写完launch文件后，我们还需要修改setup.py文件。setup.py文件是 python 包管理和构建的重要部分，通过这个文件可以让系统明白如何构建和安装包等，声明程序的入口点。在此处，它定义了ROS 2包的元数据和依赖项（与package.xml文件的功能有一点点小重叠）。这些信息包括包的名称、版本、作者、描述、依赖关系等。通过定义这些信息，可以确保包的正确构建和安装。package.xml主要用于ROS2包的构建，而setup.py用于 Python 包的构建和安装，在ROS2里都用到了，故而两个文件都存在于其下功能包目录下。

setup.py：

from setuptools import find_packages, setup
from glob import glob
import os

package_name = 'ros_car_description'  # 我们所创建的功能包名称

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),  # 排除指定目录的查找该包路径
    data_files=[  # 在这里定义出需要的额外说明的数据元素
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),  # package.xml文件与setup.py文件共同作用，一个用于定义依赖关系和元数据，一个用于包的构建与安装
        
        # 此下两行为我们额外添加的launch文件路径及urdf文件路径
        (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='ailan',
    maintainer_email='ailan@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={  # 程序的主函数入口点，让系统明白从何处进入并运行代码
        'console_scripts': [
        ],
    },
)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

编译测试

现在整个工作空间下仅有 ros_car_description 这一个功能包，我们切换到这个功能包目录中，并在其下对功能包进行编译：

colcon build
1

再使用以下指令以配置当前的终端环境变量：

source install/setup.bash
1

这里简单解释下这个命令，该命令运行了一个脚本，该脚本会设置多个环境变量，包括但不限于以下内容：

• ROS_PACKAGE_PATH：指定ROS 2包的查找路径。
• AMENT_PREFIX_PATH：指定ament构建系统的前缀路径。
• LD_LIBRARY_PATH：指定共享库的查找路径。
• PYTHONPATH：指定Python包的查找路径。
• PATH：添加可执行文件的路径，使得你可以在命令行中直接运行ROS 2的可执行文件和工具。

这些环境变量的设置确保了在当前终端会话中，你可以正确运行和找到ROS 2包和工具。因而每个新的终端当中都需要实现运行一遍这行命令，这一步骤是必不可少的。

运行我们的 launch 文件，来发布机器人模型及关节信息。

ros2 launch ros_car_description urdf_node.launch.py
1

RVIZ2可视化机器人模型：

打开rviz2，在左侧栏下点击Add，添加 RobotModel 显示件，在 Description Topic 一栏中选择robot_state_publisher_node 所发布的 ros_description话题来可视化我们URDF中定义的机器人模型。启动rviz2：

rviz2
1

通过以下指令来查看当前有何话题被发布出来了：

ros2 topic list
1

结果如下：

这里对现在这几个被发布的话题作简单的介绍：

/joint_states：该话题用于发布机器人关节状态信息，包括各个关节的位置、速度、加速度等。由我们调用的功能包中的 robot_state_publisher 结点发布。
/parameter_events：该话题用于发布与参数（Parameters）相关的事件。当ROS参数的值发生变化时，会通过这个话题发布参数事件消息，通知订阅者参数的更新情况。
/robot_description：该话题用于发布机器人的URDF描述，即发布机器人的模型信息。由我们调用的功能包中的 joint_state_publisher_gui 结点发布。
/rosout：用于发布 ROS2 系统的日志信息。所有节点的输出日志都会自动发布到这个话题上，以便于监控和调试ROS系统的运行状态。
/tf：该话题用于发布机器人的坐标变换（Transforms）,即机器人不同坐标系之间的转换关系。
/tf_static：用于发布静态的坐标变换，其所发布的变换关系通常是固定不变的，即不随时间的变化而变化。

在RVIZ2中显示机器人模型：

如图所示配置RVIZ2左侧栏，点击 Focus Camera 移动镜头以进一步观察我们定义的机器人模型，可以发现我这里的每个 link 都显示了名称，这是因为我在RVIZ2中添加了 TF 件并勾选了 Show Names 属性，如左侧显示，调整Marker Scale以控制显示字体的大小，最终显示效果如下图：