机器人系统仿真（URDF、Xacro、Arbotix、Gazebo）

ROS 0 次阅读 63212 字 约 316 分钟

文章目录

前言

马上开学，目前学校很多实验室都是人工智能这块，大部分都是和机器人相关，然后软件这块就是和cv、ros相关，就打算开始学习一下。

本章节是虚拟机安装Ubuntu18.04以及安装ROS的环境。

学习教程：【Autolabor初级教程】ROS机器人入门，博客中一些知识点是来源于赵老师的笔记在线笔记，本博客主要是做归纳总结，如有侵权请联系删除。

视频中的案例都基本敲了遍，这里给出我自己的源代码文件：

链接：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g 
提取码：0hws

所有博客文件目录索引：博客目录索引(持续更新)

一、认识机器人仿真

**机器人系统仿真：**是通过计算机对实体机器人系统进行模拟的技术，在 ROS 中，仿真实现涉及的内容主要有三:对机器人建模(URDF)、创建仿真环境(Gazebo)以及感知环境(Rviz)等系统性实现。

仿真相关组件：

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

简而言之：URDF 用于创建机器人模型，Gzebo 用于搭建仿真环境，Rviz 图形化的显示机器人各种传感器感知到的环境信息。

实际应用：三者应用中，只是创建 URDF 意义不大，一般需要结合 Gazebo 或 Rviz 使用，在 Gazebo 或 Rviz 中可以将 URDF 文件解析为图形化的机器人模型，一般的使用组合为

  • 如果非仿真环境，那么使用 URDF 结合 Rviz 直接显示感知的真实环境信息。
  • 如果是仿真环境，那么需要使用 URDF 结合 Gazebo 搭建仿真环境，并结合 Rviz 显示感知的虚拟环境信息。

二、URDF快速集成Rviz基本流程

2.1、快速集成案例

1、创建新的工程包，并创建相应的四个目录：

  • config：配置文件。
  • launch：launch启动文件。
  • meshes：机器人模型渲染文件。
  • urdf：存储urdf文件目录。

image-20220914152253896

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为07urdf_rviz的包
catkin_create_pkg --rosdistro melodic 07urdf_rviz urdf xacro

2、编写指定的urdf文件

demo01_helloworld.urdf

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

3、编写launch文件，集成URDF与Rviz节点

demo01_helloworld.launch：

<launch>
    <!-- 1、在参数服务器中载入urdf文件 -->
    <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/urdf/demo01_helloworld.urdf" />
    <!-- 2、启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

接着我们来运行下该launch启动文件：

roslaunch 07urdf_rviz demo01_helloworld.launch

image-20220914152933342

image-20220914152951192

最终效果：

image-20220914153022715

2.2、优化快速集成（存储rviz配置文件）

问题描述：在上面初始去启动rviz时，我们还需要去手动的添加机器人组件以及frame_id，若是每一次启动都需要这么修改，那就会很繁琐，能不能够通过一种方式我们重复打开时直接就已经配置好了呢？

解决方案：存储rviz文件，接着修改launch配置文件表示去进行读取，此时再去重新启动即可加载模型到rviz中，此时就能够简化机器人模型的显示。

image-20220914153707624

image-20220914153854578

image-20220914155815544

接着我们来优化一下launch配置文件，添加一个args参数表示在启动rviz节点时加载对应的rviz配置：

<launch>
    <!-- 1、在参数服务器中载入urdf文件 -->
    <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/urdf/demo01_helloworld.urdf" />
    <!-- 2、启动rviz -d 表示指定rviz文件来进行加载 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />
</launch>

接着我们再次启动看看：

roslaunch 07urdf_rviz demo01_helloworld.launch

image-20220914155945161

三、URDF配置文件语法详解

URDF官网文档配置

URDF 文件是一个标准的 XML 文件，在 ROS 中预定义了一系列的标签用于描述机器人模型，机器人模型可能较为复杂，但是 ROS 的 URDF 中机器人的组成却是较为简单，主要分为连杆(link标签) 与 关节(joint标签)。

标签如下：

  • robot 根标签，类似于 launch文件中的launch标签
  • link 连杆标签
  • joint 关节标签
  • gazebo 集成gazebo需要使用的标签

针对于gazebo标签，后期在使用 gazebo 仿真时，才需要使用到，用于配置仿真环境所需参数，比如: 机器人材料属性、gazebo插件等，但是该标签不是机器人模型必须的，只有在仿真时才需设置。

URDF语法详解：一个关节就是一个link，一个joint点去连接指定的两个关节并且进行指定。

3.1、认识robot标签

介绍：robot标签作为根标签，所有的 link 和 joint 以及其他标签都必须包含在 robot 标签内,在该标签内可以通过 name 属性设置机器人模型的名称。

属性：name: 指定机器人模型的名称。

子标签：其他标签如link、joint、gazebo。

3.2、认识link标签

介绍link标签

介绍：urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性，比如: 机器人底座、轮子、激光雷达、摄像头...每一个部件都对应一个 link, 在 link 标签内，可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性。

简而言之就是一个部件就可以是一个link。

属性：

  • name：连杆命名。

image-20220914160511082

