通达信dll接口图解-Xavier 下GMSL相机ROS驱动发布CompressedImage消息

Xavier 下GMSL相机ROS驱动发布CompressedImage消息

思路分析

06_jpeg_encode的demo是读取一张yuv文件，转换成jpg后再写入文件，关键函数encodeFromFd或者encodeFromBuffer．找到了熟悉的东西，下面我就需要考虑如何把相机读出来的yuv数据按照nvidiaAPI要求输入，再将编码后的jpeg按照CompressedImage要求塞进去publish出来就可以了．

程序开发

根据上面API接口说明可知encodeFromBuffer仅支持YUV420格式，06demo给出的示例默认设置输入片格式为V4L2_PIX_FMT_YUV420M，定义在/usr/include/linux/videodevh577行

#define V4L2_PIX_FMT_YUV420M v4l2_fourcc("Y", "M", "1", "2") /* 12 YUV420 planar */

yuv平面格式即

yyyy yyyy yyyy yyyy
uuuu
vvvv

基于libyuv，将yuv422数据转到I420.如果相机输出UYVY,则采用UYVYToI420，如果相机输出为YUYV,则采用YUY2ToI420

// Convert YUY2 to I420.
LIBYUV_API
int YUY2ToI420(const uint8_t* src_yuy2,
               int src_stride_yuy2,
               uint8_t* dst_y,
               int dst_stride_y,
               uint8_t* dst_u,
               int dst_stride_u,
               uint8_t* dst_v,
               int dst_stride_v,
               int width,
               int height);

// Convert UYVY to I420.
LIBYUV_API
int UYVYToI420(const uint8_t* src_uyvy,
               int src_stride_uyvy,
               uint8_t* dst_y,
               int dst_stride_y,
               uint8_t* dst_u,
               int dst_stride_u,
               uint8_t* dst_v,
               int dst_stride_v,
               int width,
               int height);

所以输入即v4l2读出的buffer指针buffers[buf.index].start，输出m_pYuv，调用如下：

  unsigned long len = IMAGEWIDTH*IMAGEHEIGHT*3/2;
  unsigned char* m_pYuv = new unsigned char[len];　// yuv420 planar data
    // ｀｀｀省略
  libyuv::UYVYToI420((unsigned char *)buffers[buf.index].start,2*IMAGEWIDTH,
                                          m_pYuv,IMAGEWIDTH,
                                          m_pYuv+IMAGEWIDTH*IMAGEHEIGHT,IMAGEWIDTH/2,
                                          m_pYuv+5*IMAGEWIDTH*IMAGEHEIGHT/4,IMAGEWIDTH/2,
                                          IMAGEWIDTH,
                                          IMAGEHEIGHT);

encodeFromBuffer的输入为NvBuffer，libyuv转换得到的yuv420为长度IMAGEWIDTH*IMAGEHEIGHT*3/2的unsignedchar数组．根据06_jpeg_encode示例程序中读yuv文件的程序可知,NvbufferYUV420格式下有三个plane,按照格式定义将yuv参数拷到对应的data即可．

照着葫芦画瓢，把m_pYuv指针指向的数据拷贝到nvbuffer中去，代码如下：

 unsigned char* out_buf = new unsigned char[len];　// jpeg data
 NvBuffer nvbuffer(V4L2_PIX_FMT_YUV420M, ctx.in_width,
        ctx.in_height, 0);
  nvbuffer.allocateMemory();
// ... 省略
      int i;
      char *data;
      int p_index = 0;
      for (i = 0; i < nvbuffer.n_planes; i++)
      {
          NvBuffer::NvBufferPlane &plane = nvbuffer.planes[i];
          data = (char *) plane.data;    
          plane.bytesused = plane.fmt.stride * plane.fmt.height;
          memcpy(data,&m_pYuv[p_index],plane.bytesused);
          p_index += plane.bytesused;
      }   
ret = ctx.jpegenc->encodeFromBuffer(nvbuffer, JCS_YCbCr, &out_buf,
                            len, ctx.quality);

发布消息

将编码后的数据塞入ros的CompressedImage消息发布出去即可．

// ... 省略
      sensor_msgs::CompressedImagePtr img(new sensor_msgs::CompressedImage());
      img->header = image_header;
      img->format = "jpeg";
      img->data.resize((int)len);
      std::copy(out_buf, out_buf+(int)len, img->data.begin());
      m_jpeg_pub_.publish(img);  
// ... 省略
delete[] m_pYuv;
delete[] out_buf;

４．测试

gmsl21920*108030hz相机，采用NVJPG编码jpeg发布ros消息在jetsonxavierMAXN模式下cpu占用不到10％，相比基于opencvcvtColor函数采用cpu进行yuv转rgb转换cpu占用80%下降了70%,大大提高了Xavier挂载相机的数目，充分利用了其硬件资源．

实际测试从拍照到看到有100-130ms的延迟，gmsl相机从触发串行到解串需要一定的时间无法避免，所以v4l2读到数据的时间并不是真正的拍照时间，需要配合硬件设计拿到触发时间戳，这样才能保证后面激光雷达相机的同步足够精确，目前米文天准等基于Xavier设计的域控都支持获得触发时间戳．要想做好自动驾驶，在数据之前的基础的传感器时空同步联合标定一定得做到位．

水平和时间有限，了解不够细致，欢迎留言交流．

文章为作者独立观点，不代表观点

通达信dll接口图解-接口