ROS进阶(二):移动机器人项目实战

声明:ROS进阶教程参考了《ROS机器人开发实践》,由胡春旭老师所著。此系列教程唯一目的是学习与分享,与大家共勉。若感兴趣,请大家购买原版书籍进行学习,侵权删。

本文在Ubuntu18.04 + ROS melodic环境下完成,其他ROS版本类似。

项目github链接:

一、项目介绍

1、项目内容

从零开始创建一个移动机器人,并使用rviz软件以及gazebo软件进行仿真环境的搭建与可视化,还能在仿真环境中控制机器人移动。

2、预期目标

  • 学会创建并优化小车模型。
  • 学会使用rviz+ArbotiX构建运动仿真器。
  • 掌握ros_control基本用法。
  • 学会使用gazebo进行仿真。

二、创建机器人的urdf模型

1、创建机器人描述功能包

$ catkin_create_pkg mrobot_description urdf xarco

mrobot_description功能包中包含urdf、meshes、launch和config四个文件夹。

  • urdf:用于存放机器人模型的urdf或xacro文件。
  • meshes:用于放置urdf中引用的模型渲染文件。
  • launch:用于保存相关启动文件。
  • config:用于保存rviz的配置文件。

2、创建urdf模型

在ROS入门阶段,我们已经了解了urdf常用的标签和语法,接下来使用这些基本语法创建一个如下图所示的机器人模型。

这个机器人底盘模型有7个link和6个joint。7个link包括1个机器人底板、2个电机、2个驱动轮和2个万向轮;6个joint负责将驱动轮、万向轮、电机安装到底板上,并设置相应的连接方法。

先来看一下mrobot_description/urdf/mrobot_chassis.urdf的具体内容:

<?xml version="1.0" ?>
<robot name="mrobot_chassis">

    <link name="base_link">
        <visual>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.005" radius="0.13"/>
            </geometry>
            <material name="yellow">
                <color rgba="1 0.4 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="base_left_motor_joint" type="fixed">
        <origin xyz="-0.055 0.075 0" rpy="0 0 0" />        
        <parent link="base_link"/>
        <child link="left_motor" />
    </joint>

    <link name="left_motor">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.02" length = "0.08"/>
            </geometry>
            <material name="gray">
                <color rgba="0.75 0.75 0.75 1"/>
            </material>
        </visual>
    </link>

    <joint name="left_wheel_joint" type="continuous">
        <origin xyz="0 0.0485 0" rpy="0 0 0"/>
        <parent link="left_motor"/>
        <child link="left_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.033" length = "0.017"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

    <joint name="base_right_motor_joint" type="fixed">
        <origin xyz="-0.055 -0.075 0" rpy="0 0 0" />        
        <parent link="base_link"/>
        <child link="right_motor" />
    </joint>

    <link name="right_motor">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.02" length = "0.08" />
            </geometry>
            <material name="gray">
                <color rgba="0.75 0.75 0.75 1"/>
            </material>
        </visual>
    </link>

    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="0 -0.0485 0" rpy="0 0 0"/>
        <parent link="right_motor"/>
        <child link="right_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.033" length = "0.017"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>

    <joint name="front_caster_joint" type="fixed">
        <origin xyz="0.1135 0 -0.0165" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="front_caster_link"/>
    </joint>

    <link name="front_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0"/>
            <geometry>
                <sphere radius="0.0165" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.95"/>
            </material>
        </visual>
    </link>

</robot>

urdf提供了一些命令行工具,可以帮助我们检查、梳理模型文件,需要在终端中独立安装:

$ sudo apt-get install liburdfdom-tools

然后使用check_urdf命令对mrobot_chassis.urdf文件进行检查:

$ check_urdf mrobot_chassis.urdf

check_urdf命令会解析urdf文件,并且显示解析过程中发现的错误;还可以使用urdf_to_graphiz命令查看urdf模型的整体结构:

$ urdf_to_graphiz mrobot_chassis.urdf

执行urdf_to_graphiz命令后,会在当前目录下生成一个pdf文件,打开该文件,可以看到模型的整体结构图。

3、urdf模型解析

针对上面创建的urdf模型,下面将对其关键部分进行解析。

<?xml version="1.0" ?>
<robot name="mrobot_chassis">

首先需要声明该文件使用XML描述,然后用<robot>根标签定义一个机器人模型,并定义该机器人模型的名称是“mrobot_chassis”。根标签内的内容即为对机器人模型的详细定义。

    <link name="base_link">
        <visual>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.005" radius="0.13"/>
            </geometry>
            <material name="yellow">
                <color rgba="1 0.4 0 1"/>
            </material>
        </visual>
    </link>

这一段代码用来描述机器人的底盘link,<visual>标签用来定义底盘的外观属性,在显示和仿真中,rviz或gazebo会按照这里的描述将机器人模型呈现出来。我们将机器人底盘抽象成一个圆柱结构,使用<cylinder>标签定义这个圆柱的半径和高;然后声明这个底盘圆柱在空间内的三维坐标位置和旋转姿态,底盘中心位于界面的中心点,所以使用<origin>设置起点坐标为界面的中心坐标。此外,使用<material>标签设置机器人底盘的颜色——黄色,其中<color>便是黄色的RGBA值。

RGBA代表由Red(红)Green(绿)Blue(蓝)和Alpha(阿尔法通道,图像透明程度有关的参数,一般取1),组成的色彩空间。
    <joint name="base_left_motor_joint" type="fixed">
        <origin xyz="-0.055 0.075 0" rpy="0 0 0" />        
        <parent link="base_link"/>
        <child link="left_motor" />
    </joint>