子标签：

  • visual ---> 描述外观(对应的数据是可视的)
    • geometry 设置连杆的形状
      • 标签1: box(盒状)
        • 属性:size=长(x) 宽(y) 高(z)
      • 标签2: cylinder(圆柱)
        • 属性:radius=半径 length=高度
      • 标签3: sphere(球体)
        • 属性:radius=半径
      • 标签4: mesh(为连杆添加皮肤)
        • 属性: filename=资源路径(格式:package://<packagename>/<path>/文件)
    • origin 设置偏移量与倾斜弧度
      • 属性1: xyz=x偏移 y偏移 z偏移
    • metrial 设置材料属性(颜色)
      • 属性: name
      • 标签: color
        • 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
  • collision ---> 连杆的碰撞属性
  • Inertial ---> 连杆的惯性矩阵

link案例实操

image-20220914170444712

demo02_link.urdf：编写urdf模型

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <!-- 形状 -->
                <!-- 1、立方体 size：长宽高 -->
                <!-- <box size="0.5 0.3 0.1" /> -->
                <!-- 2、圆柱：radius半径，length为高度 -->
                <!-- <cylinder radius="0.1" length="2.0"  /> -->
                <!-- 3、球体：radius半径 -->
                <!-- <sphere radius="1" /> -->
                <!-- 4、加载自定义模型文件 -->
                <mesh filename="package://07urdf_rviz/meshes/autolabor_mini.stl" /> 
            </geometry>
            <!-- 偏移量
                xyz表示x轴、y轴、z轴
                rpy表示翻转、俯仰、偏航
             -->
            <origin xyz="0 0 0" rpy="1.57 0 1.57" />
            <!-- 颜色 -->
            <!-- rgba a表示不透明度，范围值为[0,1] -->
            <material name="car_color">
                <color rgba="0.5 0.2 0.3 0.5" /> 
            </material>
        </visual>
    </link>
</robot>
  • 对应meshes目录下的素材：simple-demo
  • 链接：https://pan.baidu.com/s/1A9EXGy5jX951qbvHMDc-qQ 提取码：nbxa

demo02_link.launch

<launch>
    <!-- 1、在参数服务器中载入urdf文件 -->
    <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/urdf/demo02_link.urdf" />
    <!-- 2、启动rviz -d 表示指定rviz文件来进行加载 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />
</launch>

启动launch节点：

roslaunch 07urdf_rviz demo02_link.launch

其他几个案例展示：

image-20220914162258089

额外

其中一个偏移量的实战案例就是将autolabor_mini.stl模型进行调整：

<!-- 核心修改的就是这个偏移量  -->
<origin xyz="0 0 0" rpy="1.57 0 1.57" />
  • xyz：水平，上下
  • rpy：翻滚、俯仰、弧度。

image-20220914170850520

origin标签的详细图示

image-20220914171202588

3.3、认识joint标签

介绍joint标签

介绍：urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制....,比如:安装在底座上的轮子可以360度旋转，而摄像头则可能是完全固定在底座上。

简而言之：joint标签就是将两个link组件来连接到一起的。

属性：目前只关注旋转关节及固定关节即可。

  • name ---> 为关节命名
  • type ---> 关节运动形式
    • continuous: 旋转关节，可以绕单轴无限旋转
    • revolute: 旋转关节，类似于 continues,但是有旋转角度限制
    • prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
    • planer: 平面关节，允许在平面正交方向上平移或旋转
    • floating: 浮动关节，允许进行平移、旋转运动
    • fixed: 固定关节，不允许运动的特殊关节

子标签：

  • parent(必需的) parent link的名字是一个强制的属性：
    • link:父级连杆的名字，是这个link在机器人结构树中的名字。
  • child(必需的) child link的名字是一个强制的属性：
    • link:子级连杆的名字，是这个link在机器人结构树中的名字。
  • origin
    • 属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
  • axis
    • 属性: xyz用于设置围绕哪个关节轴运动。

实操案例（长方体+摄像头，可旋转）

初步demo

image-20220914191138371

demo03_joint.urdf：

image-20220914180930854

  • 其中中对于要将小摄像头放置到底盘上如上图，所需要调整的joint标签中origin的参数。
<robot name="mycar">
    <!-- 底盘：长方体 -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.3 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="car_color">
                <color rgba="0.8 0.5 0 1" /> 
            </material>
        </visual>
    </link>
    <!-- 摄像机 -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="camera_color">
                <color rgba="0 0 1 1" /> 
            </material>
        </visual>
    </link>
    <!-- 关节 -->
    <joint name="camera2baselink" type="continous">
        <!-- 父级link -->
        <parent link="base_link" />
        <!-- 子级link -->
        <child link="camera" />
        <origin xyz="0.2 0 0.075" rpy="0 0 0" />
        <!-- 设置关节旋转参考的坐标轴 -->
        <axis xyz="0 0 1" />
    </joint>
</robot>

demo03_joint.launch：

<launch>
    <!-- 1、在参数服务器中载入urdf文件 -->
    <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/urdf/demo03_joint.urdf" />
    <!-- 2、启动rviz -d 表示指定rviz文件来进行加载 -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />
    <!--
        若是进行关联两个link，仅仅只配置上面两个文件：
            表现：设置头显示位置与颜色异常
            提示：No transform from [camera] to [base_link] 缺少camera 到base_link的坐标变换
            原因：rviz中显示URDF时，必须发布不同部件之间的 坐标系 关系
            解决：ROS中提供了关于机器人模型显示的坐标发布相关节点(两个)
    -->
    <!-- 添加关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
    <!-- 添加机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <!-- 可选:用于控制关节运动的节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

运行launch文件：

roslaunch 07urdf_rviz demo03_joint.launch

指定Fixed Frame为base_link，右边的是控制旋转节点（与launch文件中的joint_state_publisher_gui相关，需要手动安装下可见报错方案总结）

GIF

优化初步demo（令其水平摆放）

针对于现存的一个问题就是当前的长方体并不是水平在平面里的，而是如下图所示并没有完全水平：

image-20220914182446250

解决方案：在对应的urdf文件中新增一个link底盘，并且使用joint将底盘和基准面进行连接，即可实现在水平了。

  • 在展示的时候fixed frame指定为base_footprint。

image-20220914182202918

<!-- 底盘 -->
<link name="base_footprint">
    <visual>
        <geometry>
            <sphere radius="0.001" />
        </geometry>
    </visual>
</link>
<!-- 连接底盘和基准盘 -->
<joint name="base_link2base_footprint" type="fixed">
    <parent link="base_footprint" />
    <child link="base_link" />
    <origin xyz="0 0 0.05" />
</joint>

最终的效果：以base_footprint作为Fixed Frame

image-20220914191835220

完整的urdf文件代码

这里并没有中文注释，以免在launch文件时运行报错！

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.3 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="car_color">
                <color rgba="0.8 0.5 0 0.5" /> 
            </material>
        </visual>
    </link>
    <link name="camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="camera_color">
                <color rgba="0 0 1 0.5" /> 
            </material>
        </visual>
    </link>
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>
    <joint name="camera2baselink" type="continuous">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="0.12 0 0.07" rpy="0 0 0" />
        <axis xyz="0 0 1" />
    </joint>    
    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link" />
        <origin xyz="0 0 0.05" />
    </joint>
</robot>

报错方案解决（3个）

报错1：若是命令行出现报错'ascii' codec can't encode characters in position 463-464

UnicodeEncodeError: 'ascii' codec can't encode characters in position 463-464: ordinal not in range(128)
[joint_state_publisher-3] process has died [pid 4443, exit code 1, cmd /opt/ros/melodic/lib/joint_state_publisher/joint_state_publisher __name:=joint_state_publisher __log:=/home/rosmelodic/.ros/log/b38967c0-0acb-11eb-aee3-0800278ee10c/joint_state_publisher-3.log].
log file: /home/rosmelodic/.ros/log/b38967c0-0acb-11eb-aee3-0800278ee10c/joint_state_publisher-3*.log

解决方案：URDF文件中不要有中文注释。

报错2：缺少joint_state_publisher_gui依赖（在launch节点中运行时出现）

image-20220914181340867

解决方案：安装对应的publisher-gui包即可

sudo apt-get install ros-melodic-joint-state-publisher-gui

报错3：rviz中报错描述：No transform from [camera] to [base_link]。

image-20220914175030542

解决方案：在launch文件中添加关节状态、机器人发布状态节点

<!-- 添加关节状态发布节点 -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 添加机器人状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />

3.4、URDF实操案例

需求描述

创建一个四轮圆柱状机器人模型，机器人参数如下：

  • 底盘为圆柱状，半径 10cm，高 8cm。
  • 四轮由两个驱动轮和两个万向支撑轮组成：
    • 两个驱动轮半径为 3.25cm，轮胎宽度1.5cm。
    • 两个万向轮为球状，半径 0.75cm。
    • 底盘离地间距为 1.5cm(与万向轮直径一致)。

实现流程：

  1. 新建 urdf 文件，并与 launch 文件集成
  2. 搭建底盘
  3. 在底盘上添加两个驱动轮
  4. 在底盘上添加两个万向轮

实操

image-20220916135307436

  • show_mycar.rviz就是之前保存的rviz配置导出文件。

urdf文件编写（底盘、驱动轮、万向轮）

步骤一：底盘搭建

<robot name="mycar">
    <!-- 设置base_footprint -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>

    <!-- 添加底盘
            半径：0.1m
            高度：0.08m
            离地：0.015m
     -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.08" />
            </geometry>
        </visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
            <color rgba="0.8 0.3 0.1 0.5" />
        </material>
    </link>
    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link" />
        <origin xyz="0 0 0.055" />
    </joint>
</robot>

底盘搭建的细节及搭建效果如下所示：

对应origin的关节部分就是0.04+0.015=0.055

image-20220916123906010

效果：

image-20220916124130048

步骤二：编写驱动轮

<!-- 添加驱动轮 -->
<!--
        驱动轮是侧翻的圆柱（90°）
        参数
            半径: 0.0325 cm
            宽度: 0.015  cm
            颜色: 黑色 
        关节设置:
            x = 0
            y = 底盘的半径 + 轮胎宽度 / 2 = 0.1 + 0.015 / 2 = 0.1+0.0075=0.10075
            z = 离地间距 + 底盘长度 / 2 - 轮胎半径 = 0.015 + 0.08/2 - 0.0325 = 0.0225
            0.015
            axis = 0 1 0
    -->
<link name="left_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.0325" length="0.015" />
        </geometry>
        <origin xyz="0 0 0" rpy="1.5705 0 0" />
        <material name="black">
            <color rgba="0.0 0.0 0.0 1.0" />
        </material>
    </visual>
</link>
<joint name="left_wheel2base_link" type="continuous">
    <parent link="base_link" />
    <child link="left_wheel" />
    <origin xyz="0 0.1 -0.0225" />
    <axis xyz="0 1 0" />
</joint>
<link name="right_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.0325" length="0.015" />
        </geometry>
        <origin xyz="0 0 0" rpy="1.5705 0 0" />
        <material name="black">
            <color rgba="0.0 0.0 0.0 1.0" />
        </material>
    </visual>
</link>
<joint name="right_wheel2base_link" type="continuous">
    <parent link="base_link" />
    <child link="right_wheel" />
    <origin xyz="0 -0.1 -0.0225" />
    <axis xyz="0 1 0" />
</joint>

注意点说明：对于离地的偏移量最好是设置在joint关节上，而不是在本身物体上，如下图所示，若是我们设置在joint关节上，在rviz中添加TF坐标就能够很明显的查看之间的效果：

驱动轮我们是以绿色的y轴作为轮子的位置

对于r为0.0225的说明：

image-20220916132936264

image-20220916124401032

关于axis中y=1的说明：

image-20220916131303859

效果：

image-20220916131959024

定位依旧改成base_footprint，并且添加tf坐标和控制运动

GIF

步骤三：编写万向轮

添加万向轮：

image-20220916133303123

效果：

image-20220916134422736

GIF

编写launch文件

demo04_test.launch：

<launch>
    <!-- 1、将 urdf 文件内容设置进参数服务器 -->
    <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/urdf/demo04_test.urdf" />

    <!-- 2、启动 rivz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />

    <!-- 3、启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <!-- 4、启动图形化的控制关节运动节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />

</launch>

最后去进行编译运行节点：

source ./devel/setup.bash

roslaunch 07urdf_rviz demo04_test.launch

3.5、URDF工具（语法检查、结构生成）

在 ROS 中提供了一些工具来方便 URDF 文件的编写：

  • check_urdf命令可以检查复杂的 urdf 文件是否存在语法问题
  • urdf_to_graphiz命令可以查看 urdf 模型结构，显示不同 link 的层级关系

安装命令：sudo apt install liburdfdom-tools

1、check_urdf：语法检查

# 进入到urdf目录，来执行check检查命令
check_urdf demo04_test.urdf

image-20220916140318079

2、urdf_to_graphiz：结构查看

# 进入到urdf目录
urdf_to_graphiz demo04_test.urdf

此时就会在该目录下生成pdf文件：

image-20220916140639779

四、URDF优化—Xacro

4.1、认识Xacro

纯使用urdf存在的问题如下：

1、在设计关节的位置时，一些位置数据都是算好之后填上去的，那么如果部件的一些属性修改了，那么就需要重新来去计算。

2、在urdf中若是有一些重复的组件，我们需要反复的重复声明，不能够很好的进行复用。

在ros中给出了类似编程的优化方案，称之为：Xacro。

介绍：Xacro 是 XML Macros 的缩写，Xacro 是一种 XML 宏语言，是可编程的 XML。

原理：Xacro 可以声明变量，可以通过数学运算求解，使用流程控制控制执行顺序，还可以通过类似函数的实现，封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。

作用：较之于纯粹的 URDF 实现，可以编写更安全、精简、易读性更强的机器人模型文件，且可以提高编写效率。

4.2、快速集成使用Xacro

在当前的urdf目录下创建xacro文件：

image-20220916142019015

demo01_helloworld.urdf.xacro：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.0015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="base_link_length" value="0.08" />
    <xacro:property name="lidi_space" value="0.015" />

    <!-- 宏 -->
    <xacro:macro name="wheel_func" params="wheel_name flag" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>

        <!-- 3-2.joint -->
        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
            <origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>
    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />
</robot>

接着进入到xacro目录下执行编译命令：

# 打印Xacro解析内容到屏幕
rosrun xacro xacro demo01_helloworld.urdf.xacro

# 将Xacro解析内容到文件
rosrun xacro xacro demo01_helloworld.urdf.xacro > demo01_helloworld.urdf

此时就可以快速拿到urdf文件（根据相应的公式编译好的）：

image-20220916142147605

<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from demo01_helloworld.urdf.xacro   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="mycar">
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder length="0.0015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <!-- 3-2.joint -->
  <joint name="left2link" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
    <origin rpy="0 0 0" xyz="0 0.1 -0.0225"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder length="0.0015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <!-- 3-2.joint -->
  <joint name="right2link" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
    <origin rpy="0 0 0" xyz="0 -0.1 -0.0225"/>
    <axis xyz="0 1 0"/>
  </joint>
</robot>

4.3、语法介绍（属性、宏调用、文件包含）

在编写xacro文件时，在robot标签上一定要添加空间声明：

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

语法一：属性与算数运算

用途：用于封装 URDF 中的一些字段，比如: PAI 值，小车的尺寸，轮子半径 ....

# 属性定义
<xacro:property name="xxxx" value="yyyy" />

# 属性调用
${属性名称}

# 算数运算
${数学表达式}

语法二：宏

# 宏定义
<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">

    .....

    参数调用格式: ${参数名}

</xacro:macro>

# 宏调用
<xacro:宏名称 参数1=xxx 参数2=xxx/>

语法三：文件包含

# 不同部件单独封装为宏，接着来统一集成组合出机器人（不同组件之间可以互相进行属性调用）
<robot name="xxx" xmlns:xacro="http://wiki.ros.org/xacro">
      <xacro:include filename="my_base.xacro" />
      <xacro:include filename="my_camera.xacro" />
      <xacro:include filename="my_laser.xacro" />
      ....
</robot>

实际举例：

image-20220916142533705

image-20220916142633438

4.4、使用Xacro优化URDF练习案例（之前的3.4）

4.4.1、编写xacro文件

image-20220916150747576

  • demo02_car_base.urdf-z.xacro：这个文件末尾带-z的表示是带带有中文注释的。

demo02_car_base.urdf-z.xacro：

<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <xacro:property name="PI" value="3.141"/>
    <!-- 宏:黑色设置 -->
    <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" /> <!-- 离地间距 -->

    <!-- 底盘 -->
    <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>
    </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>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <!-- 驱动轮宏实现 -->
    <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>
      </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>
    </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: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>
      </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>
    </xacro:macro>

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

</robot>

demo02_car_base.urdf.xacro：不带注释的（对于python2.7版本我们在运行launch文件时想要使用xacro编译的能用就需要使用这个文件）

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

    <xacro:property name="PI" value="3.141"/>

    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>

    <xacro:property name="base_footprint_radius" value="0.001" />
    <xacro:property name="base_link_radius" value="0.1" /> 
    <xacro:property name="base_link_length" value="0.08" /> 
    <xacro:property name="earth_space" value="0.015" />


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


    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />

    <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>
      </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>
    </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: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>
      </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>
    </xacro:macro>

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

</robot>

4.4.2、编写launch文件

接着我们来去编写来launch配置文件，两种方式来进行运行urdf文件：

  • 方式一：先手动运行xacro编译得到的urdf文件，然后launch去直接引入urdf文件。
  • 方式二：直接在launch中配置xacro命令+xacro文件，实现自动化（推荐）。

demo05_car_base.launch：两种方式如下图注释即可

<launch>
    <!-- 1、将 urdf 文件内容设置进参数服务器 -->
    <!-- 方式一：采用传统urdf文件方式 -->
    <!-- <param name="robot_description" textfile="$(find 07urdf_rviz)/urdf/xacro/demo02_car_base.urdf" /> -->
    <!-- 方式二：采用xacro工具来进行编译xacro文件 -->
    <param name="robot_description" command="$(find xacro)/xacro  $(find 07urdf_rviz)/urdf/xacro/demo02_car_base.urdf.xacro" />

    <!-- 2、启动 rivz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />

    <!-- 3、启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <!-- 4、启动图形化的控制关节运动节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />

</launch>

4.4.3、编译及运行launch节点

编译命令：

rosrun xacro xacro demo02_car_base.urdf-z.xacro > demo02_car_base.urdf

# 将Xacro解析内容到文件
rosrun xacro xacro demo02_car_base.urdf.xacro > demo02_car_base.urdf

运行launch节点命令：

source ./devel/setup.bash

roslaunch 07urdf_rviz demo05_car_base.launch

image-20220916151345416

4.5、Xacro实操案例（基于之前3.4添加摄像头、雷达）

在当前案例中，我们对摄像头、雷达来进行单独封装文件，最后统一在car.launch里进行统一封装。

image-20220916153056142

  • 对应后面-z的都表示带有中文注释的。
  • camera表示的是摄像头组件、laser表示雷达组件。

摄像头组件

image-20220916153703197

最终引用的是没有中文注释的

demo02_car_camera.urdf-z.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  -->

    <!-- 摄像头关节以及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>
    </link>

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

demo02_car_camera.urdf.xacro：实际引入的组件，无中文注释

<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:property name="camera_length" value="0.01" />
    <xacro:property name="camera_width" value="0.025" /> 
    <xacro:property name="camera_height" value="0.025" />
    <xacro:property name="camera_x" value="0.08" />
    <xacro:property name="camera_y" value="0.0" />
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> 

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

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

雷达组件

image-20220916153724169

最终引入的是无中文注释的

demo02_car_laser.urdf-z.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  -->

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

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


    <!-- 雷达属性 -->
    <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  -->

    <!-- 雷达关节以及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>
    </link>

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

demo02_car_laser.urdf.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" /> 
    <xacro:property name="support_y" value="0.0" /> 
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> 

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

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


    <xacro:property name="laser_length" value="0.05" />
    <xacro:property name="laser_radius" value="0.03" />
    <xacro:property name="laser_x" value="0.0" /> 
    <xacro:property name="laser_y" value="0.0" /> 
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" />

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

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

编写launch文件（集成基础组件、摄像头、雷达）

image-20220916153825665

car.launch：

<launch>
    <!-- 1、载入urdf模型（这里是去使用xacro编译得到的） -->
    <param name="robot_description" command="$(find xacro)/xacro  $(find 07urdf_rviz)/urdf/xacro/car.urdf.xacro" />

    <!-- 2、启动 rivz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />

    <!-- 3、启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <!-- 4、启动图形化的控制关节运动节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />

</launch>

最终我们来进行编译执行launch文件：

source ./devel/setup.bash

roslaunch 07urdf_rviz car.launch

image-20220916153928689

五、学习Arbotix（Rviz中控制机器人模型运动）

5.1、认识与安装Arbotix

使用URDF 结合 rviz 可以创建并显示机器人模型。当前实现的只是静态模型，如何控制模型的运动呢？

  • 我们可以调用 Arbotix 实现此功能。

介绍：Arbotix 是一款控制电机、舵机的控制板，并提供相应的 ros 功能包，这个功能包的功能不仅可以驱动真实的 Arbotix 控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动。

  • 这个差速控制器在 arbotix_python 程序包中，完整的 arbotix 程序包还包括多种控制器，分别对应 dynamixel 电机、多关节机械臂以及不同形状的夹持器。

安装Arbotix

安装Arbotix：

方式一：命令行安装

# 语法：sudo apt-get install ros-<<VersionName()>>-arbotix
sudo apt-get install ros-melodic-arbotix

方式二：源码包安装

git clone https://github.com/vanadiumlabs/arbotix_ros.git

# 接着放入到当前工程目录下，调用 catkin_make 编译
catkin_make

5.2、Arbotix使用（实操结合4.5案例控制运动）

image-20220916160833039

步骤一：添加一个Arbotix配置文件

demo06_control.launch：

# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
          #类型: 差速控制器（依据速度差异控制）
       type: diff_controller,
       #参考坐标
       base_frame_id: base_footprint, 
       #两个轮子之间的间距
       base_width: 0.2,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制
       accel_limit: 1.0 
    }
}

