从头手搓一台ros2复合机器人（带机械臂）

一.前言   

        大家好呀，从本小节开始我们就步入了仿真篇，主要对机器人仿真进行介绍与操作，当然仿真有优点也有缺陷，基于对此学习，我们可以对上几小节创建的小车模型模拟硬件的特性，

比如：

• 有多重，
• 有多大的惯性
• 重心在哪
• 碰撞边界在哪
• 关节的上下界限
• 其他的一些必要信息等等

让机器人在gazebo中显示出来，并且实现了在Gazebo中简单的搭建了一个环境，好了，话不多说，马上开始！

二.ros2学习

        前17小节内容可订阅往期博客

18 为机器人URDF模型注入物理属性

18.1 需要的物理信息

碰撞描述是物体的用于碰撞检测的包围形状。

内参用于描述物体的质量，惯性矩阵。

link的摩擦力。

18.1.1 碰撞检测

在机器人仿真中，我们要对物体之前是否接触，是否发生碰撞做检测，常用的检测方法比如包围盒，判断两个物体的包围盒是否相交来快速判断物体是否发生碰撞。

在URDF中，我们可以可以在link标签下添加collison子标签来对物体的形状进行描述。

collision可以包含的子标签如下：

• origin，表示碰撞体的中心位姿
• geometry，用于表示用于碰撞检测的几何形状
• material，可选的，描述碰撞几何体的材料(这个设置可以在gazebo仿真时通过view选项看到碰撞包围体的形状)

一个完整的collision标签实例如下：

    <collision>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.10"/>
      </geometry>
      <material name="blue">
          <color rgba="0.1 0.1 1.0 0.5" /> 
      </material>
    </collision>

18.1.2 旋转惯量

旋转惯量矩阵是用于描述物体的惯性的，在做动力学仿真的时候，这些参数尤为重要。

在URDF中我们可以通过在link下添加inertial子标签，为link添加惯性参数的描述。

intertial标签包含的子标签如下：

• mass，描述link的质量
• inertia，描述link的旋转惯量（该标签有六个属性值ixx\ixy\ixz\iyy\iyz\izz）

一个完整的inertial标签示例如下：

   <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.0122666" ixy="0" ixz="0" iyy="0.0122666" iyz="0" izz="0.02"/>
    </inertial>

关于intertial的属性设置，常见的几何体我们可以通过公式进行计算。

计算方法可以看小鱼老师的这篇文章-URDF仿真惯性参数不知道怎么配？快收藏，常见几何物体URDF分享。

18.1.3 摩擦力和刚性系数

在上节Fishbot的URDF中，前面的支撑轮主要起支撑作用，因为我们将其使用fixed标签固定到了base_link上，所以它无法转动。

我们可以要把这个轮子的摩擦力设置为0，让它直接在地上滑动即可

  <gazebo reference="caster_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="1000000.0" />
    <kd value="10.0" />
  </gazebo>

其中mu1,mu2代表摩擦力，kp,kd代表刚性系数。

 18.2 为上节的模型添加物理惯性

完全添加好的机器人URDF模型在这里：fishbot_gazebo.urdf

新建一个urdf文件，将添加好的机器人URDF模型代码放入

18.3 使用gazebo加载URDF

18.3.1 安装Gazebo插件

18.3.2 启动Gazebo并启动插件

通过下面的命令行来启动gazebo并加载ros2插件。

18.3.3 插件节点及其服务介绍

来看插件的节点，以及改节点为我们提供的服务有哪些

这个节点对外提供的服务有哪些

我们可以看到另外三个特殊服务：

• /spawn_entity，用于加载模型到gazebo中
• /get_model_list，用于获取模型列表
• /delete_entity，用于删除gazbeo中已经加载的模型

我们想要让gazebo显示出我们配置好的模型使用/spawn_entity来加载即可。

接着我们可以来请求服务来加载模型，先看一下服务的接口类型。

看到服务的请求内容包括：

• string name ，需要加载的实体的名称 (可选的)。
• string xml ，实体的XML描述字符串, URDF或者SDF。
• string robot_namespace ，产生的机器人和所有的ROS接口的命名空间，多机器人仿真的时候很有用。
• geometry_msgs/Pose initial_pose ，机器人的初始化位置
• string reference_frame ，初始姿态是相对于该实体的frame定义的。如果保持"empty"或"world"或“map”，则使用 gazebo的world作为frame。如果指定了不存在的实体，则会返回错误

18.3.4 调用服务加载模型

命令行输入rqt，在插件选项中选择Services->Service Caller,然后再下拉框选择/spawn_entity服务，即可看到下面的界面。

把我们的FishBot的URDF模型复制粘贴，放到xml中

此时再看Gazebo,一个机器人出现了

18.3.5 在不同位置加载多个机器人

