ROS Realsense SDK发布图像和Camera_info

原创已于 2022-04-07 10:33:24 修改 · 1k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#linux

于 2022-03-30 19:49:34 首次发布

本文介绍如何使用ROS与librealsense库实现Realsense摄像头的数据发布，包括图像与相机内参的同步发布，并提供了关键代码示例。

先上代码：

#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>
#include <vector>
#include <camera_info_manager/camera_info_manager.h>

using namespace std;

sensor_msgs::CameraInfoPtr getCameraInfo(rs2_intrinsics intrinsecs)
{
    sensor_msgs::CameraInfoPtr cam(new sensor_msgs::CameraInfo());
    vector<double> D{intrinsecs.coeffs[0], intrinsecs.coeffs[1], intrinsecs.coeffs[2], intrinsecs.coeffs[3], intrinsecs.coeffs[4]};
    boost::array<double, 9> K = {
        intrinsecs.fx,          0,  intrinsecs.ppx,
        0,        intrinsecs.fy,    intrinsecs.ppy,
        0,        0,        1,
    };
    
    vector<double> k = {intrinsecs.fx,          0,  intrinsecs.ppx,
        0,        intrinsecs.fy,    intrinsecs.ppy,
        0,        0,        1};

    cv::Mat p = cv::getOptimalNewCameraMatrix(k, D, cv::Size(640, 480), 0);        // get rectified projection.
    boost::array<double, 12> P = {
        p.at<double>(0,0),        p.at<double>(0,1),        p.at<double>(0,2),        p.at<double>(0,3),
        p.at<double>(1,0),        p.at<double>(1,1),        p.at<double>(1,2),        p.at<double>(1,3),
        p.at<double>(2,0),        p.at<double>(2,1),        p.at<double>(2,2),        p.at<double>(2,3)
    };
    boost::array<double, 9> r = {1, 0, 0, 0, 1, 0, 0, 0, 1};

    cam->height = 480;
    cam->width = 640;
    cam->distortion_model = "plumb_bob";
    cam->D = D;
    cam->K = K;
    cam->P = P;
    cam->R = r;
    // cam.binning_x = 0;
    // cam.binning_y = 0;
    return cam;
}

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

    image_transport::CameraPublisher depth_pub = it.advertiseCamera("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);

        sensor_msgs::CameraInfoPtr color_cameraInfo_, depth_cameraInfo_;
        bool key_param = true;

        ROS_INFO("=============Realsense Successful start================");
        
        ros::Rate rate(60);
        while (ros::ok())
        {
            frames = pipe.wait_for_frames();

            rs2::depth_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();

            if(key_param)
            {
                rs2::stream_profile dprofile =  depth.get_profile();
                rs2::stream_profile cprofile =  color.get_profile();
                // get color intrinsics
                rs2::video_stream_profile cvsprofile(cprofile);
                rs2_intrinsics color_intrin =  cvsprofile.get_intrinsics();

                // get depth intrinsics
                rs2::video_stream_profile dvsprofile(dprofile);
                rs2_intrinsics depth_intrin =  dvsprofile.get_intrinsics();

                color_cameraInfo_ = getCameraInfo(color_intrin);
                depth_cameraInfo_ = getCameraInfo(depth_intrin);
				
				rgb_msg->header.stamp = ros::Time::now();
            	depth_msg->header.stamp = ros::Time::now();
            	
                color_cameraInfo_->header.stamp = rgb_msg->header.stamp;
                color_cameraInfo_->header.frame_id = rgb_msg->header.frame_id;

                depth_cameraInfo_->header.stamp = depth_msg->header.stamp;
                depth_cameraInfo_->header.frame_id = depth_msg->header.frame_id;
                
                key_param = false;
            }

            rgb_pub.publish(rgb_msg, color_cameraInfo_);
            depth_pub.publish(depth_msg, depth_cameraInfo_);
            
            ros::spinOnce();

            rate.sleep();
        }
        
        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;
        ROS_ERROR("[rs_camera_node]: RealSense error calling ");
        return EXIT_FAILURE;
    }
    catch (const std::exception& e)
    {   
        ROS_ERROR("[rs_camera_node]: RealSense failed connect");
        return EXIT_FAILURE;
    }
}