步骤二：编写launch文件，并引入arbotix节点服务

demo06_control.launch：

<launch>

    <!-- 1、加载xacro文件 -->
    <param name="robot_description" command="$(find xacro)/xacro  $(find 07urdf_rviz)/urdf/xacro/car.urdf.xacro" />

    <!-- 2、启动 rivz -->
    <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find 07urdf_rviz)/config/show_mycar.rviz" />

    <!-- 3、启动机器人状态和关节状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

    <!-- 集成arbotix运动控制结点，并加载参数 -->
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find 07urdf_rviz)/config/control.yaml" command="load" />
        <!-- 表示是仿真环境 -->
        <param name="sim" value="true" />
    </node>

</launch>

ok，接着我们来尝试运行launch文件：

source ./devel/setup.bash

roslaunch 07urdf_rviz demo06_control.launch

image-20220916161935290

image-20220916162022584

在启动节点之后，我们可以使用rostopic list查看到当前有一个/cmd_vel的话题，接着我们要是想要机器人动起来就需要去发布控制话题命令：

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0"

test

  • 其中/odom的坐标是根据机器人根据自己的速度信息来进行自己推导原点在哪里得到的。

六、URDF集成Gazebo

6.1、URDF快速集成Gazebo

流程：

  1. 创建功能包，导入依赖项
  2. 编写 URDF 或 Xacro 文件
  3. 启动 Gazebo 并显示机器人模型

