camera数据在ROS2中的publish方式

相机中常用的格式：

1. 点云格式

// 点云格式
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ros_pointCloudPublisher;
if(ros_pointCloudPublisher->get_subscription_count() > 0)
{
   	const int nWidth = static_cast<int>(pointcloudWidth);
   	const int nHeight = static_cast<int>(pointcloudHeight);
   	pcl::PointCloud<pcl::PointXYZ> cloud;
   	sensor_msgs::msg::PointCloud2 output;
   	cloud.points.resize(nWidth * nHeight);
   	float* pCloudSrc = (float*)pointcloudSrc;
   
   	float fUnitRate = 1.0f;//m为单位，mm为单位就设成0.001
   
   	for (int nPos = 0; nPos < nHeight * nWidth; nPos++)
   	{
   		cloud.points[nPos].x = pCloudSrc[nPos*3] * fUnitRate;
   		cloud.points[nPos].y = pCloudSrc[nPos*3+1] * fUnitRate;
   		cloud.points[nPos].z = pCloudSrc[nPos*3+2] * fUnitRate;
   	}
   	pcl::toROSMsg(cloud, output);
   	output.header.frame_id = "camera";
   	ros_pointCloudPublisher->publish(output); 
}

2. IMU传感器

#include <sensor_msgs/msg/imu.hpp>
// IMU
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ros_imuPublisher;
if(ros_imuPublisher->get_subscription_count() > 0)
{
    ImuFrameData* imu_ptr = (ImuFrameData*) dataSrc;

    sensor_msgs::msg::Imu imu; 
    imu.header.stamp = rclcpp::Time(imu_ptr->lAcceTimeStamp/1000, (imu_ptr->lAcceTimeStamp%1000)*1000000);
    imu.header.frame_id = "camera";

    imu.linear_acceleration.x = imu_ptr->fAcce_x * G / 1000.0;
    imu.linear_acceleration.y = imu_ptr->fAcce_y * G / 1000.0;
    imu.linear_acceleration.z = imu_ptr->fAcce_z * G / 1000.0;

    imu.angular_velocity.x = imu_ptr->fGyro_x * M_PI/180.0;
    imu.angular_velocity.y = imu_ptr->fGyro_y * M_PI/180.0;
    imu.angular_velocity.z = imu_ptr->fGyro_z * M_PI/180.0;

    ros_imuPublisher->publish(imu);
}

3. CameraInfo相机内外参

#include <sensor_msgs/msg/camera_info.hpp>
// camera info
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr ros_cameraInfoPublisher;
if(ros_cameraInfoPublisher.get_subscription_count() > 0)
{
	// 将数据填充到CameraInfo消息中
    sensor_msgs::msg::CameraInfo msg;
    msg.header.frame_id = "camera"; // 设置参考坐标系
    msg.width = w;
    msg.height = h;
	msg.d = {ins.K1, ins.K2, ins.P1, 
		ins.P2, ins.K3, ins.K4, 
		ins.K5, ins.K6, ins.S1, 
		ins.S2, ins.S3, ins.S4}; // 畸变系数
	msg.k = {ins.FocalX, 0, ins.CenterX, 
		0, ins.FocalY, ins.CenterY, 
		0, 0, 1}; // 相机内参矩阵
	msg.r = {exs.r[0], exs.r[1], exs.r[2], 
		exs.r[3], exs.r[4], exs.r[5], 
		exs.r[6], exs.r[7], exs.r[8]}; // 旋转矩阵
	msg.p = {ins.FocalX, 0, ins.CenterX, exs.t[0], 
		0, ins.FocalY, ins.CenterY, exs.t[1], 
		0, 0, 1, 0}; // 投影矩阵
    msg.distortion_model = "plumb_bob";

    // 发布CameraInfo消息
    ros_cameraInfoPublisher->publish(msg);
}

4. 压缩格式，针对mjpeg格式

#include <sensor_msgs/msg/compressed_image.h>
// jpg格式
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr ros_jpgPublisher;
if(ros_jpgPublisher.get_subscription_count() > 0)
{
	sensor_msgs::msg::CompressedImage jpgmsg;
	jpgmsg.header.stamp = rclcpp::Time(mataData->lTimeStamp/1000, (mataData->lTimeStamp%1000)*1000000);
	jpgmsg.header.frame_id = "camera";
	jpgmsg.format = "jpeg";
	jpgmsg.data = jpeg_data;
	ros_jpgPublisher->publish(jpgmsg);
}

5. 非压缩格式，针对raw8、raw16、raw32、rgb888、yuv格式

#include <image_transport/image_transport.hpp>
#include <sensor_msgs/msg/image.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
// raw8
image_transport::Publisher ros_irImagePublisher;
if(ros_irImagePublisher.getNumSubscribers() > 0)
{
	cv::Mat irImg = cv::Mat(mataData->iMetaHeight, mataData->iMetaWidth, CV_8UC1, (void *)(mataData->pMetaAddr), cv::Mat::AUTO_STEP);
	sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", irImg).toImageMsg();
	msg->header.stamp = rclcpp::Time(mataData->lTimeStamp/1000, (mataData->lTimeStamp%1000)*1000000);
	ros_irImagePublisher.publish(msg);
}

// raw16
image_transport::Publisher ros_depthImagePublisher;
if(ros_depthImagePublisher.getNumSubscribers() > 0)
{
	cv::Mat depthImg = cv::Mat(mataData->iMetaHeight, mataData->iMetaWidth, CV_16UC1, (void *)(mataData->pMetaAddr), cv::Mat::AUTO_STEP);
	sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", depthImg).toImageMsg();
	msg->header.stamp = rclcpp::Time(mataData->lTimeStamp/1000, (mataData->lTimeStamp%1000)*1000000);
	ros_depthImagePublisher.publish(msg);
}

// rgb888
image_transport::Publisher ros_rgbImagePublisher;
if(ros_rgbImagePublisher.getNumSubscribers() > 0)
{
    cv::Mat rgbdImg = cv::Mat(mataDataRgb.iMetaHeight, mataDataRgb.iMetaWidth, CV_8UC3, (void *)(mataDataRgb.pMetaAddr), cv::Mat::AUTO_STEP);

    sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "rgb8", rgbdImg).toImageMsg();
    msg->header.stamp = rclcpp::Time(mataDataRgb.lTimeStamp/1000, (mataDataRgb.lTimeStamp%1000)*1000000);
    ros_rgbImagePublisher.publish(msg);
}