Xavier 下GMSL相机ROS驱动发布CompressedImage消息(基于NVJPG硬件编码)

1. 背景

在Xavier接入多个gmsl相机，采用yuv转rgb，再通过cv_bridge转成ros消息发出来的方式太耗费cpu,同时运行多路相机驱动会造成系统卡顿. Xavier上支持硬件的编解码，使用jtop可以看到硬件资源如下图，之前采用cpu将yuv转rgb后，还需要将采用image_transport（https://wiki.ros.org/image_transport）数据发布出来,在发布原始话题（sensor_msgs/Image）的时候，同时发布压缩图像（sensor_msgs/CompressedImage）话题，为了减小录制的数据量，方便离线仿真测试，通常仅录制压缩图像消息．因此考虑利用Nvidia自身的硬件编解码能力，直接将yuv数据编码为jpeg格式发布出来,一步到位,从而节省cpu资源．

 2. 思路分析

（１）有了初步想法后，第一反应搜github看有没有别人写好的驱动，没有找到可以直接使用的demo,不过还是发现了一些有用的工程，例如https://github.com/peter-moran/jetson_csi_cam

后面给CompressedImage塞数据就参考了这(https://github.com/ros-drivers/gscam/blob/master/src/gscam.cpp#L343)里面的写法．

（２）上nvidia官方找demo，参考https://docs.nvidia.com/jetson/l4t-multimedia/中的Multimedia APIs, 给出了一些使用底层资源的demo. Xavier刷机的过程中一般会选择安装Multimedia模块，这些demo都在系统/usr/src/jetson_multimedia_api/下面，参考官方说明(https://docs.nvidia.com/jetson/l4t-multimedia/group__l4t__mm__test__group.html)把相关demo都跑了一下,重点关注jpeg的编解码部分．

 （３）06_jpeg_encode的demo是读取一张yuv文件，转换成jpg后再写入文件，关键函数encodeFromFd(这个因为自身水平有限，暂时不理解Fd准备先搞buffer传数据的)或者encodeFromBuffer．找到了熟悉的东西，下面我就需要考虑如何把相机读出来的yuv数据按照nvidia API要求输入，再将编码后的jpeg按照CompressedImage要求塞进去publish出来就可以了．

3. 程序开发

（１）encodeFromBuffer

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

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

yuv平面格式即

yyyy yyyy yyyy yyyy
uuuu
vvvv

(2) 根据自己相机数据格式转到yuv420 planar模式，格式转换可参考开源项目libyuv

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

libyuv/convert.h中定义

// 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);

（３）V4l2读取相机数据，可参考ros中的usb相机驱动usb_cam, 或者自己写一个, 不再赘述．

（４）encodeFromBuffer的输入为NvBuffer，libyuv转换得到的yuv420为长度IMAGEWIDTH*IMAGEHEIGHT*3/2的unsigned char数组．根据06_jpeg_encode示例程序中读yuv文件的程序可知(加入一些cout, 运行demo确定参数值), Nvbuffer YUV420格式下有三个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;

４．测试

(1) gmsl2 1920*1080 30hz　相机，采用NVJPG编码jpeg发布ros消息在jetson xavier MAXN　模式下cpu占用不到10％(采用gmsl转usb3的采集盒)，相比基于opencv cvtColor函数采用cpu进行yuv转rgb转换（COLOR_YUV2BGR_UYVY）cpu占用80%下降了70%,大大提高了Xavier挂载相机的数目，充分利用了其硬件资源．

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

NOTE

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