image-20220916185822808

步骤一：创建功能包

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为07urdf_gazebo的包 引入urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins
catkin_create_pkg --rosdistro melodic 07urdf_gazebo urdf xacro urdf xacro gazebo_ros gazebo_ros_control gazebo_plugins

步骤二：编写urdf文件（带z的是有注释的）

demo01_helloworld-z.urdf：

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="yellow">
                <color rgba="0.5 0.3 0.0 1" />
            </material>
        </visual>
        <!-- 1、设置碰撞参数 -->
        <!-- 如果是标准几何体，直接复制 visual 的 geometry 和 origin 即可 -->
        <collision>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <!-- 2、设置惯性矩阵 -->
        <inertial>
            <origin xyz="0 0 0" />
            <mass value="6" />
            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
        </inertial>
    </link>
    <!-- 设置Gazebo自己的颜色标签 -->
    <gazebo reference="base_link">
        <material>Gazebo/Red</material>
    </gazebo>

</robot>

demo01_helloworld.urdf：

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="yellow">
                <color rgba="0.5 0.3 0.0 1" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <origin xyz="0 0 0" />
            <mass value="6" />
            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
        </inertial>
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Red</material>
    </gazebo>

</robot>

步骤三：编写launch文件

demo01_helloworld.launch：