这一段代码定义了第一个关节joint,用来连接机器人底盘和左边驱动电机,joint的类型是fixed类型,表明joint是固定的,不允许关节发生运动。<origin>标签定义了joint的起点,我们将起点设置在需要安装电机的底盘位置。<parent>标签定义了父连杆是base_link,<child>标签定义了子两岸是left_motor,他们共同表明了关节与连杆的连接关系。

    <link name="left_motor">
        <visual>
            <origin xyz="0 0 0" rpy="1.5707 0 0" />
            <geometry>
                <cylinder radius="0.02" length = "0.08"/>
            </geometry>
            <material name="gray">
                <color rgba="0.75 0.75 0.75 1"/>
            </material>
        </visual>
    </link>

上面这一段代码描述了左侧电机的模型。电机的外在形象抽象成圆柱体,圆柱体的半径为0.02m,高为0.08m。其他的描述和机器人底盘类似,定义了电机中心的起点位置和外观颜色。

之前定义了一个joint将电机连接到底盘上,电机的坐标位置是相对于joint计算的。在joint的位置坐标设置中,我们已经将joint放置在安装电机的底盘位置,所以在电机模型的坐标设置中,不需要位置偏移,放置到 (0, 0, 0)坐标即可。由于圆柱体默认是垂直地面创建的,需要把圆柱体绕x轴旋转90° (使用弧度表示大约为0.75),才能成为电机的模样。
    <joint name="left_wheel_joint" type="continuous">
        <origin xyz="0 0.0485 0" rpy="0 0 0"/>
        <parent link="left_motor"/>
        <child link="left_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

接着定义电机和轮子之间的joint。joint的类型是continuous型的,这种类型的joint可以围绕一个轴进行旋转,很适合轮子这种模型。<origin>标签定义了joint的起点,将起点设置到安装轮子的位置,即电机的一端。<axis>标签定义该joint的旋转轴是正y轴,轮子在运动是就会绕着y轴旋转。

机器人底盘模型的其他部分都采用类似的方式,不再赘述,并建议大家动手从无到有写一个urdf文件,才能更好地理解每一个标签的含义、标签内元素的用法。

4、在rviz中显示模型

完成urdf模型的设计后,可以使用rviz将该模型可视化显示出来,检查是否符合设计目标。

在mrbot_description功能包launch文件夹中已经创建用于显示mrbot_chassis模型的launch文件mrbot_description/launch/display_mrobot_chassis_urdf.launch,详细内容如下:

<launch>
	<param name="robot_description" textfile="$(find mrobot_description)/urdf/mrobot_chassis.urdf" />

	<!-- 设置GUI参数,显示关节控制插件 -->
	<param name="use_gui" value="true"/>
	
	<!-- 运行joint_state_publisher节点,发布机器人的关节状态  -->
	<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
	
	<!-- 运行robot_state_publisher节点,发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
	
	<!-- 运行rviz可视化界面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot_urdf.rviz" required="true" />
</launch>

打开终端运行该launch文件,如果一切正常,可以在rviz中看到如下图所示的机器人模型。

$ sudo apt-get install ros-melodic-joint-state-publisher-gui
$ roslaunch mrbot_description display_mrobot_chassis_urdf.launch

运行成功后,不仅启动了rviz,而且出现了一个名为“joint_state_publisher_gui”的GUI。这是因为我们在启动文件中启动了joint_state_publisher。该节点可以发布每个joint的状态,而且可以通过UI对joint进行控制。所以在控制界面中用鼠标滑动控制条,rviz中对应的轮子就会开始转动。

除了joint_state_publisher,launch文件还会启动一个名为“robot_state_publisher”的节点,这两个节点的名称相似,所以很多开发者会把两者弄混淆。与joint_state_publisher不同,robot_state_publisher节点的功能是将机器人各个link、joint之间的关系,通过TF的形式整理成三维姿态信息发布出去。在rviz中,可以选择添加TF插件来显示各部分的坐标系。

5、改进urdf模型

到目前为止,我们创建的机器人模型还非常简陋,仅可以在rviz中可视化显示,如果要将其放入仿真环境中,还需要进一步改进。

(1)添加物理和碰撞属性

在之前的模型中,我们仅创建了模型外观的可视化属性,除此之外,还需要添加物理属性和碰撞属性。这里以机器人底盘base_link为例,介绍如何添加这些属性。

在base_link中加入<inertial>和<collision>标签,描述机器人的物理属性和碰撞属性:

    <link name="base_link">
        <inertial>
            <mass value="2" />
            <origin xyz="0 0 0.0" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0" izz="0.5" />
        </inertial>

        <visual>
            <origin xyz=" 0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
            </geometry>
            <material name="yellow" />
        </visual>

        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
            </geometry>
        </collision>
    </link>

其中,惯性参数的设置主要包含质量和惯性矩阵。如果是规则物体,可以通过尺寸、质量等公式计算得到惯性矩阵,在这里我们使用一组虚拟的惯性矩阵数据。<collision>标签中的内容和<visual>标签中的内容几乎一致,这是因为我们使用的模型都是较为简单的规则模型,如果使用真实机器人的设计模型,<visual>标签内可以显示复杂的机器人外观,但是为了减少碰撞检测时的计算量,<collision>中往往使用简化后的机器人模型,例如将机械臂的一根连杆简化为圆柱体或长方体。

(2)使用xacro优化urdf

回顾我们创建的模型,可以发现模型文件十分冗杂,其中很多内容除了参数,几乎都是重复的。但是urdf文件不支持代码复用的特性,如果为一个复杂的机器人建模,那urdf模型将十分的复杂。

针对这个问题,urdf模型产生了另外一种精简化、可复用、模块化的描述形式——xacro,它具备以下优势:

  • 精简模型代码:在xacro文件中,可以通过创建宏定义的方式定义常量或者复用代码,不仅可以减少代码量,而且可以让模型代码更加模块化、更具可读性。
  • 提供可编程接口:xacro语法支持一些可编程接口,如常量、变量、数学公式、条件语句等,可以让建模过程更加智能有效。

xacro是urdf的升级版,模型文件的后缀名由.urdf变为.xacro,而且在模型<robot>标签中要加入xacro声明:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

