ORB_SLAM2的ROS入门2

准备RGBD节点

由于之前的单目相机Mono节点默认接收的RGB图像节点名称为 /camera/image_raw ，而深度相机RGB-D节点默认接收的RGB图像节点和深度Depth节点为 /camera/rgb/image_raw 和 /camera/depth_registered/image_raw，此时如果我们还是用之前的相机，就需要修改之前相机的RGB图像发布话题名称。

故，推荐将文件 ./Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc 的以下部分内容：

ros::NodeHandle nh;

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", 1);

修改为：

ros::NodeHandle nh("~");

std::string rgb_topic = nh.param<std::string>("rgb", "/camera/rgb/image_raw");
std::string depth_topic = nh.param<std::string>("depth", "/camera/depth_registered/image_raw");

cout << "rgb: " << rgb_topic << endl;
cout << "depth: " << depth_topic << endl;

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic, 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic, 1);

然后在项目跟路径执行以下命令，重新编译ros环境：

./build_ros.sh

这样，我们就可以指定任意的rgb话题和depth话题了（当然，如果没有指定则会使用默认值）。

发布实时数据

从相机发布数据

启动Astra相机

执行以下命令启动相机

roslaunch astra_camera astrapro.launch

启动后，可以通过执行 rosrun rqt_image_view rqt_image_view 确保以下两种数据是否可见：

1、RGB彩色数据：/camera/rgb/image_raw

2、Depth深度数据：/camera/depth_registered/image_raw

启动RGBD节点

此时，我们可以使用以下方式启动RGBD节点：

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/astra.yaml _rgb:=/camera/rgb/image_raw _depth:=/camera/depth_registered/image_raw

效果如下：

从bag中发布数据

我们可以直接在该网站 https://vision.in.tum.de/data/datasets/rgbd-dataset/download 中下载到bag数据。

我们以 rgbd_dataset_freiburg1_xyz.bag 为例，下载后，可以使用 rosbag info xxx.bag 命令查看到彩色数据和深度数据：

ty@ty-PC:~/Workspace/SLAM/Res$ rosbag info rgbd_dataset_freiburg1_xyz.bag 
path:         rgbd_dataset_freiburg1_xyz.bag
version:      2.0
duration:     30.4s
start:        May 10 2011 20:38:18.38 (1305031098.38)
end:          May 10 2011 20:38:48.81 (1305031128.81)
size:         480.4 MB
messages:     25626
compression:  bz2 [1598/1598 chunks; 29.14%]
uncompressed:   1.6 GB @ 54.1 MB/s
compressed:   479.4 MB @ 15.8 MB/s (29.14%)
types:        sensor_msgs/CameraInfo         [c9a58c1b0b154e0e6da7578cb991d214]
              sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
              sensor_msgs/Imu                [6a62c6daae103f4ff57a132d6f95cec2]
              tf/tfMessage                   [94810edda583a504dfda3829e70d7eec]
              visualization_msgs/MarkerArray [f10fe193d6fac1bf68fad5d31da421a7]
topics:       /camera/depth/camera_info     798 msgs    : sensor_msgs/CameraInfo        
              /camera/depth/image           798 msgs    : sensor_msgs/Image             
              /camera/rgb/camera_info       798 msgs    : sensor_msgs/CameraInfo        
              /camera/rgb/image_color       798 msgs    : sensor_msgs/Image             
              /cortex_marker_array         3034 msgs    : visualization_msgs/MarkerArray
              /imu                        15158 msgs    : sensor_msgs/Imu               
              /tf                          4242 msgs    : tf/tfMessage

可以看到彩色和深度分别对应的话题为：/camera/rgb/image_color 和 /camera/depth/image

启动bag数据

rosbag play --pause -l rgbd_dataset_freiburg1_xyz.bag

这里启动之后，需要按下空格才会真正发布数据。-l 参数表示循环播放该数据包，可以去掉。

启动RGBD节点

此时，我们可以使用以下方式启动RGBD节点，但是我们要注意先准备一个新的相机参数yaml文件

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1_rosbag.yaml _rgb:=/camera/rgb/image_color _depth:=/camera/depth/image

注意这里我们修改了相机配置文件，彩色图话题，深度图话题。

这里的相机配置文件有所修改，我们要将 Examples/RGB-D/TUM1.yaml 复制一份为 Examples/RGB-D/TUM1_rosbag.yaml 将其中的一行DepthMapFactor改为如下值：

# Deptmap values factor 
DepthMapFactor: 1.0

效果如下：