Inter realsense 发布RGB和深度图

直接上代码：

#include <ros/ros.h>
#include <librealsense2/rs.hpp>
#include <iostream> 
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

int main(int argc, char * argv[]) 
{   
    ros::init(argc, argv, "librealsense_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher rgb_pub = it.advertise("librealsense/rgb", 1);
    image_transport::Publisher depth_pub = it.advertise("librealsense/depth", 1);

    try
    {
        rs2::config cfg;
        
        cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 60);
        cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);

        rs2::pipeline pipe;
        pipe.start(cfg);
        rs2::frameset frames;

        rs2::device dev = pipe.get_active_profile().get_device();
        dev.query_sensors()[1].set_option(rs2_option::RS2_OPTION_EXPOSURE, 50);
        
        ros::Rate rate(60);

        while (ros::ok())
        {
            frames = pipe.wait_for_frames();

            rs2::frame depth  = frames.get_depth_frame();
            rs2::frame color = frames.get_color_frame();

            if (!color || !depth) break; 
            cv::Mat image(cv::Size(640, 480), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP);
            cv::Mat depthmat(cv::Size(640, 480), CV_16U, (void*)depth.get_data(), cv::Mat::AUTO_STEP);

            sensor_msgs::ImagePtr rgb_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
            sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(std_msgs::Header(), "16UC1", depthmat).toImageMsg();

            rgb_pub.publish(rgb_msg);
            depth_pub.publish(depth_msg);
            
            // ros::spin();  
        }
        
        pipe.stop();
        return EXIT_SUCCESS;
    }
    catch (const rs2::error & e)
    {
        std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
        return EXIT_FAILURE;
    }
    catch (const std::exception& e)
    {   
        std::cerr << "Realsense failed connect" << std::endl;
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}

注意，此时需要对package.xml和CMakelist进行修改，配置如下：
package.xml需增加

<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>opencv2</build_depend>

<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<build_export_depend>opencv2</build_export_depend> 

<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>opencv2</exec_depend>

Cmakelist需在find_package中增加

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
)

其余配置和安装篇一致。
此处踩了两个小坑：

1. 获取照片的while（）中，判断条件需要设为ros::ok，然后循环后设置pipe.stop，否则在命令行关闭节点后，realsense会被关掉线，只能重新插拔才能用；
2. while循环中不要对齐深度图和rgb图，否则帧率从60降到8。。。