(3)xacro常用语法

  • 使用常量定义

在之前的urdf模型中有很多尺寸、坐标等常量使用,但是这些常量分布在整个文件中,不仅可读性差,而且后期修改起来十分困难。xacro提供了一种常量属性的定义方式:

<xacro:property name="M_PI" value="3.14159"/>

当需要使用该常量时,使用如下语法调用即可:

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

现在,各种参数的定义都可以使用常量定义的方式进行声明:

<xacro:property name="M_PI" value="3.14159"/>
<xacro:property name="wheel_radius" value="0.033"/>
<xacro:property name="wheel_length" value="0.017"/>
<xacro:property name="base_link_radius" value="0.13"/>
<xacro:property name="base_link_length" value="0.005"/>
<xacro:property name="motor_radius" value="0.02"/>
<xacro:property name="motor_length" value="0.08"/>
<xacro:property name="motor_x" value="-0.055"/>
<xacro:property name="motor_y" value="0.075"/>
<xacro:property name="plate_height" value="0.07"/>
<xacro:property name="standoff_x" value="0.12"/>
<xacro:property name="standoff_y" value="0.10"/>

如果改动机器人模型,只需要修改这些参数即可,十分方便。

  • 调用数学公式

在“${}”语句中,不仅可以调用常量,还可以用一些常用的数学运算,包括加、减、乘、除、负号、括号等,例如:

<origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy="0 0 0"/>
备注:所有数学运算都会转换成浮点数进行,以保证运算精度。

(4)使用宏定义

xacro文件可以使用宏定义来声明重复使用的代码模块,而且可以包含输入参数,类似编程中的函数概念。例如,在MRobot底盘上还有两层支撑板,支撑板之间共需要八根支撑柱,支撑柱模型是一样的,只是位置不同,如果用urdf文件描述共需要实现8次。在xacro中,这种相同的模型就可以通过定义一种宏定义模块的方式来重复使用。

    <xacro:macro name="mrobot_standoff_2in" params="parent number x_loc y_loc z_loc">
        <joint name="standoff_2in_${number}_joint" type="fixed">
            <origin xyz="${x_loc} ${y_loc} ${z_loc}" rpy="0 0 0" />
            <parent link="${parent}"/>
            <child link="standoff_2in_${number}_link" />
        </joint>

        <link name="standoff_2in_${number}_link">
            <inertial>
                <mass value="0.001" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
                         iyy="0.0001" iyz="0.0"
                         izz="0.0001" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.01 0.07" />
                </geometry>
                <material name="black">
                    <color rgba="0.16 0.17 0.15 0.9"/>
                </material>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.01 0.07" />
                </geometry>
            </collision>
        </link>
    </xacro:macro>

以上宏定义中包含五个输入参数:joint的parent link,支撑住的序号number,支撑柱在x、y、z三个方向上的偏移。需要该宏模块时,使用如下语句调用,设置输入参数即可:

<mrobot_standoff_2in parent="base_link" number="1" x_loc="-${standoff_x/2 + 0.03}" y_loc="-${standoff_y - 0.03}" z_loc="${plate_height/2}"/>
<mrobot_standoff_2in parent="base_link" number="2" x_loc="-${standoff_x/2 + 0.03}" y_loc="${standoff_y - 0.03}" z_loc="${plate_height/2}"/>
<mrobot_standoff_2in parent="base_link" number="3" x_loc="${standoff_x/2}" y_loc="-${standoff_y}" z_loc="${plate_height/2}"/>
<mrobot_standoff_2in parent="base_link" number="4" x_loc="${standoff_x/2}" y_loc="${standoff_y}" z_loc="${plate_height/2}"/>
<mrobot_standoff_2in parent="standoff_2in_1_link" number="5" x_loc="0" y_loc="0" z_loc="${plate_height}"/>
<mrobot_standoff_2in parent="standoff_2in_2_link" number="6" x_loc="0" y_loc="0" z_loc="${plate_height}"/>
<mrobot_standoff_2in parent="standoff_2in_3_link" number="7" x_loc="0" y_loc="0" z_loc="${plate_height}"/>
<mrobot_standoff_2in parent="standoff_2in_4_link" number="8" x_loc="0" y_loc="0" z_loc="${plate_height}"/>

6、xacro文件引用

改进后的机器人模型文件是mrobot_description/urdf/mrobot.urdf.xacro,详细内容如下:

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mrobot_description)/urdf/mrobot_body.urdf.xacro" />

    <!-- MRobot机器人平台 -->
    <mrobot_body/>

</robot>

<robot>标签只有两行代码:

<xacro:include filename="$(find mrobot_description)/urdf/mrobot_body.urdf.xacro" />

第一行代码描述该xacro文件所包含的其他xacro文件,类似于C语言中的include文件。声明包含关系后,该文件就可以使用被包含文件中的模块了。

<mrobot_body/>

第二行代码就调用了被包含文件mrobot_body.urdf.xacro中的机器人模型宏定义,类似于实例化对象。也就是说,机器人的模型文件全部是在mrobot_body.urdf.xacro中使用一个宏来描述的,那么为什么还需要mrobot.urdf.xacro来调用呢?

这是因为我们把机器人本体看作是一个模块,如果需要其他模块集成,使用这种方法就不需要需改机器人的模型文件,只需要在上层实现一个瓶装模块的顶层文件即可,灵活性强。比如后续在机器人上装配camera、Kinect、rplidar,只需要修改这里的mrobot.urdf.xacro即可。

7、显示优化后的模型

xacro文件设计完成后,可以通过两种方式将优化后的模型显示在rviz中:

  • 将xacro文件转换成urdf文件
$ rosrun xacro xacro mrobot.urdf.xacro > mrobot.urdf

当前目录下会生成一个转化后的urdf 文件,然后使用上面介绍的launch文件可将该urdf模型显示在rviz中,launch文件中需要修改如下代码:

<param name="robot_description" textfile="$(find mrobot_description)/urdf/mrobot.urdf" />
  • 直接调用xacro文件解析器

也可以省略手动转换模型的过程,直接在启动文件中调用xacro解析器,自动将xacro转换成urdf文件,该过程可以在launch文件中使用如下语句进行配置:

<arg name="model" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot.urdf.xacro'" />
<param name="robot_description" command="$(arg model)" />

在终端中运行修改后的launch文件mrbot_description/launch/display_mrobot.launch,即可启动rviz并看到优化后的机器人模型:

$ roslaunch mrobot_description display_mrobot.launch

三、添加传感器模型

上一节创建的机器人模型相比之前创建的urdf模型又复杂一些,机器人底盘上安装了八根支撑柱,架起了两层支撑板,可以在这些支撑板上放置电池、控制板、传感器等硬件设备。通常室内移动机器人会装配彩色摄像头、RGB-D摄像头、激光雷达等传感器,我们可以在虚拟世界中仿真这一切。

1、添加摄像头

首先创造一个摄像头模型。我们以一个长方体代表摄像头模型,对应的模型文件是mrobot_description/urdf/camera.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">

    <xacro:macro name="usb_camera" params="prefix:=camera">
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
                <material name="black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
            </collision>
        </link>
    </xacro:macro>

</robot>

以上代码中使用了一个名为usb_cameara的宏来描述摄像头,输入参数是摄像头的名称,宏中包含了表示摄像头长方体link的参数。

然后,还需要创建一个顶层xacro文件,把机器人和摄像头这两个模块拼装在一起。顶层xacro文件mrobot_description/urdf/mrobot_with_camera.urdf.xacro内容如下:

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mrobot_description)/urdf/mrobot_body.urdf.xacro" />
    <xacro:include filename="$(find mrobot_description)/urdf/camera.xacro" />

    <xacro:property name="camera_offset_x" value="0.1" />
    <xacro:property name="camera_offset_y" value="0" />
    <xacro:property name="camera_offset_z" value="0.02" />

    <!-- MRobot机器人平台-->
    <mrobot_body/>

    <!-- Camera -->
    <joint name="camera_joint" type="fixed">
        <origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
        <parent link="plate_2_link"/>
        <child link="camera_link"/>
    </joint>

    <xacro:usb_camera prefix="camera"/>
</robot>

在这个顶层xacro文件中,包含了描述摄像头的模型文件,然后使用一个fixed类型的joint把摄像头固定到机器人顶部支撑板靠前的位置。

运行如下命令,在rviz中查看安装有摄像头的机器人模型:

$ roslaunch mrobot_description display_mrobot_with_camera.launch

这个简单的长方体可不简单!当我们在gazebo中仿真时,我们可以调用gazebo插件实现摄像头的功能。另外,我们也可以在Solidworks等软件中创建更加形象、具体的传感器模型,然后转换成urdf模型格式装配到机器人上。

2、添加Kinect

Kinect是一种常用的RGB-D摄像头,三维模型文件kinect.dae可以在TurtleBot功能包中找到。Kinect模型描述文件mrobot_description/urdf/kinect.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">

    <xacro:macro name="kinect_camera" params="prefix:=camera">
        <link name="${prefix}_link">
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
                <geometry>
                    <mesh filename="package://mrobot_description/meshes/kinect.dae" />
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <box size="0.07 0.3 0.09"/>
                </geometry>
            </collision>
        </link>

        <joint name="${prefix}_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
            <parent link="${prefix}_link"/>
            <child link="${prefix}_frame_optical"/>
        </joint>

        <link name="${prefix}_frame_optical"/>
    </xacro:macro>

</robot>

在可视化设置中使用<mesh>标签可以导入该模型的mesh文件,<collision>标签中可将模型简化为一个长方体,精简碰撞检测的数学计算。

然后将Kinect和机器人拼装到一起,顶层xacro文件mrobot_description/urdf/mrobot_
with_kinect.urdf.xacro的内容如下:

<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mrobot_description)/urdf/mrobot_body.urdf.xacro" />
    <xacro:include filename="$(find mrobot_description)/urdf/kinect.xacro" />

    <xacro:property name="kinect_offset_x" value="-0.06" />
    <xacro:property name="kinect_offset_y" value="0" />
    <xacro:property name="kinect_offset_z" value="0.035" />

    <!-- MRobot机器人平台-->
    <mrobot_body/>

    <!-- Kinect -->
    <joint name="kinect_frame_joint" type="fixed">
        <origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
        <parent link="plate_2_link"/>
        <child link="camera_link"/>
    </joint>
    <xacro:kinect_camera prefix="camera"/>

</robot>

运行如下命令,即可在rviz中看到安装有Kinect的机器人模型了:

$ roslaunch mrobot_description display_mrobot_with_kinect.launch

3、添加激光雷达

使用类似的方式还可以为机器人模型添加一个激光雷达模型,这里不再赘述,激光雷达模型文件请参考mrobot_description/urdf/rplidar.xacro,顶层装配文件请参考mrobot_description/launch/display_mrobot_with_laser.launch。

运行如下命令,即可看到装有激光雷达的机器人模型:

$ roslaunch mrobot_description display_mrobot_with_laser.launch

到目前为止,我们创建了装有摄像头/Kinect/激光雷达的机器人模型,为了实现机器人仿真,还需要想办法控制机器人在仿真环境中运动。另外,还需要使传感器在仿真环境中像真实设备一样获取周围的信息就更好了。

四、基于ArbotiX和rviz的仿真器

在rviz的模型显示中使用了一个小插件来控制机器人的轮子转动,既然轮子可以转动,那么机器人就应该可以在rviz中运动。

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

本小节将为机器人配置ArbotiX差速控制器,配合rviz创建一个简单的仿真环境。

1、安装ArbotiX

