ROS各消息类型的data理解

1.sensor_msgs::Image

• data按rgbrgb…的方式保存图像

#include <fstream>
#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;

int data[3 * 640 * 480];

void array2mat(const std::vector<unsigned char> data, int width, int height)
{
  cv::Mat src = cv::Mat::zeros(height, width, CV_8UC3);
  for (int i = 0; i < height; ++i) 
  {
    for (int j = 0; j < width; j++)
    {
      src.at<cv::Vec3b>(i, j)[0] = data[(width * i + j)*3+2];
      src.at<cv::Vec3b>(i, j)[1] = data[(width * i + j)*3+1];
      src.at<cv::Vec3b>(i, j)[2] = data[(width * i + j)*3];
      //cout << (int)src.at<cv::Vec3b>(i, j)[0] << endl;
      //cout << (int)src.at<cv::Vec3b>(i, j)[1] << endl;
      //cout << (int)src.at<cv::Vec3b>(i, j)[2] << endl;
    }
  }
  cv::imshow("img", src);
  cv::waitKey(10);
}

void CallBack(const sensor_msgs::Image::ConstPtr& img) {
  printf("%d  %d\n", img->height, img->width);
  array2mat(img->data, img->width, img->height);
}

2.sensor_msgs::PointCloud2

• 点云属性
  • height: 1 #表示无序点云
  • width: 1249 #无序点云的点数
  • fields:x,y,z #float，各占4个字节
  • is_bigendian: False #大小端，决定字节如何转换成float
  • point_step: 16 #一个点占多少字节
  • row_step: 19984 #点云共占多少字节，即width×point_step，也是data的元素个数

typedef union FLOAT_CONV {
  float f;
  char c[4];
} float_conv;

float BLEndianFloat(char c0, char c1, char c2, char c3) {
  float_conv d2;
  d2.c[0] = c0;
  d2.c[1] = c1;
  d2.c[2] = c2;
  d2.c[3] = c3;
  return d2.f;
}

void cb(const sensor_msgs::PointCloud2::ConstPtr& pc) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>());
    tmp->height = pc->height;
    tmp->width = pc->width;
    tmp->points.resize(pc->width * pc->height);
    for (int i = 0; i < pc->row_step; i += 16) {
      float x = BLEndianFloat(pc->data[i], pc->data[i + 1], pc->data[i + 2], pc->data[i + 3]);
      float y = BLEndianFloat(pc->data[i + 4], pc->data[i + 5], pc->data[i + 6], pc->data[i + 7]);
      float z = BLEndianFloat(pc->data[i + 8], pc->data[i + 9], pc->data[i + 10], pc->data[i + 11]);
      tmp->points[i / 16].x = x;
      tmp->points[i / 16].y = y;
      tmp->points[i / 16].z = z;
    }
}

3.sensor_msgs/LaserScan

• 将angle_min和angle_max之间的角度以angle_increment均分，每个角度的距离存在ranges中

for (int i = 0; i < scan->ranges.size(); ++i) {
	if(laser->ranges[i] < laser->range_min || laser->ranges[i] > laser->range_max) continue;
	double angle = scan->angle_min + i * scan->angle_increment;
	double x = cos(angle) * scan->ranges[i];
	double y = sin(angle) * scan->ranges[i];
}