<launch>

    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" textfile="$(find 07urdf_gazebo)/urdf/demo01_helloworld.urdf" />

    <!-- 启动 gazebo（使用的是gazbo提供的空世界） -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- 在 gazebo 中显示机器人模型
        -urdf 加载的是 urdf 文件
        -model mycar 模型名称是 mycar
        -param robot_description 从参数 robot_description 中载入模型
        -x 模型载入的 x 坐标
        -y 模型载入的 y 坐标
        -z 模型载入的 z 坐标
     -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
    
</launch>

最后来执行ros的launch命令启动节点：

# 启动节点
roslaunch 07urdf_gazebo demo01_helloworld.launch

image-20220916190232169

6.2、URDF集成Gazebo配置详解

URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:

1、必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。

2、必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。

3、颜色设置，也需要重新使用 gazebo 标签使用reference标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项。

collision标签

机器人link是标准的几何体形状，和link的 visual 属性设置一致即可，可直接将geometry、origin复制过来。

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

圆柱惯性矩阵

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

立方体惯性矩阵

 <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 中出现抖动，移动等现象。

  • 所以针对于不同形状的，我们最好直接使用官方提供给出的相应xacro模板。

颜色设置标签

在 gazebo 中显示 link 的颜色，必须要使用指定的标签：

  • 再material标签中颜色可以设置为 Red Blue Green Black .....
