从头手搓一台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的插件按照用途大致可以分为两种：

用于控制的插件，通过插件可以控制机器人关节运动，可以进行位置、速度、力的控制，比如我们这节课的两轮差速控制器。
用于数据采集的插件，比如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

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

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