ORB-SLAM2：(三)仿真实现&kinect2在线定位建图

前言

这是运行ORB-SLAM2代码系列文章的第三篇，主要完成ORB-SLAM2在仿真环境中及基于kinect2在线定位建图。默认已经完成源码下载、编译和相关环境的配置，环境配置具体可参考：ORB-SLAM2：环境配置&源码编译。

仿真实现

这里使用的是由北京中科重德智能科技有限公司提供的机器人仿真环境，包含了XBot机器人和中科院软件博物馆仿真环境，同时也提供好了使用ORB_SLAM2的launch文件，执行起来比较方便。首先从github上下载：

1. 下载环境

cd ~/catkin_ws/src
git clone https://github.com/DroidAITech/ROS-Academy-for-Beginners.git

2. 安装依赖项

参照讲义安装依赖项，否则编译可能会出问题。

cd ~/catkin_ws
rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y

更新Gazebo，确保Gazebo版本在7.0以上，讲义中推荐是7.9，直接升级即可。

// gazebo -v # 查看gazebo版本
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt-get install gazebo7

3. 修改话题名

上述操作配置好了仿真环境，接下来需要对ORB-SLAM2源码进行修改，保证仿真环境发布的话题名和算法订阅的话题名相同。首先定位到如下文件：

~/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc

修改话题名：(depth_registered --> depth)

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_registered/image_raw/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth/image_raw", 1)

4. 重新编译ORB-SLAM2

因为使用的是ROS环境下的RGB-D方式，所以只./build_ros.sh即可：

chmod +x build_ros.sh
./build_ros.sh

5. 运行

分别在不同终端运行如下launch文件，即可在仿真环境下运行ORB-SLAM2

roslaunch robot_sim_demo robot_spawn.launch		// 启动仿真环境
roslaunch orbslam2_demo ros_orbslam2.launch		// 运行ORB-SLAM2算法
rosrun robot_sim_demo robot_keyboard_teleop.py 	// 键盘控制节点，通过按键控制机器人移动：i , j l（前后左右）

6. 结果

程序运行后可看到如下图所示的初始状态，通过控制小车移动即可完成三维稀疏点云的重建和相机位姿的定位。

7. 可能出现的问题

如果出现如下错误，

[ERROR] [1527494740.144219702, 307.635000000]: GazeboRosControlPlugin missing <legacyModeNS> while using DefaultRobotHWSim, defaults to true.
This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, where the robotNamespace is disregarded and absolute paths are used instead.
If you do not want to fix this issue in an old package just set <legacyModeNS> to true.

解决方法：
定位到如下.gazebo文件，添加下图