<gazebo reference="link节点名称">
     <material>Gazebo/Blue</material>
</gazebo>

6.3、URDF集成Gazebo（在Gazebo中复现4.5中的机器人）

image-20220916195319098

封装惯性矩阵算法的 xacro 文件

head.xacro：之前在6.2中给出的官方各个形状组件实例进行整体封装入一个文件

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

重新编辑各个组件xacro文件（设置 collision inertial 以及 color 等参数）

下面贴出各个组件的xacro封装文件（带有注释的）

demo02_car_base.urdf.xacro：底盘、支撑轮、驱动轮

<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 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>
      <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>

demo02_car_camera.urdf.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>

demo02_car_laser.urdf.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>

编写launch文件集成Gazebo运行

demo02_gazebo.launch：

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find 07urdf_gazebo)/urdf/xacro/car.urdf.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />

    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

启动launch文件：

# 启动节点
roslaunch 07urdf_gazebo demo02_gazebo.launch

效果：

image-20220916201740046

报错解决

报错描述：UnicodeEncodeError: 'ascii' codec can't encode characters in position 454-463: ordinal not in range(128)

image-20220916194013412

问题描述：python2.7的原因。

解决方案：

进入到ros安装目录下的python2.7包中（/opt/ros/melodic/lib/python2.7/dist-packages），创建文件：sitecustomize.py

image-20220916194152045

#coding=utf8
import sys
reload(sys)
sys.setdefaultencoding('utf8')

6.4、Gazebo仿真环境搭建

6.4.1、添加内置组件创建仿真环境（引入world资源）

image-20220916203543028

将world资源防止到worlds目录下：

  • 链接：https://pan.baidu.com/s/1A9EXGy5jX951qbvHMDc-qQ 提取码：nbxa

demo03_evn.launch：接着我们编写launch文件，在加载gazebo时添加参数来引入world，如下第7行

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find 07urdf_gazebo)/urdf/xacro/car.urdf.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <!-- 额外添加加载模型 -->
        <arg name="world_name" value="$(find 07urdf_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

启动launch文件：

# 启动节点
roslaunch 07urdf_gazebo demo03_evn.launch

效果：

image-20220916203839001

6.4.2、自定义仿真环境

运行gazebo：

gazebo

保存的内容有两个如下：

1、保存绘制的环境

image-20220916205736565

