使用ORB-SLAM2算法和Turtlebot3机器人在自己构建的室内场景中建图

本文叙述了在gazebo仿真环境和rqt可视化工具的背景下，使用ORB-SLAM2算法和turtlebot3-burger机器人在自己构建的室内场景中建图过程。目前存在的问题是运行RGBD节点时无法显示图像，报错“WAITING FOR IMAGES”。

1. 构建ORB-SLAM2工作空间并编译

参考前面文章
Ubuntu 16.04系统下安装Open-CV 3.1.0
Ubuntu 16.04系统下安装Eigen3矩阵库（简便法）
Ubuntu 16.04系统下使用数据集运行ORB-SLAM2算法
关于ORB-SLAM2算法编译过程中运行./build_ros.sh命令报错问题的解决方法

2. 在ORB-SLAM2工作空间中编译Turtlebot3-Burger并添加kienct

参考前面文章
Turtlebot3仿真环境搭建
为Turtlebot3-Burger机器人添加Kinect深度相机并获取环境图像

3. 在ORB-SLAM2工作空间添加gazebo场景功能包

（1）在catkin_ws_orb下创建功能包orb_room

catkin_create_pkg orb_room std_msgs rospy roscpp

cd ..

catkin_make
source devel/setup.bash

（2）在orb_room功能包下放置world文件夹，其中包含之前搭建好的my_room.world文件

（3）编写launch文件
在orb_room功能包下建立launch文件夹，并在launch文件夹中新建orb_room.launch文件，内容如下：

<launch>
    <!-- 设置launch文件的参数 -->
    <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="$(find orb_room)/world/my_room.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->	
	<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="turtlebot3_description" command="$(find xacro)/xacro --inorder '$(find turtlebot3_description)/urdf/turtlebot3_burger.urdf.xacro'" /> 

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

</launch>

（4）编译刷新环境

catkin_make
source devel/setup.bash

4. 启动建图仿真

（1）启动场景

roslaunch orb_room orb_room.launch

（2）启动ORB-SLAM2的RGBD节点
在catkin_ws_orb/src/ORB_SLAM2下运行

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

注： 如果出现了找不到opencv库的问题，请参考此文章
ERROR WHILE LOADING SHARED LIBRARIES: LIBOPENCV_HIGHGUI.SO.3.4: CANNOT OPEN SHARED OBJECT FILE解决方法


此时出现了问题，图像没有显示，报错：WAITING FOR IMAGES。是因为话题名没有修改。全部话题如下图所示：



根据话题名将ros_rgbd.cc文件中的话题名进行更改。

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth/image_raw", 1);

然后在catkin_ws_orb/src/ORB_SLAM2下重新运行命令

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml

（3）启动键盘控制节点

sudo apt-get install ros-kinetic-teleop-twist-keyboard

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

参考资料

ERROR WHILE LOADING SHARED LIBRARIES: LIBOPENCV_HIGHGUI.SO.3.4: CANNOT OPEN SHARED OBJECT FILE解决方法
ORB_SLAM2功能包的构建和在ROS中的应用
ROS入门：ORB_Slam2踩坑，RGBD无法获取数据