$ sudo apt-get install ros-melodic-arbotix-*

或者从github上下载功能包:

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

下载成功后在工作空间根目录下编译:

$ catkin_make

2、配置ArbotiX控制器

ArbotiX功能包安装完成后,就可以针对机器人模型进行配置了。配置步骤较为简单,不需要修改机器人的模型文件,只需要创建一个启动ArbotiX节点的launch文件,再创建一个控制器相关的配置文件即可。

  • 创建launch文件

以装配了Kinect的机器人模型为例,创建启动ArbotiX节点的launch文件mrobot_
description/launch/arbotix_mrobot_with_kinect.launch,代码如下:

<launch>
    <param name="/use_sim_time" value="false" />
    
    <!-- 加载机器人URDF/Xacro模型 -->
    <arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot_with_kinect.urdf.xacro'" />
    <arg name="gui" default="false" />

    <param name="robot_description" command="$(arg urdf_file)" />
    <param name="use_gui" value="$(arg gui)"/>

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find mrobot_description)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <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">
        <param name="publish_frequency" type="double" value="20.0" />
    </node>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot_arbotix.rviz" required="true" />
</launch>

这个launch文件和之前显示机器人模型的launch文件几乎一致,只是添加了启动arbotix_driver节点的相关内容:

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find mrobot_description)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

arbotix_driver可以针对真实控制板进行控制,也可以在仿真环境中使用,需要配置“sim”参数为true。另外,该节点的启动还需要加载控制器相关的配置文件,该配置文件在功能包的config路径下。

  • 创建配置文件

配置文件mrobot_ description/config/fake_mrobot_arbotix.yaml的内容如下:

controllers: {
   base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}

控制器命名为base_controller,类型是diff_controller,也就是差速控制器,刚好可以控制机器人模型的双轮差速运动。此外,还需要配置参考坐标系、底盘尺寸、PID控制等参数。

3、运行仿真环境

完成上述配置后,rviz+AbotiX的仿真环境就搭建完成了,通过如下命令运行仿真:

$ roslaunch mrobot_description arbotix_mrobot_with_kinect.launch

启动成功后,可以看到机器人模型在rviz中已准备好。

查看当前ROS系统中的话题列表,可以看到cmd_vel话题,类似小乌龟运动,arbotix_driver节点订阅此话题,然后驱动模型运动。

运行键盘控制程序,然后在终端中根据提示信息点击键盘,就可以控制rviz中的机器人模型了。

$ roslaunch mrobot_teleop mrobot_teleop.launch

如视频所示,rviz中的机器人模型已按照速度控制指令开始运动,箭头代表机器人运动过程中的姿态。

rviz+ArbotiX构建运动仿真器
Melody 的视频
此时,机器人中的“Fixed Frame”是odom,也就是机器人的里程计坐标系。这是一个全局坐标系,通过里程计记录机器人当前的运动位姿,从而更新rviz中的模型状态。

rviz+ArbotiX可以构建一个较为简单的运动仿真器,除此之外,ROS还集成了更大的物理仿真平台gazebo。在真正学习gazebo之前,需要对这里使用到的ArbotiX差速控制器做一个升级,为gazebo仿真做准备。

五、ros_control

在rviz+ArbotiX构建的仿真器中使用了一款ArbotiX差速控制器以实现对机器人模型的控制,但是这款控制器有很大的局限性,无法在ROS丰富的机器人应用中通用,如果要将SLAM、导航、MoveIt!等功能包应用到机器人模型,甚至真实机器人之上时,应该如何实现这其中的控制环节呢?

ros_control就是ROS为开发者提供的机器人控制中间件,包含一系列控制器接口、传动转换接口、硬件接口、控制器工具等,可以帮助机器人应用功能包快速落地,提高开发效率。

1、ros_control框架

下图是ros_control的总体框架,针对不同类型的机器人(移动机器人、机械臂等),ros_control可以提供多种类型的控制器(controller),但是这些控制器的接口各不相同。为了提高代码的复用率,ros_control还提供一个硬件抽象层,负责机器人硬件资源的管理,而controller从抽象层请求资源即可,并不直接接触硬件。

ros_control总体框架

下图是ros_control的数据流图,可以更加清晰地看到每个层次所包含的功能。

ros_control数据流图
  • 控制器管理器(Controller Manager)

每个机器人可能有多个控制器,所以这里有一个控制管理器,提供一种通用的接口来管理不同的控制器。控制管理器的输入就是ROS上层应用功能包的输出。

  • 控制器(Controller)

控制器可以完成每个joint的控制,读取硬件资源接口中的状态,再发布控制指令,并且提供PID控制器。

  • 硬件资源接口(Hardware Resoure Interface)

为上下两层提供硬件资源的接口。

  • 机器人抽象层(RobotHW)

机器人硬件抽象和硬件直接打交道,通过write和read方法完成硬件操作,这一层也包含关节约束、力矩转换、物理状态转换等功能。

  • 真实机器人(Real Robot)

真实机器人上也需要有自己的嵌入式控制器,将接收到的命令反映到执行器上,比如接收到旋转90°的命令后,就需要让执行器快速、稳定地旋转90°。

2、控制器

目前ROS中的ros_controllers功能包提供了以下控制器:

  • effort_controllers:在关节上施加所需的力/扭矩。
    • joint_effort_controller (力/扭矩输入直接输出到关节)
    • joint_position_controller (位置输入进入PID控制器,该PID控制器向关节输出力/扭矩)
    • joint_velocity_controller (速度输入进入PID控制器,该PID控制器向关节输出力/扭矩)
  • joint_state_controller:将注册到hardware_interface::JointStateInterface的所有资源的状态发布到类型为sensor_msgs/JointState的topic。
    • joint_state_controller
  • position_controllers:一次设置一个或多个关节位置。
    • joint_position_controller
    • joint_group_position_controller
  • velocity_controllers:一次设置一个或多个关节速度。
    • joint_velocity_controller
    • joint_group_velocity_controller
  • joint_trajectory_controllers:附加功能,可为整个轨迹加花样。
    • position_controller
    • velocity_controller
    • effort_controller
    • position_velocity_controller
    • position_velocity_acceleration_controller

