利用ROS与XTDrone仿真平台采集数据用于SLAM

• 0. 背景
• 1. 运行仿真环境
• 2. 开启通讯节点
• 3. 启动位姿转发节点
• 4. 启动可视化影像节点
• 5. 开启控制脚本
• 6. 录制ROS Bag数据包
• 7. 参考资料

0. 背景

本篇博客主要记录利用XTDrone平台采集数据，用于SLAM等应用的具体步骤，方便查阅与参考。关于XTDrone的安装，可以参考官方文档进行配置安装，如果遇到问题，多搜索一般可以找到解决办法。首先介绍我的电脑的软件环境：

• 系统: Ubuntu 18.04
• ROS: ROS Melodic
• XTDrone安装路径: /root/Softwares/XTDrone
• XTDrone Workspace: /root/Softwares/xtdrone_ws
• PX4安装路径: /root/Softwares/PX4_Firmware
• .bashrc文件中的相关路径：

  # for XTDrone & PX4
  source /root/Softwares/xtdrone_ws/devel/setup.bash
  source /root/Softwares/PX4_Firmware/Tools/setup_gazebo.bash /root/Softwares/PX4_Firmware/ /root/Softwares/PX4_Firmware/build/px4_sitl_default
  export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/root/Softwares/PX4_Firmware/
  export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/root/Softwares/PX4_Firmware/Tools/sitl_gazebo
  

了解了基本环境之后，下面就进入数据采集阶段。需要说明的是，如果只是想快速跑起来仿真环境采集数据，可以不用看解释部分，直接复制命令执行即可，所有命令均亲测可用。

1. 运行仿真环境

打开一个终端，输入如下命令，启动Gazebo仿真环境：

roslaunch px4 indoor1.launch

解释：这一行命令的作用是启动XTDrone的仿真环境，后面的xxx.launch文件对应了不同的世界模型和传感器模型，比如indoor1.launch文件的内容如下。 我们可以清楚地看到他指定了哪些世界模型、载荷平台、传感器等。如果需要修改，我们只需要顺藤摸瓜地找到对应文件修改即可。 这些默认的.launch文件都放在了PX4安装路径下面的launch文件夹，比如我的路径是/root/Softwares/PX4_Firmware/launch。可能遇到的错误是说找不到px4节点或者.launch文件，这个时候需要检查.bashrc文件里是否把对应的路径添加好了。

结果：执行完上面的命令后，你就应该会启动XTDrone界面了，如下所示。

2. 开启通讯节点

打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，启动通讯节点：

cd ~/Softwares/XTDrone/communication
python multirotor_communication.py iris 0

解释：利用XTDrone提供的脚本，实现与传感器平台的通讯。iris是平台名称，0表示索引。可能遇到的错误是Python版本问题(在装了Anaconda的系统上容易出现)。如果是这样的话，记得在.bashrc中把Anaconda相关的路径注释掉，基本应该就可以了。

结果：执行完上面的命令后，你应该就会看到“iris_0: communication initialized”的提示，如下所示。

3. 启动位姿转发节点

再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，启动位姿转发节点:

cd ~/Softwares/XTDrone/sensing/pose_ground_truth
python get_local_pose.py iris 1

解释：作为为SLAM服务的数据采集，自然希望可以提供位姿真值。默认情况下XTDrone并不会自动发布位姿真值，需要我们输入上面的命令手动发布。这里的1表示1架无人机。默认发布的Topic名称为/iris_0/mavros/vision_pose/pose。可能的错误和第二步类似，参考上面的办法解决。更详细介绍可以参考官方文档。

结果：执行完上面的命令后，你应该就会看到“Get iris_0 groundtruth pose”的提示，如下所示。

4. 启动可视化影像节点

再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令，用于实时可视化相机数据：

rqt_image_view /iris_0/stereo_camera/left/image_raw

