ROS下机器人系统仿真及部分SLAM建图

温馨提示：如果有幸看到这个文章，不要看里面的内容，因为只是实验的记录，你看这个文章只会头疼。如果要了解，主要看后面的参考资料和以及这篇文章遇到的某些问题的解决可能对你帮助更大。

一、 Launch文件使用

launch文件的作用是，将你要启动的节点配置编写进launch文件中，然后使用roslaunch启动就不用再开很多bash了。使用步骤如下：

- 在功能包下创建launch文件夹
- 在launch文件夹下创建launch后缀的文件
- 编写launch文件
- roslaunch 功能包名称 launch文件（不用先roscore，roslaunch会自动完成）
1
2
3
4

以下列出一些编写规则：

<launch>
    <node pkg="功能包名称" type="节点（就是add_excutable中的目标文件名）" name="随便的一个命名"></node>
    <node pkg="功能包名称" type="节点（就是add_excutable中的目标文件名）" name="随便的一个命名" output="screen"></node>
</launch>
# 其中的output属性是当有输出的时候输出到控制台，如果没有这个属性，就不会产生输出
1
2
3
4
5

二、 参考资料

机器人模型构建参考资料
gazebo的参考资料

三、 遇到的问题

1. 启动gazebo的时候，显示连接不到服务，并且开启没有机器人模型，查资料是显示gazebo的world文件中有一个<sim_time>标签，需要改成0，似乎是超时问题。
   

四、 效果演示

启动gazebo仿真环境

启动SLAM建图节点

启动键盘节点

开始运动建图：

建好的图：

五、相关代码

5.1 一些简介

相关文件简介：

• URDF 是 Unified Robot Description Format 的首字母缩写，直译为统一(标准化)机器人描述格式，可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度…,该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件
• RViz 是 ROS Visualization Tool 的首字母缩写，直译为ROS的三维可视化工具。它的主要目的是以三维方式显示ROS消息，可以将 数据进行可视化表达。例如:可以显示机器人模型，可以无需编程就能表达激光测距仪（LRF）传感器中的传感 器到障碍物的距离，RealSense、Kinect或Xtion等三维距离传感器的点云数据（PCD， Point Cloud Data），从相机获取的图像值等
• Gazebo是一款3D动态模拟器，用于显示机器人模型并创建仿真环境,能够在复杂的室内和室外环境中准确有效地模拟机器人。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。

5.2 机器人模型

在参考资料里面，是从urdf开始的，但是urdf代码复用性比较低，所以后面使用了xacro工具，它有点类似于C的模板类，就是给一个宏定义加一些属性，套进模板中。这里直接贴后期使用xacro的代码。但是还是建议跟着参考资料 (非常详细) 那个文档做，然后再加上大佬的视频，配套起来看，真香！！！（建议初次接触哪里不理解就点哪里的视频看，这样速度会快一些，到后期再去深究吧）。

5.2.1 机器人底盘

创建一个四轮圆柱状机器人模型，机器人参数如下：
1、底盘：圆柱状，半径 10cm，高 8cm
2、两个驱动轮：圆柱，半径为 3.25cm,轮胎宽度1.5cm，垂直放置（翻转90度）
3、两个万向支撑轮：球状，半径 0.75cm
4、底盘离地间距为 1.5cm(与万向轮直径一致)

因为我是全部跟着参考资料做到导航那个板块才倒回来写报告的，以下代码包括了xacro的优化，以及是为了集成到gazebo中的，所以还加上了collision和inertial这两个标签，一个是碰撞属性，一个是惯性属性，这两个标签是gazebo仿真必须要添加的。

pillarrobot.xacro文件

<robot name = "pillarrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:property name="PI" value="3.1415926"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>

    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->
    <xacro:property name="base_link_m" value="0.5"/><!-- 质量 -->

    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${base_footprint_radius}" />
            </geometry>
        </visual>
    </link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
      </collision>
      <xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}"/>
    </link>

    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link"/>
        <origin xyz="0 0 ${earth_space+base_link_length/2}" />
    </joint>
    <gazebo reference="base_link">
      <material>Gazebo/Yellow</material>
    </gazebo>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <xacro:property name="wheel_m" value="0.05"/><!-- 质量 -->

    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
        <collision>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}"/>
      </link>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag*base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>

      <gazebo reference="${name}_wheel">
        <material>Gazebo/Red</material>
      </gazebo>
    </xacro:macro>

    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />

<!-- 万向轮半径 -->
    <xacro:property name="support_wheel_radius" value="0.0075" />
    <xacro:property name="support_wheel_m" value="0.03"/>

    <!-- 万相轮宏实现 -->
    <xacro:macro name ="add_support_wheel" params="name flag">
        <link name = "${name}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${support_wheel_radius}" />
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <material name="black"/>
            </visual>
            <collision>
              <geometry>
                  <sphere radius="${support_wheel_radius}" />
              </geometry>
              <origin xyz="0 0 0" rpy="0 0 0" />
            </collision>
            <xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" />
        </link>

        <joint name="${name}_wheel2base_link" type="continuous">
            <parent link="base_link" />
            <child link="${name}_wheel" />
            <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
            <axis xyz="1 1 1" />
        </joint>

        <gazebo reference="${name}_wheel">
          <material>Gazebo/Red</material>
        </gazebo>
    </xacro:macro>

    <xacro:add_support_wheel name="front" flag="1"/>
    <xacro:add_support_wheel name="back" flag="-1"/>

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

launch文件：

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find 你创建的功能包)/保存的路径/上面创建的那个文件(pillarrobot.xacro)" />

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 你创建的功能包)/保存的rviz配置文件路径" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" />
</launch>
1
2
3
4
5
6
7
8

启动指令
roslaunch 功能包 launch文件

5.2.2 摄像头

<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 摄像头属性 -->
    <xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->
    <xacro:property name="camera_m" value="0.01"/>

    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}"/>
    </link>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    </joint>

    <gazebo reference="camera">
        <material>Gazebo/Blue</material>
    </gazebo>
</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

5.2.3 雷达

<!--
    小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 雷达支架 -->
    <xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
    <xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
    <xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
    <xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2  -->
    <xacro:property name="support_m" value="0.02"/>

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red">
                <color rgba="0.8 0.2 0.0 0.8" />
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>

        <xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}"/>
    </link>

    <joint name="support2base_link" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${support_x} ${support_y} ${support_z}" />
    </joint>

    <gazebo reference="support">
        <material>Gazebo/White</material>
    </gazebo>


    <!-- 雷达属性 -->
    <xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->
    <xacro:property name="laser_m" value="0.1"/>

    <!-- 雷达关节以及link -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}"/>
    </link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    </joint>

    <gazebo reference="laser">
        <material>Gazebo/Black</material>
    </gazebo>
</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

5.3 惯性矩阵

上面代码涉及到了inertial，这是惯性配置，一般是需要计算出来的，惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 xacro宏 实现):

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</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

六、代码传送门

https://gitee.com/quartfee/ros_-slam/tree/master

实验结果及分析

通过本次ROS实验，跟着参考资料做，认识到了ros的神奇之处。实验过程中很少出现bug，因为目前只是简单的跟练，比较简单的操作，但是收获也很多。同时也学会了哪里不会点哪里的思维。因为B站视频太多集了，如果看完会很长时间。主要跟着文档做，不会的再看对应的视频。先不着急弄懂底层逻辑，提升兴趣后再去研究。