当然,我们也可以根据自己的需求创建控制器,然后通过控制器管理器来管理。如何创建请参考之前的教程:

3、硬件资源接口

硬件资源接口是控制器和RobotHW沟通的接口,基本与控制器种类相互对应,ros_controllers功能包提供了以下硬件接口:

  • Joint Command Interface:硬件接口支持命令关节阵列。请注意,这些命令可以具有任何语义含义,只要它们都可以由单个double表示即可,它们不一定是控制力的命令。要指定此命令的含义,请参见派生类:
  • Joint State Interfaces:硬件接口支持读取命名关节数组的状态,每个关节都有一些位置,速度和作用力(力或扭矩)。
  • Actuator State Interfaces:硬件接口支持读取命名的执行器数组的状态,每个执行器都有一定的位置,速度和作用力(力或扭矩)。
  • Actuator Command Interfaces
    • Effort Actuator Interface
    • Velocity Actuator Interface
    • Position Actuator Interface
  • Force-torque sensor Interface
  • IMU sensor Interface

同样可以自己创建需要的接口,详情请参考:

4、传动系统

传动系统(Transmission)可以将机器人的关节指令转换成执行器控制信号。机器人每个需要运动的关节都需要配置相应的传动系统,可以在机器人的urdf文件中按照以下方法配置:

<transmission name="simple_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="foo_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="foo_motor">
        <hardwareInterface>EffortJointInterface</hardwareInterface>
        <mechanicalReduction>50</mechanicalReduction>
    </actuator> 
</transmission>

5、关节约束

关节约束(Joint Limits)是硬件抽象层中的一部分,维护一个关节约束的数据结构,这些约束数据可以从机器人的urdf文件中加载,也可以从ROS的参数服务器上加载(需要先用YAML配置文件导入ROS参数服务器),不仅包含关节速度、位置、加速度、力矩等方面的约束,还包含起安全作用的位置软限位、速度边界(k_v)和位置边界(k_p)等。

可以使用如下方式在urdf中设置Joint Limits参数:

<joint name="$foo_joint" type="revolute">
  <!-- other joint description elements -->
  <!-- Joint limits -->
  <limit lower="0.0" upper="1.0" effort="10.0" />
  <!-- Soft limits -->
  <safety_controller k_position="100" k_velocity="10" soft_lower_limit="0.1" soft_upper_limit="0.9" />
</joint>

还有一些参数需要通过YAML配置文件事先加载到参数服务器中,YAML文件的格式如下:

joint_limits:
    foo_joint:
        has_position_limits: true
        min_position: 0.0
        max_position: 1.0
        has_velocity_limits: true
        max_velocity: 2.0
        has_acceleration_limits: true
        max_acceleration: 5.0
        has_jerk_limits: true
        max_jerk: 100.0
        has_effort_limits: true
        max_effort: 5.0
    bar_joint:
        has_position_limits: false
        has_velocity_limits: true
        max_velocity: 4.0

6、控制器管理器

controller_manager提供了一种多控制器控制的机制,可以实现控制器的加载、开始运行、停止运行、卸载等多种操作。下图就是controller manager控制控制器实现的状态跳转。

controller_manager还提供多种工具来辅助完成这些操作。

(1)命令行工具

controller_manager命令的格式为:

$ rosrun controller_manager controller_manager <command> <controller_name>

支持的<command>命令如下:

  • load:加载一个控制器。
  • upload:卸载一个控制器。
  • start:启动控制器。
  • stop:停止控制器。
  • spwan:加载并启动一个控制器。
  • kill:停止并卸载一个控制器。

如果想查看某个控制器的状态,可以使用如下命令:

$ rosrun controller_manager controller_manager <command>

支持的<command>命令如下:

  • list:根据执行顺序列出所有控制器,并显示每个控制器的状态。
  • list-types:显示所有控制器的类型。
  • reload-libraries:以插件的形式重载所有控制器的库,不需要重新启动,方便对控制器的开发和测试。
  • reload-libraries --restore:以插件的形式重载所有控制器的库,并恢复到初始状态。

但是,很多时候,我们需要控制的控制器有很多,比如六轴机器人至少有6个控制器,这时候也可以使用spawner命令一次控制多个控制器:

$ rosrun controller_manager spawner [--stopped] name1 name2 name3

上面的命令可以自动加载、启动控制器,如果加上--stopped参数,那么控制器则只会被加载,但是并不会开始运行。如果想要停止一系列控制器,但是不需要卸载,可以使用如下命令:

$ rosrun controller_manager unspawner name1 name2 name3

(2)launch工具

在launch文件中,同样可以通过运行controller_manager命令,加载和启动一系列控制器:

<launch>
    <node pkg="controller_manager" type="spawner" args="controller_name1 controller_name2" />
</launch>

以上launch文件加载并启动controller,如果只需要加载不必启动,则使用以下配置:

<launch>
    <node pkg="controller_manager" type="spawner" args="--stopped controller_name1 controller_name2" />
</launch>

(3)可视化工具rqt_controller_manager

ros_control功能包的下载网址:

改功能包集成了许多拓展功能,如rqt_controller_manager可视化工具,下载成功后,在工作空间内编译,然后使用以下命令打开:

$ roscore
$ rosrun rqt_controller_manager rqt_controller_manager

六、gazebo仿真

1、为机器人模型添加gazebo属性

使用xacro设计的机器人urdf模型已经描述了机器人的外观与物理属性,还是无法让模型在gazebo中动起来,那么要如何开始仿真呢?

