先上代码:
#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;
}
}
主要踩坑点在发布和内参控制部分,发布的时候照片类型是sensor_msgs::ImagePtr,相机数据类型是sensor_msgs::CameraInfoPtr,使用image_transport::CameraPublisher 来发布消息,否则没法同时发布图像和相机信息。