所用的学习链接:

【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程P271-277

【以上视频笔记见http://www.autolabor.com.cn/book/ROSTutorials/】

前文参考

ROS入门(五)——仿真机器人一(URDF+Rviz)

ROS入门(六)——仿真机器人二(Xacro+Rviz+Arbotix小车运动)

一、介绍
1.目标

1.编写封装了惯性矩阵算法的 xacro 文件
2.为机器人模型中的每一个 link 添加 collision 和 inertial 标签,并且重置颜色属性
3.在 launch 文件中启动 gazebo 并添加机器人模型

2.Gazebo与rviz的配置区别
这些区别在后面的实操里会在代码中添加相应注释

(1)urdf 中必须使用 collision 标签

因为既然是仿真环境,那么必然涉及到碰撞检测,collision 提供碰撞检测的依据。如果机器人link是标准的几何体形状,和link原本的 visual 属性设置一致即可。

(2)urdf 中必须使用 inertial 标签

此标签标注了当前机器人某个刚体部分的惯性矩阵,用于一些力学相关的仿真计算。惯性矩阵的设置需要结合link的质量与外形参数动态生成,标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 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>

原则上,除了 base_footprint 外,机器人的每个刚体部分都需要设置惯性矩阵,且惯性矩阵必须经计算得出,如果随意定义刚体部分的惯性矩阵,那么可能会导致机器人在 Gazebo 中出现抖动,移动等现象。 

(3)urdf 中的颜色设置

颜色也需要重新使用 gazebo 标签标注,因为之前的颜色设置为了方便调试包含透明度,仿真环境下没有此选项。在 gazebo 中显示 link 的颜色,必须要使用指定的标签

<gazebo reference="link节点名称">
    <material>Gazebo/Blue</material>
</gazebo>

material 标签中,设置的值区分大小写,颜色可以设置为 Red Blue Green Black ... 

(4)launch文件区别

原本的 pkg = " rviz " ,现在的 pkg = " gazebo_ros " ,并调整一些相关参数

二、实操流程
1.创建功能包,导入依赖和配置
(1)创建功能包

在原本 工作空间(7.19_demo01) / src 目录 → 右键src → 新建一个 catkin package→ 自定义命名【urdf_gazebo】,导入dependences  :

urdf xacro gazebo_ros gazebo_ros_control gazebo_plugins
(2)创建文件夹

在urdf_gazebo目录下创建三个文件夹: urdf (存放urdf或xacro文件)、worlds(存放gazebo仿真环境)、launch

2.xacro文件编写
xacro文件包含五个:

封装惯性矩阵算法的 t1_head.xacro
底盘+轮子 t2_car.xacro
摄像头 t3_camera.xacro
雷达 t4_laser.xacro
组合t123和head的 t5_1234combine.xacro
(1)封装惯性矩阵算法的 t1_head.xacro 文件

在urdf 目录下新建一个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>

(2)底盘+轮子 t2_car.xacro 文件

<!-- 根标签,必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <!-- PI 值设置精度需要高一些,否则后续车轮翻转量计算时,可能会出现肉眼不能察觉的车轮倾斜,从而导致模型抖动 -->
    <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>
        <!-- 与rviz不同(1):gazebo的urdf或xacro文件中必须设置碰撞 -->
        <collision>
            <geometry>
                <cylinder radius="${base_link_radius}" length="${base_link_length}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
        </collision>
        <!-- 与rviz不同(2):使用在head.xacro中封装过的inertial标签 -->
        <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">
        <!-- 与rviz不同(3):重新设置颜色 -->
        <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>

(3)摄像头 t3_camera.xacro 文件

<!-- 摄像头相关的 xacro 文件 -->
<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>

(4)雷达 t4_laser.xacro 文件

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

(5)组合前面xacro的 t5_1234combine.xacro 文件

<!-- 组合惯性矩阵文件、小车、摄像头和雷达 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="t1_head.xacro" />
    <xacro:include filename="t2_car.xacro" />
    <xacro:include filename="t3_camera.xacro" />
    <xacro:include filename="t4_laser.xacro" />
</robot>

3.素材下载

素材链接:https://github.com/zx595306686/sim_demo.git

(1)下载

打开一个终端,输入命令和素材链接,将会下载素材

git clone https://github.com/zx595306686/sim_demo.git

在运行代码所在的文件夹下生成一个 sim_demo 文件夹

(2)复制box_house.world文件

打开 sim_demo 文件夹,下面有一个box_house.world 文件,将其复制到下面目录底下

# 工作空间/src/urdf_gazebo/worlds
7.19_demo01/src/urdf_gazebo/worlds

4.launch文件编写

  • t5_xacro.launch
<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf_gazebo)/urdf/t5_1234combine.xacro" />
    <!-- 启动 gazebo -->
 
    <!-- 加载仿真环境 -->
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch" /> -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find urdf_gazebo)/worlds/box_house.world"></arg>
    </include>
 
    <!-- 在 gazebo 中显示机器人模型 -->
    <!-- 与rviz不同(4):launch-->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
</launch>

如果直接调用<include file="$(find gazebo_ros)/launch/empty_world.launch" />则会加载一个空环境。

5.运行

(1)ctrl+shift+b编译

(2)新终端1

roscore

(2)新终端2

roslaunch urdf_gazebo t5_xacro.launch