修改rqt中的参数，增加一个命名空间，然后修改一个位置，让第二个机器人和第一个相距1m的地方生产，然后点击Call。

18.3.6 查询和删除机器人

利用rqt工具，我们再对另外两个服务接口进行请求。

查到了三个模型，一个大地，一个fishbot，一个fishbot_0。

我们接着尝试把fishbot_0删掉，选择删除实体，输入fishbot_0的名字，点击call

调用成功，观察gazebo发现机器人没了

18.3.7 将启动gazebo和生产fishbot写成launch文件

打开urdf工作空间，在src/fishbot_description/launch中添加一个gazebo.launch.py文件，我们开始编写launch文件来在gazebo中加载机器人模型。

编译运行

18.3.8 Gazebo仿真插件之两轮差速

通过配置两轮差速控制插件，让我们的机器人动起来,先介绍一下gazebo的插件

gazebo的插件按照用途大致可以分为两种：

1. 用于控制的插件，通过插件可以控制机器人关节运动，可以进行位置、速度、力的控制，比如我们这节课的两轮差速控制器。
2. 用于数据采集的插件，比如IMU传感器用于采集机器人的惯性，激光雷达用于采集机器人周围的点云信息。

当然上面两类插件功能也可以写到一个插件里，两轮差速插件就是一个二合一加强版

两轮差速插件用于控制机器人轮子关节的位置变化，同时该插件还会获取轮子的位置以及速度的信息的反馈，根据反馈的位置信息结合运动学模型即可计算出当前机器人的位姿（里程计）。

该插件的名称为：gazebo_ros_diff_drive

两轮差速控制器和Gazebo的关系

两轮差速控制器可以将轮子的目标转速发送给Gazebo，并从Gazebo获取到实际的速度和位置。

注意：发送给Gazebo是目标速度，反馈回来的是实际速度。目标不等于实际，比如轮子卡住了，无论你发什么目标速度，实际速度都是0。

看到该插件主要输入控制指令，主要输出里程计信息。

输入参数

该插件还提供了一些可以控制输出的选项，因为是仿真，所以还要告诉插件轮子对应的joint名称等信息，这样就有了下面这个参数表格：

控制指令

两轮差速控制器默认通过订阅话题cmd_vel来获取目标线速度和角速度。该话题的类型为：geometry_msgs/msg/Twist

线速度和角速度都包含在x、y、z，代表坐标系的三个方向上的对应速度。

两轮差速控制器收到这个话题数据后将其中的角速度和线速度转换上两个轮子的转动速度发送给Gazebo。

输出参数

1.里程计

里程计信息默认的输出话题为odom，其消息类型为：nav_msgs/msg/Odometry

可以看到其数据主要包含三个部分：

• header，表示该消息发布的时间
• pose，表示当前机器人位置和朝向
• twist，表示当前机器人的线速度和角速度

covariance，其代表协方差矩阵

2.里程计TF信息

3.左右轮子TF信息

在URDF中配置两轮差速模型

因为是给Gazebo的插件，所以在URDF中，我们需要使用<gazebo>进行配置，因为是要给gazebo配置插件，所有要在gazebo标签下添加plugin子插件。

编译测试

使用CLI工具看一下系统有哪些节点在运行

已经看到了插件订阅的的/cmd_vel和发布的/odom了

使用键盘控制fishbot

键盘控制工具，可以用下面的指令安装

这个功能包下有一个节点，这个节点会监听键盘的按键事件，然后发布cmd_vel话题，该话题被gazebo的两轮差速插件所订阅。所以我们就可以通过这个节点来控制fishbot。

如果你想让这个节点不是发布cmd_vel话题，而是别的，可以采用ROS2的话题重映射功能。 eg: ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=cmd_vel1

使用rqt显示速度数据

接着使用rqt将数据在rqt中可视化出来

选择Plugin->Visualization->Plot

在上方Topic输入/cmd_vel/linear/x，再输入/cmd_vel/angular/z，然后用键盘控制机器人移动

cmd_vel中的速度代表目标速度，接着我们显示一下当前速度（在odom.twist中）

添加好后点一下右上角刷新

在RVIZ2中显示模型及其轨迹

打开rviz2

1. 修改FixedFrame为odom
2. 添加插件，Add->Odometry->OK
3. 选择话题，Odometry->Topic->选/odom
4. 去除协方差显示，Odometry->Covariance>取消勾选
5. 键盘控制节点，点个U，原地转圈圈

虽然机器人的轨迹已经在RVIZ中显示出来了，但是并没有机器人的模型，也看不到轮子的转动，要发布机器人模型我们所使用的节点是robot_state_publisher,所以我们在gazebo.launch.py中加入这个节点，同时再加上rviz2的启动节点，最终的gazebo.launch.py内容如下：

保存编译启动

18.3.9 Gazebo仿真插件之IMU

1.惯性测量单元IMU介绍

        惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