首先,我们要确保每个link的<inertial>元素已经进行了合理的设置,然后要为每个必要的<link>、<joint>、<robot>设置<gazebo>标签。<gazebo>标签是urdf模型中描述gazebo仿真时所需要的扩展属性。

添加gazebo属性之后的模型文件放置在mrobot_gazebo功能包的urdf文件夹下,以区别mrobot_description中的urdf模型。
  • 为link添加<gazebo>标签

需要对每一个link添加<gazebo>标签,包含的属性仅有material。material属性的作用与link里<visual>中material属性的作用相同。gazebo无法通过<visual>中的material参数设置外观,所以需要单独设置,否则默认默情况下gazebo显示的模型全是灰白色的。

以base_link为例,<gazebo>标签的内容如下:

<gazebo reference="wheel_${ir}_link">
    <material>Gazebo/Black</material>
<gazebo>

2、添加传动装置

我们的机器人模型是一个两轮差速驱动的机器人,通过调节两个轮子的速度比例,完成前进、转向、倒退等动作。但在之前的模型中,并没有加入驱动机器人的运动源,这是仿真必不可少的。为了使ROS控制器驱动机器人,需要在模型中加入<transmission>元素, 将传动装置与joint绑定:

<transmission name="wheel_${lr}_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_to_wheel_$lir}_joint">
    <actuator name="wheel_${lr}_joint_motor">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator> 
</transmission>

以上代码中,<joint name="">定义了将要绑定驱动器的joint,<type>声明了所使用的传动装置类型,<hardwareInterface>定义了硬件接口的类型,这里使用速度控制接口。

到目前为止,机器人还是一个静态模型,如果要让它在gazebo中动起来,还需使用gazebo插件。gazebo插件功能强大,可以帮助模型绑定ROS信息,从而完成传感器的仿真输出以及电机的控制,让机器人模型更加真实。

3、添加gazebo控制器插件

gazebo插件可以根据插件的作用范围应用到urdf模型的<robot>、<link>、<joint>上,需要使用<gazebo>标签作为封装。

(1)为<robot>元素添加插件

<gazebo>
    <plugin name="unique_name" filename="plugin_name.so">
        ... plugins parameters ...
    </plugin>
<gazebo>

与其他的<gazebo>元素相同,如果<gazebo>元素中没有设置reference="x"属性,则默认应用于<robot>标签。

(2)为<link>、<joint>标签添加插件

如果需要为<link>、<joint>标签添加插件,则需要设置<gazebo>标签中的reference=“x”属性:

<gazebo reference="your_link_name">
    <plugin name="unique_name" filename="plugin_name.so">
        ... plugins parameters ...
    </plugin>
</gazebo>

gazebo目前所支持的插件种类,可以查看ROS默认安装路径下的/opt/ros/melodic/lib文件夹。