解释：虽然我们在飞无人机的时候可以在Gazebo里看到无人机的状态，但显然我们更关心采集到的影像数据的状态，所以我们可以利用RQT工具，实施可视化出采集的影像，方便进行调整。这里，iris无人机本身自带了一个双目相机，分别发布左右影像的Topic，为/iris_0/stereo_camera/left/image_raw和/iris_0/stereo_camera/right/image_raw。这里我们订阅左影像的数据就够了。可能的错误是Topic名称不对。如果是这样的话，检查前面提到的各种配置文件，看是否自定义了Topic名称。以默认的indoor1.launch为例，如下。 可以看到，在第36行，又包含了一个叫做iris_stereo_camera的模型，我们就去找这个文件。这个文件默认是放在PX4安装路径下面的Tools/sitl_gazebo/models中。比如我的电脑上就是/root/Softwares/PX4_Firmware/Tools/sitl_gazebo/models。不出意外你可以在这个文件夹下找到一个叫做iris_stereo_camera的文件夹，如下。 打开，就可以看到iris_stereo_camera.sdf文件了，如下。 我们进一步打开这个文件，如下。 在这个模型文件中，重点描述了各个部件的组成和相对位置关系等等。而跟我们相关的主要是两个：双目相机和IMU。可以看到，像是套娃一样，双目相机和IMU又分别引用的stereo_camera和imu_gazebo模型。再一次，我们在PX4安装路径下面的Tools/sitl_gazebo/models文件夹中寻找。可以找到stereo_camera文件夹，如下。 打开文件夹中的stereo_camera.sdf，就可以揭开“庐山真面目”了，如下。 可以看到，在一个名叫stereo_camera的sensor节点下面，有两个camera节点，分别叫做left和right。不仅如此，这里还定义了相机的详细信息，比如FOV、影像长宽、色彩格式等。而在plugin节点下面有个imageTopicName节点，它的值为image_raw。至此，我们也就明白了完整的影像Topic的构成。不仅如此，还知道了相机的相关参数是在哪里设置的，自然也就知道如何修改了。而双目相机的外参，同样可以通过.sdf文件获取到(camera节点下面有pose属性)。类似的，对于IMU也是同理。我们可以打开imu_gazebo.sdf文件，如下。 可以看到，在这里定义了IMU的全部参数，包括帧率、噪声等等，如果需要，可以对应修改。

结果：执行完上面的命令后，你应该就会看到可视化的相机影像，如下所示。

5. 开启控制脚本

再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，用于实时控制无人机：

cd ~/Softwares/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel

解释：通过键盘来控制无人机的起飞、降落等。推荐起飞流程: 按i把向上速度加到0.3以上，再按b切offboard模式，最后按t解锁。基本的关于控制无人机的按键包括：w-前进、x-后退、i-上升、,-下降、j-左转、l-右转。 更多信息，可以参考官方文档。可能遇见的错误有上面提到的Python版本问题。还有就是用键盘控制无人机需要多加练习才能得心应手，不然比较难控制。当然了，要按照上面提到的起飞流程才能把飞机飞起来，不然飞机是不会动的(没有解锁)。

结果：执行完上面的命令后，你应该就会看到相关提示，在终端中按照提示就可以控制无人机了，如下所示。

6. 录制ROS Bag数据包

再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令，用于录制想要的Topic:

rosbag record /iris_0/stereo_camera/left/image_raw /iris_0/stereo_camera/right/image_raw /iris_0/imu_gazebo /iris_0/mavros/vision_pose/pose

解释：经过上面的步骤，无人机已经在飞行了，我们就可以录制数据了。以SLAM为例，我们可以录制左右两个双目影像以及一个IMU的观测，当然还有位姿真值，这些都在前面介绍过了。录制完以后，按Ctrl+C结束录制。默认情况下，录制的Bag文件以日期命名，存放在当前终端目录下。根据经验，强烈推荐做法：先录一小段试试看效果，确认没有问题以后，再正式开始录制。之前就遇到过采集了半天数据回来发现没录上，一检查发现是Topic名称打错了个字母(血的教训)。

结果：录制完成以后，就可以看到ROS Bag文件了，可以利用rosbag info工具查看信息，如下。

至此，我们便完成了利用XTDrone采集数据并保存的任务，拿着录制好的ROS Bag包就可以跑SLAM了。当然，后续还有一些其它零碎的操作，比如把ROS Bag包中的真值位姿提取出来，方便利用EVO进行评价、抽取ROS Bag中的影像帧等等，这些操作可以见之前的这篇博客和这个Github项目(不定期更新好用的脚本)，就不在这里赘述了。

7. 参考资料

• [1] https://www.yuque.com/xtdrone/manual_cn/basic_config
• [2] https://www.yuque.com/xtdrone/manual_cn/pose_groundtruth