首页 > 编程学习 > 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. 思路分析

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

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

(2)上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的编解码部分.

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

3. 程序开发

(1)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);

(3)V4l2读取相机数据,可参考ros中的usb相机驱动usb_cam, 或者自己写一个, 不再赘述.

(4)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);

(5)发布消息

将编码后的数据塞入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;

4.测试

(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

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

Copyright © 2010-2022 mfbz.cn 版权所有 |关于我们| 联系方式|豫ICP备15888888号