gazebo已经提供了一个用于差速控制的插件libgazebo_ros_diff_drive.so,可以将其应用到现有机器人模型上,在mrobot_gazebo/urdf/mrobot_body.urdf.xacro文件中添加如下插件声明:

        <!-- controller -->
        <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>base_to_wheel_left_joint</leftJoint>
                <rightJoint>base_to_wheel_right_joint</rightJoint>
                <wheelSeparation>${base_link_radius*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</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> 

在加载差速控制器插件的过程中,需要配置一系列参数,其中比较关键的参数如下:

  • <robotNamespace>:机器人的命名空间,插件所有数据的发布、订阅都在该命名空间下。
  • <leftJoint>和<rightJoint>:左右转动的关节joint,控制器插件最终需要控制这两个joint转动。
  • <wheelSeparation>和<wheelDiameter>:这是机器人模型的相关尺寸,在计算差速参数时需要用到。
  • <wheelAcceleration>:车轮转动的加速度。
  • <commandTopic>:控制器订阅的速度控制指令,ROS中一般都命名为cmd_vel,生成全局命名时需要结合<robotNamespace>中设置的命名空间。
  • <odometryFrame>:里程计数据的参考坐标系,ROS中一般都命名为odom。

4、在gazebo中显示机器人模型

创建一个启动文件robot_mrobot/mrobot_gazebo/view_mrobot_gazebo.launch,运行gazebo,加载机器人模型,并且启动一些必要的节点:

<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/playground.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/mrobot.urdf.xacro'" /> 

    <!-- 运行joint_state_publisher节点,发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" ></node> 

    <!-- 运行robot_state_publisher节点,发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mrobot -param robot_description"/> 

</launch>

以上launch文件主要做了两项工作:

  • 启动机器人的状态发布节点,同时加载带有gazebo属性的机器人urdf模型。
  • 启动gazebo,并且将机器人模型加载到gazebo仿真环境中。

现在,启动这个launch文件,如果一切正常,可以看到机器人模型已经导入到仿真环境中。

$ roslaunch mrobot_gazebo view_mrobot_gazebo.launch

5、控制机器人在gazebo中运动

机器人模型中已经加入了libgazebo_ros_diff_drive.so插件,可以使用差速控制器实现机器人运动,查看/cmd_vel话题信息。

从上图可以看出,gazebo仿真中已经开始订阅cmd_vel话题了。接下来运行键盘控制节点,发布该话题的速度控制消息,机器人就会在gazebo中开始运动了。

$ roslaunch mrobot_teleop mrobot_teleop.launch
键盘控制机器人在gazebo中运动
Melody 的视频

当机器人在仿真环境中撞到障碍物时,会根据两者的物理属性决定机器人是否反弹,或者障碍物是否推动,这也说明了gazebo是一种贴近真实环境的物理仿真平台。

6、摄像头仿真

之前在rviz+ArbotiX搭建的机器人仿真环境中,机器人装配了多种传感器模型,但是这些模型无法获取任何环境数据。gazebo强大之处在于提供了一系列传感器插件,可以帮助我们模拟真实传感器,获取gazebo中的传感信息。

(1)为摄像头模型添加gazebo插件

类似差速控制器插件,我们需要修改机器人urdf模型。复制mrobot_description中的传感器模型到mrobot_gazebo包中,然后在摄像头的模型文件mrobot_gazebo/urdf/
camera.xarco中添加<gazebo>的相关标签。

        <gazebo reference="${prefix}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <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_link</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>

新的摄像头模型文件在模型描述部分没有发生变化,只需要加入两个<gazebo>标签。

第一个<gazebo>标签用来设置摄像头模型在gazebo中的material,与机器人模型的配置相似,只需要设置颜色参数。

重点是第二个设置摄像头插件的<gazebo>标签。在加载传感器的时候,需要使用<sensor>标签来包含传感器的各种属性。例如,使用的是摄像头传感器,需要设置type为camera,传感器的命名(name)可以随意设置;然后使用<camera>标签具体描述摄像头的参数,包括分辨率、编码格式、图像范围、噪音参数等;最后使用<plugin>标签加载摄像头的插件libgazebo_ros_camera.so,同时设置插件的参数,包括命名空间、发布图像的话题、参考坐标系等。

(2)运行仿真环境

现在摄像头插件已经设置完成,使用如下命令启动仿真环境,并加载装配了摄像头的机器人模型:

$ roslaunch mrobot_gazebo view_mrobot_with_camera_gazebo.launch

启动成功后,可以看到机器人已经在仿真环境中了,如下图所示:

查看当前系统中的话题列表,从下图发布的话题中可以看到摄像头已经开始发布图像消息了。

使用rqt查看当前机器人眼前的世界,选择仿真摄像头发布的图像话题/camera/image_raw,即可看到如下图所示的图像信息。

$ rqt_image_view

现在在仿真环境中机器人不仅可以进行运动控制,还可以获得传感器的反馈信息。

7、Kinect仿真

很多应用还会用到Kinect等RGB-D传感器,接下来就为gazebo中的机器人装配一个Kinect,让它可以获取更加丰富的三位信息。

(1)为Kinect模型添加gazebo插件

在Kinect模型文件mrobot_gazebo/urdf/kinect.xacro中添加以下<gazebo>标签:

        <gazebo reference="${prefix}_link">
            <sensor type="depth" name="${prefix}">
                <always_on>true</always_on>
                <update_rate>20.0</update_rate>
                <camera>
                    <horizontal_fov>${60.0*M_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_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
                    <cameraName>${prefix}</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>${prefix}_frame_optical</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>

这里需要选择的传感器类型是depth,<camera>中的参数与摄像头类似,具体可查阅Kinect手册,<plugin>标签中加载的Kinect插件是libgazebo_ros_openni_kinect.so,需要设置发布的各种数据话题名以及参考坐标系等参数。

(2)运行仿真环境

使用如下命令启动仿真环境,并加载装配了Kinect的机器人模型:

$ roslaunch mrobot_gazebo view_mrobot_with_kinect_gazebo.launch

查看当前系统的话题列表,确保Kinect插件已经启动成功:

然后使用如下命令打开rviz,查看Kinect的点云数据:

$ rosrun rviz rviz

在rviz中需要设置"Fix Frame"为"camera_frame_optical",然后添加一个PointCloud2类型的插件,修改插件订阅的话题为/camera/depth/points,此时就可以在主界面中看到如下图所示的点云信息。

8、激光雷达仿真

在SLAM和导航等机器人应用中,为了获取更精确的环境信息,往往会使用激光雷达作为主要传感器。

(1)为rplidar模型添加gazebo插件

我们使用的激光雷达是rplidar,在rplidar的模型文件mrobot_gazebo/urdf/rplidar.xacro中添加以下<gazebo>标签:

        <gazebo reference="${prefix}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <sensor type="ray" name="rplidar">
                <pose>0 0 0 0 0 0</pose>
                <visualize>false</visualize>
                <update_rate>5.5</update_rate>
                <ray>
                    <scan>
                      <horizontal>
                        <samples>360</samples>
                        <resolution>1</resolution>
                        <min_angle>-3</min_angle>
                        <max_angle>3</max_angle>
                      </horizontal>
                    </scan>
                    <range>
                      <min>0.10</min>
                      <max>6.0</max>
                      <resolution>0.01</resolution>
                    </range>
                    <noise>
                      <type>gaussian</type>
                      <mean>0.0</mean>
                      <stddev>0.01</stddev>
                    </noise>
                </ray>
                <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
                    <topicName>/scan</topicName>
                    <frameName>laser_link</frameName>
                </plugin>
            </sensor>
        </gazebo>

激光雷达的传感器类型是ray,rplidar的相关参数可查阅手册。为了尽量贴近真实的仿真环境效果,需要根据实际参数配置<ray>中的雷达参数:360°检测范围、单圈360个采样点、5.5Hz采样频率,最远6m检测范围等。最后使用<plugin>标签加载激光雷达的插件libgazebo_ros_laser.so,所发布的激光雷达话题是“/scan”。

(2)运行仿真环境

使用如下命令启动环境,并加载装配了激光雷达的机器人:

$ roslaunch mrobot_gazebo view_mrobot_with_laser_gazebo.launch

查看当前系统中,确保laser插件已经成功启动:

然后使用如下命令打开rviz,查看rplidar的激光数据:

$ rosrun rviz rviz

在rviz中设置"Fix Frame"为"base_footprint",然后添加一个LaserScan类型的插件,修改插件订阅的话题为"/scan",就可以看到界面中的激光数据了。

到目前为止,gazebo中的机器人模型已经比较完整了,接下来我们就可以在这个仿真环境的基础上实现丰富的机器人功能。

七、本章小结

仿真是系统开发中的重要步骤,学习完本章内容后,你应该已经了解了如何使用urdf文件创建一个机器人模型,然后使用xacro文件优化该模型,并且放置到rviz+Abotix或gazebo仿真环境中,以实现丰富的ROS功能。