赞
踩
在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资源.
(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出来就可以了.
(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;
(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设计的域控都支持获得触发时间戳.要想做好自动驾驶,在数据之前的基础的传感器时空同步联合标定一定得做到位.
水平和时间有限,了解不够细致,欢迎留言交流.
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。