点击: 左上角 file ---> Save (保存路径功能包下的: models)

然后 file ---> Exit Building Editor，保存的文件内容如下：

image-20220916205916867

2、保存word世界文件

image-20220916210002480

七、URDF、Gazebo与Rviz综合应用

三者关系：URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息，Gazebo 用于仿真，可以模拟外界环境，以及机器人的一些传感器。

如何在 Gazebo 中运行这些传感器，并显示这些传感器的数据(机器人的视角)呢？

通过Gazebo 模拟机器人的传感器，然后在 Rviz 中显示这些传感器感知到的数据。

  • 运动控制以及里程计信息显示
  • 雷达信息仿真以及显示
  • 摄像头信息仿真以及显示
  • kinect 信息仿真以及显示

7.1、运动控制与里程计显示（ros_control）

运动控制（ros_control）

基本流程介绍

运动控制基本流程:

  1. 已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加传动装置以及控制器
  2. 将此文件集成进xacro文件
  3. 启动 Gazebo 并发布 /cmd_vel 消息控制机器人运动

添加的运动控制部件：左右轮胎

image-20220917113227124

快速集成6.3案例中的demo（实现运动控制）

步骤一：添加两轮差速配置

image-20220916213707491

move-z.xacro：这里贴出有中文注释的配置

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

    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="left_wheel2base_link" />
    <xacro:joint_trans joint_name="right_wheel2base_link" />

    <!-- 控制器 -->
    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <rosDebugLevel>Debug</rosDebugLevel>
            <publishWheelTF>true</publishWheelTF>
            <robotNamespace>/</robotNamespace>
            <publishTf>1</publishTf>
            <publishWheelJointState>true</publishWheelJointState>
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <legacyMode>true</legacyMode>
            <leftJoint>left_wheel2base_link</leftJoint> <!-- 左轮 -->
            <rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 -->
            <wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
            <wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 -->
            <broadcastTF>1</broadcastTF>
            <wheelTorque>30</wheelTorque>
            <wheelAcceleration>1.8</wheelAcceleration>
            <commandTopic>cmd_vel</commandTopic> <!-- 运动控制话题 -->
            <odometryFrame>odom</odometryFrame> 
            <odometryTopic>odom</odometryTopic> <!-- 里程计话题 -->
            <robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 -->
        </plugin>
    </gazebo>

</robot>

tips：在move.xacro中需要绑定好对应的左轮、右轮以及对应的左右关节，与之前的xacro中配置的对应上

image-20220916213508796

步骤二：修改car.urdf.xacro配置

image-20220916213916173