IMU可以测量以下三组数据：

• 三维角度
• 三维加速度
• 三维角速度

2.Gazebo-IMU插件

仿真的IMU也是对应一个后缀为.so的动态链接库，使用下面的指令可以查看所有的动态链接库：

IMU对应的消息类型为sensor_msgs/msg/Imu

除了每个数据对应的三个协方差之外，每一个还都对应一个3*3的协方差矩阵。

3.配置IMU传感器

        为了更真实的模拟IMU传感器，我们需要给我们的仿真IMU传感器加点高斯噪声，高斯噪声只需要指定平均值和标准差两个参数即可，不过因为IMU传感器的特殊性，我们还需要给模型添加两个偏差参数，分别是 平均值偏差和标准差偏差。

下面是IMU传感器的URDF配置代码，IMU对应的插件库libgazebo_ros_imu_sensor.so

4.编译测试

CLI看话题

rqt可视化：

18.3.10 Gazebo仿真插件之激光雷达

1.激光雷达原理介绍

普通的单线激光雷达一般有一个发射器，一个接收器，发射器发出激光射线到前方的目标上，物品会将激光反射回来，然后激光雷达的接受器可以检测到反射的激光。

通过计算发送和反馈之间的时间间隔，乘上激光的速度，就可以计算出激光飞行的距离，该计算方法成为TOF（飞行时间法Time of flight，也称时差法）。

2.Gazebo激光雷达插件

激光雷达是属于射线类传感器，该类传感在在Gazebo插件中都被封装成了一个动态库libgazebo_ros_ray_sensor.so。

看一下LiDAR的话题消息接口sensor_msgs/msg/LaserScan

3.为FishBot添加雷达插件

需要在URDF添加以下内容

可以看到:

1. 雷达可以设置更新频率update_rate，这里设置为5
2. 雷达可以设置分辨率，设置为1，采样数量360个，最终生成的点云数量就是360
3. 雷达也有噪声，模型为gaussian
4. 雷达有扫描范围range，这里配置成0.12-3.5，0.015分辨率
5. 雷达的pose就是雷达的joint中位置的设置值

4.编译测试

CLI看话题

使用rviz2进行可视化激光雷达数据

添加和修改RVIZ2的如下：（通过LaserScan插件可以看到激光数据）

改完之后依然是看不到任何激光雷达的数据的，反看topic的echo出来的数据，不是0就是inf(无限大)，再看看gazebo你会发现，激光雷达并没有达到任何一个物体上。

我们可以手动的给激光雷达周围添加一下东西，点击Gazebo工具栏的正方体，圆球或者圆柱，随意放置几个到我们激光雷达的最大扫描半径内。

再看一下RVIZ2，把size改大了10倍0.01->0.1。

18.3.11 Gazebo仿真环境搭建

1.Gazebo的world介绍

world即世界，gazebo的world文件就是用于描述世界模型的，也就是环境模型。

Gazebo已经为我们准备了很多常用的物体模型，除了基础的圆球，圆柱，立方体外的，其实还有飞机、汽车、房子等。

好这些模型，需要我们手动下载，万幸的是小鱼老师已经帮我们封装成了一行代码下载

因为一共有281个模型，是逐一下载并解压到~/.gazebo/models/目录的。

此时再次打开终端，输入gazebo，把选项卡切换到Insert

在Insert选项卡下可以看到一个目录，以及目录下的模型名称，随着下载脚本的不断下载，这里的模型会越来越多。

2.通过建墙工具建立world

Gazebo左上角->Edit->Building Editor

接着可以看到这样一个编辑界面

点击左边的Wall,就可以在上方的白色区域进行建墙了

建完后还可以用选Add Color或者Add Texture，然后点击下方墙，给墙添加颜色或者纹理

按下鼠标滑轮可以旋转视角

我们也可以从已有地图画墙

打开Gazebo->Gazebo左上角->Edit->Building Editor->左下方选Import

点击Next

左边选尺寸对应关系，选择默认的，100像素/米。点击OK（需要手动将100改变一下才能点击OK），之后就可以用图片画墙了。

注意：导入完图片不会直接出来墙，图片只是提供了墙的大概位置，需要手动用墙再将边描一遍。

建完后点击File->Exit,在退出的弹框中选Save and Exit。

接着在Gazebo界面中就可以看到墙了，我们再手动添加几个物体，就可以用于下面的导航使用了。

添加完，接着点击File->SaveWorld，将文件保存到我们的fishbot_descrption的world下

3.启动时加载world

1 命令行加载World

加载world其实也很简单，可以先启动Gazebo，再手动的加载文件，也可以在Gazebo启动时加载：

2 在launch中加载World

修改launch文件，将上面的命令行写到gazebo.launch.py中

修改setup.py文件，让编译后将world文件拷贝到install目录下

编译测试