include标签添加gazebo目录下的``car.urdf.xacro`：

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="demo02_car_base.urdf.xacro" />
    <xacro:include filename="demo02_car_camera.urdf.xacro" />
    <xacro:include filename="demo02_car_laser.urdf.xacro" />
    <xacro:include filename="../gazebo/move.xacro" />
</robot>

步骤三：运行6.4案例中编写的launch文件

image-20220916214100719

<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find 07urdf_gazebo)/urdf/xacro/car.urdf.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <!-- 额外添加加载模型 -->
        <arg name="world_name" value="$(find 07urdf_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

最后运行launch节点：

# 启动节点
roslaunch 07urdf_gazebo demo03_evn.launch

image-20220916214219805

如何控制小车运动呢？

方式一：通过向话题发送消息

# 目标话题/cmd_vel
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.2"

方式二：通过键盘控制teleop_twist_keyboard

# 安装teleop_twist_keyboard
sudo apt-get install ros-melodic-teleop-twist-keyboard

# 运行键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制效果如下图所示：

test

里程计显示（rviz）

介绍

在 Gazebo 的仿真环境中，机器人的里程计信息以及运动朝向等信息是无法获取的，可以通过 Rviz 显示机器人的里程计信息以及运动朝向

里程计：机器人相对出发点坐标系的位姿状态(X 坐标 Y 坐标 Z坐标以及朝向)。

使用

当小车运动起来之后，我们可以使用rviz来进行查看相应的里程信息。

demo03_evn_rviz.launch：

image-20220917105535596

<launch>
    <!-- 启动 rviz,这里有指定配置rviz文件，你需要提前先保存一份配置才能够成功启动！ -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 07urdf_gazebo)/config/show_mycar.rviz"/>

    <!-- 关节以及机器人状态发布节点 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

</launch>

接着我们来进行启动节点：

# 启动节点
roslaunch 07urdf_gazebo demo03_evn_rviz.launch

image-20220917105851918

最后我们来看一下里程信息的动图效果：

test

7.2、雷达信息仿真

目的

通过 Gazebo 模拟激光雷达传感器，并在 Rviz 中显示激光数据。

雷达仿真基本流程:

  1. 已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加雷达配置；
  2. 将此文件集成进xacro文件；
  3. 启动 Gazebo，使用 Rviz 显示雷达信息。

添加的雷达部件位置：

image-20220917113057988

实操

image-20220917112126293

步骤一：编写雷达组件

car.urdf.xacro：

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="demo02_car_base.urdf.xacro" />
    <xacro:include filename="demo02_car_camera.urdf.xacro" />
    <xacro:include filename="demo02_car_laser.urdf.xacro" />
    <!-- 运动控制 -->
    <xacro:include filename="../gazebo/move.xacro" />
    <!-- 雷达 -->
    <xacro:include filename="../gazebo/laser.xacro" />
</robot>

注意事项如下：

image-20220917110630173

步骤二：在总配置中引入雷达组件

car.urdf.xacro：

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="demo02_car_base.urdf.xacro" />
    <xacro:include filename="demo02_car_camera.urdf.xacro" />
    <xacro:include filename="demo02_car_laser.urdf.xacro" />
    <!-- 运动控制 -->
    <xacro:include filename="../gazebo/move.xacro" />
    <!-- 雷达 -->
    <xacro:include filename="../gazebo/laser.xacro" />
</robot>

最后我们就来一一运行三个组件分别是：gazebo、rviz以及键盘控制组件

# 启动gazebo
roslaunch 07urdf_gazebo demo03_evn.launch

# 启动rviz
roslaunch 07urdf_gazebo demo03_evn_rviz.launch

# 启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

image-20220917112530300

启动rviz之后，我们需要额外添加一个新的组件Laser Scan：添加要做的关键事情就是指定/scan主题

image-20220917111650433

接着来看效果：

test

test

7.3、摄像头信息仿真与显示

介绍

目的：通过 Gazebo 模拟摄像头传感器，并在 Rviz 中显示摄像头数据。

基本流程:

  1. 已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加摄像头配置；
  2. 将此文件集成进xacro文件；
  3. 启动 Gazebo，使用 Rviz 显示摄像头信息。

添加的摄像头组件位置：

image-20220917113344507

实操

image-20220917114457978

步骤一：编写摄像头组件

camera-z.xacro：

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- 被引用的link -->
  <gazebo reference="camera">
    <!-- 类型设置为 camara -->
    <sensor type="camera" name="camera_node">
      <update_rate>30.0</update_rate> <!-- 更新频率 -->
      <!-- 摄像头基本信息设置 -->
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <!-- 核心插件 -->
      <plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
</robot>

注意点：

image-20220917113539494

步骤二：整合摄像头组件

car.urdf.xacro·：

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="demo02_car_base.urdf.xacro" />
    <xacro:include filename="demo02_car_camera.urdf.xacro" />
    <xacro:include filename="demo02_car_laser.urdf.xacro" />
    <!-- 运动控制 -->
    <xacro:include filename="../gazebo/move.xacro" />
    <!-- 雷达 -->
    <xacro:include filename="../gazebo/laser.xacro" />
    <!-- 摄像头 -->
    <xacro:include filename="../gazebo/camera.xacro" />
</robot>

接着老样子，启动三个服务：

# 启动gazebo
roslaunch 07urdf_gazebo demo03_evn.launch

# 启动rviz
roslaunch 07urdf_gazebo demo03_evn_rviz.launch

# 启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

image-20220917113822543

在rviz中添加camera组件，并且订阅主题：/camera/image_raw

image-20220917114915238

效果展示：

test

7.4、深度相机仿真（kinect，点云数据）

7.4.1、Rviz显示kinect数据

介绍

目的：通过 Gazebo 模拟kinect摄像头，并在 Rviz 中显示kinect摄像头数据。

kinect摄像头仿真基本流程:

  1. 已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加kinect摄像头配置；
  2. 将此文件集成进xacro文件；
  3. 启动 Gazebo，使用 Rviz 显示kinect摄像头信息。

添加的摄像头组件位置：其实也可以直接替换之前的camera位置

image-20220917120352079

实操

image-20220917120437992

步骤一：编写深度相机配置

kinect.xacro：

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <gazebo reference="support">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>support</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>

</robot>
  • 同样要修改reference以及frameName要对应上之前的组件。
  • 在这个配置中，提供了多个topic主题信息发布，包含深度信息（depth/image_raw）以及rgb的信息（rgb/camera_info），还有点云（depth/points）。

步骤二：整个到之前的include配置文件

car.urdf.xacro：

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="head.xacro" />
    <xacro:include filename="demo02_car_base.urdf.xacro" />
    <xacro:include filename="demo02_car_camera.urdf.xacro" />
    <xacro:include filename="demo02_car_laser.urdf.xacro" />
    <!-- 运动控制 -->
    <xacro:include filename="../gazebo/move.xacro" />
    <!-- 雷达 -->
    <xacro:include filename="../gazebo/laser.xacro" />
    <!-- 摄像头 -->
    <xacro:include filename="../gazebo/camera.xacro" />
    <!-- 深度相机（借助支架） -->
    <xacro:include filename="../gazebo/kinect.xacro" />
</robot>

接着启动三个部分：

# 启动gazebo
roslaunch 07urdf_gazebo demo03_evn.launch

# 启动rviz
roslaunch 07urdf_gazebo demo03_evn_rviz.launch

# 启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

rviz使用的是之前的camera，我们来选择不同的主题即可

image-20220917120845203

为了能够让其自转，这里我们去发布一个位姿消息：

# 目标话题/cmd_vel
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.3"

效果展示：

/camera/depth/image_raw：深度

test

/camera/rgb/image_raw：rgb

test

7.4.2、Kinect点云数据显示（添加PointCloud2组件）

在rviz中添加PointCloud2，然后去订阅主题：/camera/depth/points

image-20220917122455749

效果展示：

test

若是出现点云投像显示错误，解决方案如下

image-20220917122041086

主要原因：在kinect中图像数据与点云数据使用了两套坐标系统，且两套坐标系统位姿并不一致。

1、在插件中为kinect设置坐标系，修改配置文件的<frameName>标签内容：

image-20220917122127570

<frameName>support_depth</frameName>

2、在启动rviz的launch文件中发布新设置的坐标系到kinect连杆的坐标变换关系

image-20220917122224003

<!-- 添加点云坐标系到kinect连杆坐标系的变换 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />

最终重启各个服务，即可解决！

参考资料

[1]. ERROR: cannot launch node of type [joint_state_publisher_gui/joint_state_publisher_gui]: joint_state

[2]. ROS启动gazebo出现UnicodeEncodeError: ‘ascii‘ codec can‘t encode character u‘\u5b8f‘ in position 492:……

ROSURDFXacroGazebo机器人建模

评论区请在客户端页面查看