Apollo 11.0
自动驾驶开放平台
usb_cam.cc
浏览该文件的文档.
1/*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2014, Robert Bosch LLC.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Robert Bosch nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 *********************************************************************/
36
37#include <cmath>
38#include <string>
39
40#ifndef __aarch64__
41#include "adv_trigger.h"
42#include "modules/drivers/camera/util.h"
43#else
44#include <cuda_runtime.h>
45#include "arm_neon.h"
46#include "boost/lexical_cast.hpp"
47#include "sys/sysinfo.h"
48#endif
49
50#include "modules/drivers/camera/usb_cam.h"
51
52#define __STDC_CONSTANT_MACROS
53
54#define CLEAR(x) memset(&(x), 0, sizeof(x))
55
56namespace apollo {
57namespace drivers {
58namespace camera {
59
60using apollo::drivers::camera::config::Config;
61using apollo::drivers::camera::config::IO_METHOD_MMAP;
62using apollo::drivers::camera::config::IO_METHOD_READ;
63using apollo::drivers::camera::config::IO_METHOD_UNKNOWN;
64using apollo::drivers::camera::config::IO_METHOD_USERPTR;
65using apollo::drivers::camera::config::RGB;
66using apollo::drivers::camera::config::YUYV;
67
68UsbCam::UsbCam() :
69 fd_(-1),
70#ifdef __aarch64__
71 time_compensator_fd_(-1),
72#endif
73 buffers_(NULL),
74 n_buffers_(0),
75 is_capturing_(false),
76 image_seq_(0),
77 device_wait_sec_(2),
78 last_nsec_(0),
79 frame_drop_interval_(0.0) {
80}
81
82UsbCam::~UsbCam() {
83 stop_capturing();
84 uninit_device();
85 close_device();
86#ifdef __aarch64__
87 handler_.Destory();
88#endif
89}
90
91bool UsbCam::init(const std::shared_ptr<Config>& cameraconfig) {
92 config_ = cameraconfig;
93
94 if (config_->pixel_format() == "yuyv") {
95 pixel_format_ = V4L2_PIX_FMT_YUYV;
96 } else if (config_->pixel_format() == "uyvy") {
97 pixel_format_ = V4L2_PIX_FMT_UYVY;
98 } else if (config_->pixel_format() == "mjpeg") {
99 pixel_format_ = V4L2_PIX_FMT_MJPEG;
100 } else if (config_->pixel_format() == "yuvmono10") {
101 pixel_format_ = V4L2_PIX_FMT_YUYV;
102 config_->set_monochrome(true);
103 } else if (config_->pixel_format() == "rgb24") {
104 pixel_format_ = V4L2_PIX_FMT_RGB24;
105 } else {
106 pixel_format_ = V4L2_PIX_FMT_YUYV;
107 AERROR << "Wrong pixel fromat:" << config_->pixel_format()
108 << ",must be yuyv | uyvy | mjpeg | yuvmono10 | rgb24";
109 return false;
110 }
111 if (pixel_format_ == V4L2_PIX_FMT_MJPEG) {
112 init_mjpeg_decoder(config_->width(), config_->height());
113 }
114
115 // Warning when diff with last > 1.5* interval
116 frame_warning_interval_ = static_cast<float>(1.5 / config_->frame_rate());
117 // now max fps 30, we use an appox time 0.9 to drop image.
118 frame_drop_interval_ = static_cast<float>(0.9 / config_->frame_rate());
119 frame_drop_interval_ns_ = static_cast<uint64_t>(frame_drop_interval_ * 1000 * 1000 * 1000);
120
121#ifdef __aarch64__
122 handler_ = CudaConvertHandler(config_->width(), config_->height());
123 if (!handler_.Init()) {
124 AERROR << "Init cuda convert handler error";
125 return false;
126 }
127#endif
128
129 return true;
130}
131
132int UsbCam::init_mjpeg_decoder(int image_width, int image_height) {
133 avcodec_register_all();
134
135 avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
136 if (!avcodec_) {
137 AERROR << "Could not find MJPEG decoder";
138 return 0;
139 }
140
141 avcodec_context_ = avcodec_alloc_context3(avcodec_);
142
143#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(57, 0, 0)
144 avframe_camera_ = av_frame_alloc();
145 avframe_rgb_ = av_frame_alloc();
146
147 avpicture_alloc(reinterpret_cast<AVPicture*>(avframe_rgb_), AV_PIX_FMT_RGB24, image_width, image_height);
148#else
149 avframe_camera_ = avcodec_alloc_frame();
150 avframe_rgb_ = avcodec_alloc_frame();
151
152 avpicture_alloc(reinterpret_cast<AVPicture*>(avframe_rgb_), PIX_FMT_RGB24, image_width, image_height);
153#endif
154
155 avcodec_context_->codec_id = AV_CODEC_ID_MJPEG;
156 avcodec_context_->width = image_width;
157 avcodec_context_->height = image_height;
158
159#if LIBAVCODEC_VERSION_MAJOR > 52
160#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(57, 0, 0)
161 avcodec_context_->pix_fmt = AV_PIX_FMT_YUV422P;
162#else
163 avcodec_context_->pix_fmt = PIX_FMT_YUV422P;
164#endif
165 avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
166#endif
167
168#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(57, 0, 0)
169 avframe_camera_size_ = avpicture_get_size(AV_PIX_FMT_YUV422P, image_width, image_height);
170 avframe_rgb_size_ = avpicture_get_size(AV_PIX_FMT_RGB24, image_width, image_height);
171#else
172 avframe_camera_size_ = avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
173 avframe_rgb_size_ = avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
174#endif
175
176 /* open it */
177 if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0) {
178 AERROR << "Could not open MJPEG Decoder";
179 return 0;
180 }
181 return 1;
182}
183
184void UsbCam::mjpeg2rgb(char* mjpeg_buffer, int len, char* rgb_buffer, int NumPixels) {
185 (void)NumPixels;
186 int got_picture = 0;
187
188 memset(rgb_buffer, 0, avframe_rgb_size_);
189
190#if LIBAVCODEC_VERSION_MAJOR > 52
191 int decoded_len;
192 AVPacket avpkt;
193 av_init_packet(&avpkt);
194
195 avpkt.size = len;
196 avpkt.data = (unsigned char*)mjpeg_buffer;
197 decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_, &got_picture, &avpkt);
198
199 if (decoded_len < 0) {
200 AERROR << "Error while decoding frame.";
201 return;
202 }
203#else
204 avcodec_decode_video(
205 avcodec_context_, avframe_camera_, &got_picture, reinterpret_cast<uint8_t*>(mjpeg_buffer), len);
206#endif
207
208 if (!got_picture) {
209 AERROR << "Camera: expected picture but didn't get it...";
210 return;
211 }
212
213 int xsize = avcodec_context_->width;
214 int ysize = avcodec_context_->height;
215 int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
216 if (pic_size != avframe_camera_size_) {
217 AERROR << "outbuf size mismatch. pic_size:" << pic_size << ",buffer_size:" << avframe_camera_size_;
218 return;
219 }
220
221#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(57, 0, 0)
222 video_sws_ = sws_getContext(
223 xsize,
224 ysize,
225 avcodec_context_->pix_fmt,
226 xsize,
227 ysize,
228 AV_PIX_FMT_RGB24,
229 SWS_BILINEAR,
230 nullptr,
231 nullptr,
232 nullptr);
233#else
234 video_sws_ = sws_getContext(
235 xsize,
236 ysize,
237 avcodec_context_->pix_fmt,
238 xsize,
239 ysize,
240 PIX_FMT_RGB24,
241 SWS_BILINEAR,
242 nullptr,
243 nullptr,
244 nullptr);
245#endif
246
247 sws_scale(
248 video_sws_,
249 avframe_camera_->data,
250 avframe_camera_->linesize,
251 0,
252 ysize,
253 avframe_rgb_->data,
254 avframe_rgb_->linesize);
255 sws_freeContext(video_sws_);
256
257#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(57, 0, 0)
258 int size = avpicture_layout(
259 reinterpret_cast<AVPicture*>(avframe_rgb_),
260 AV_PIX_FMT_RGB24,
261 xsize,
262 ysize,
263 reinterpret_cast<uint8_t*>(rgb_buffer),
264 avframe_rgb_size_);
265#else
266 int size = avpicture_layout(
267 reinterpret_cast<AVPicture*>(avframe_rgb_),
268 PIX_FMT_RGB24,
269 xsize,
270 ysize,
271 reinterpret_cast<uint8_t*>(rgb_buffer),
272 avframe_rgb_size_);
273#endif
274 if (size != avframe_rgb_size_) {
275 AERROR << "webcam: avpicture_layout error: " << size;
276 return;
277 }
278}
279
280CameraDeviceState UsbCam::poll(const CameraImagePtr& raw_image, const CameraImagePtr& sensor_raw_image) {
281 raw_image->is_new = 0;
282 // no need to memeset
283 // memset(raw_image->image, 0, raw_image->image_size * sizeof(char));
284
285 int r = 0;
286
287 FD_ZERO(&fds_);
288 FD_SET(fd_, &fds_);
289
290 /* Timeout. */
291 tv_.tv_sec = 2;
292 tv_.tv_usec = 0;
293
294 r = select(fd_ + 1, &fds_, nullptr, nullptr, &tv_);
295
296 if (-1 == r) {
297 if (EINTR == errno) {
298 return CameraDeviceState::STATE_INTR;
299 }
300
301 // errno_exit("select");
302 reconnect();
303 }
304
305 if (0 == r) {
306 AERROR << "select timeout";
307 reconnect();
308 }
309
310 CameraDeviceState s = read_frame(raw_image, sensor_raw_image);
311
312 if (s == CameraDeviceState::STATE_SUCCESS) {
313 raw_image->is_new = 1;
314 }
315
316 return s;
317}
318
319bool UsbCam::open_device(void) {
320 struct stat st;
321
322 if (-1 == stat(config_->camera_dev().c_str(), &st)) {
323 AERROR << "Cannot identify '" << config_->camera_dev() << "': " << errno << ", " << strerror(errno);
324 return false;
325 }
326
327 if (!S_ISCHR(st.st_mode)) {
328 AERROR << config_->camera_dev() << " is no device";
329 return false;
330 }
331
332 fd_ = open(config_->camera_dev().c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
333
334 if (-1 == fd_) {
335 AERROR << "Cannot open '" << config_->camera_dev() << "': " << errno << ", " << strerror(errno);
336 return false;
337 }
338
339#ifdef __aarch64__
340 if (config_->has_time_compensator_conf() && config_->time_compensator_conf().enable_compensator()) {
341 time_compensator_fd_
342 = open(config_->time_compensator_conf().compensator_fnode().c_str(), O_RDONLY | O_NONBLOCK);
343 if (-1 == time_compensator_fd_) {
344 AERROR << "Cannot open '" << config_->time_compensator_conf().compensator_fnode() << "': " << errno << ", "
345 << strerror(errno);
346 return false;
347 }
348 }
349#endif
350
351 return true;
352}
353
354bool UsbCam::init_device(void) {
355 struct v4l2_capability cap;
356 struct v4l2_cropcap cropcap;
357 struct v4l2_crop crop;
358 struct v4l2_format fmt;
359 unsigned int min = 0;
360
361 if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
362 if (EINVAL == errno) {
363 AERROR << config_->camera_dev() << " is no V4L2 device";
364 return false;
365 }
366 AERROR << "VIDIOC_QUERYCAP";
367 return false;
368 }
369
370 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
371 AERROR << config_->camera_dev() << " is no video capture device";
372 return false;
373 }
374
375 switch (config_->io_method()) {
376 case IO_METHOD_READ:
377 if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
378 AERROR << config_->camera_dev() << " does not support read i/o";
379 return false;
380 }
381
382 break;
383
384 case IO_METHOD_MMAP:
385 case IO_METHOD_USERPTR:
386 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
387 AERROR << config_->camera_dev() << " does not support streaming i/o";
388 return false;
389 }
390
391 break;
392 case IO_METHOD_UNKNOWN:
393 AERROR << config_->camera_dev() << " does not support unknown i/o";
394 return false;
395 break;
396 }
397
398 /* Select video input, video standard and tune here. */
399 CLEAR(cropcap);
400
401 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
402
403 if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap)) {
404 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
405 crop.c = cropcap.defrect; /* reset to default */
406
407 if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop)) {
408 switch (errno) {
409 case EINVAL:
410 /* Cropping not supported. */
411 break;
412 }
413 }
414 }
415
416 CLEAR(fmt);
417
418 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
419 fmt.fmt.pix.width = config_->width();
420 fmt.fmt.pix.height = config_->height();
421 fmt.fmt.pix.pixelformat = pixel_format_;
422 fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
423
424 if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) {
425 AERROR << "VIDIOC_S_FMT";
426 return false;
427 }
428
429 /* Note VIDIOC_S_FMT may change width and height. */
430
431 /* Buggy driver paranoia. */
432 min = fmt.fmt.pix.width * 2;
433
434 if (fmt.fmt.pix.bytesperline < min) {
435 fmt.fmt.pix.bytesperline = min;
436 }
437
438 min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
439
440 if (fmt.fmt.pix.sizeimage < min) {
441 fmt.fmt.pix.sizeimage = min;
442 }
443
444 config_->set_width(fmt.fmt.pix.width);
445 config_->set_height(fmt.fmt.pix.height);
446
447 struct v4l2_streamparm stream_params;
448 memset(&stream_params, 0, sizeof(stream_params));
449 stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
450
451 // if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0) {
452 // // errno_exit("Couldn't query v4l fps!");
453 // AERROR << "Couldn't query v4l fps!";
454 // // reconnect();
455 // return false;
456 // }
457
458 AINFO << "Capability flag: 0x" << stream_params.parm.capture.capability;
459
460 stream_params.parm.capture.timeperframe.numerator = 1;
461 stream_params.parm.capture.timeperframe.denominator = config_->frame_rate();
462
463 if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0) {
464 AINFO << "Couldn't set camera framerate";
465 } else {
466 AINFO << "Set framerate to be " << config_->frame_rate();
467 }
468
469 switch (config_->io_method()) {
470 case IO_METHOD_READ:
471 init_read(fmt.fmt.pix.sizeimage);
472 break;
473
474 case IO_METHOD_MMAP:
475 init_mmap();
476 break;
477
478 case IO_METHOD_USERPTR:
479 init_userp(fmt.fmt.pix.sizeimage);
480 break;
481
482 case IO_METHOD_UNKNOWN:
483 AERROR << " does not support unknown i/o";
484 break;
485 }
486
487 return true;
488}
489
490#ifndef __aarch64__
491bool UsbCam::set_adv_trigger() {
492 AINFO << "Trigger enable, dev:" << config_->camera_dev() << ", fps:" << config_->trigger_fps()
493 << ", internal:" << config_->trigger_internal();
494 int ret = adv_trigger_enable(
495 config_->camera_dev().c_str(),
496 static_cast<unsigned char>(config_->trigger_fps()),
497 static_cast<unsigned char>(config_->trigger_internal()));
498 if (ret != 0) {
499 AERROR << "trigger failed:" << ret;
500 return false;
501 }
502 return true;
503}
504#endif
505
506int UsbCam::xioctl(int fd, int request, void* arg) {
507 int r = 0;
508 do {
509 r = ioctl(fd, request, arg);
510 } while (-1 == r && EINTR == errno);
511
512 return r;
513}
514
515bool UsbCam::init_read(unsigned int buffer_size) {
516 buffers_ = reinterpret_cast<buffer*>(calloc(1, sizeof(*buffers_)));
517
518 if (!buffers_) {
519 AERROR << "Out of memory";
520 // exit(EXIT_FAILURE);
521 // reconnect();
522 return false;
523 }
524
525 buffers_[0].length = buffer_size;
526 buffers_[0].start = malloc(buffer_size);
527
528 if (!buffers_[0].start) {
529 AERROR << "Out of memory";
530 return false;
531 }
532
533 return true;
534}
535
536bool UsbCam::init_mmap(void) {
537 struct v4l2_requestbuffers req;
538 CLEAR(req);
539
540 req.count = 1;
541 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
542 req.memory = V4L2_MEMORY_MMAP;
543
544 if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
545 if (EINVAL == errno) {
546 AERROR << config_->camera_dev() << " does not support memory mapping";
547 return false;
548 }
549 return false;
550 }
551
552 buffers_ = reinterpret_cast<buffer*>(calloc(req.count, sizeof(*buffers_)));
553
554 if (!buffers_) {
555 AERROR << "Out of memory";
556 return false;
557 }
558
559 for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_) {
560 struct v4l2_buffer buf;
561 CLEAR(buf);
562
563 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
564 buf.memory = V4L2_MEMORY_MMAP;
565 buf.index = n_buffers_;
566
567 if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) {
568 AERROR << "VIDIOC_QUERYBUF";
569 return false;
570 }
571
572 buffers_[n_buffers_].length = buf.length;
573 buffers_[n_buffers_].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, buf.m.offset);
574#ifdef __aarch64__
575#define CUDA_ZERO_COPY
576 cudaError_t err = cudaHostRegister(buffers_[n_buffers_].start, buf.length, cudaHostRegisterMapped);
577 if (err != cudaSuccess) {
578 AERROR << "Failed to register mapped memory: " << cudaGetErrorString(err);
579 return false;
580 }
581 err = cudaHostGetDevicePointer(&buffers_[n_buffers_].cuda_data, buffers_[n_buffers_].start, 0);
582 if (err != cudaSuccess) {
583 AERROR << "Failed to get device pointer: " << cudaGetErrorString(err);
584 cudaHostUnregister(buffers_[n_buffers_].start);
585 return false;
586 }
587#endif
588
589 if (MAP_FAILED == buffers_[n_buffers_].start) {
590 AERROR << "mmap";
591 return false;
592 }
593 }
594
595 return true;
596}
597
598bool UsbCam::init_userp(unsigned int buffer_size) {
599 struct v4l2_requestbuffers req;
600 unsigned int page_size = 0;
601
602 page_size = getpagesize();
603 buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
604
605 CLEAR(req);
606
607 req.count = 4;
608 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
609 req.memory = V4L2_MEMORY_USERPTR;
610
611 if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
612 if (EINVAL == errno) {
613 AERROR << config_->camera_dev()
614 << " does not support "
615 "user pointer i/o";
616 return false;
617 }
618 AERROR << "VIDIOC_REQBUFS";
619 return false;
620 }
621
622 buffers_ = reinterpret_cast<buffer*>(calloc(4, sizeof(*buffers_)));
623
624 if (!buffers_) {
625 AERROR << "Out of memory";
626 return false;
627 }
628
629 for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_) {
630 buffers_[n_buffers_].length = buffer_size;
631 buffers_[n_buffers_].start = memalign(/* boundary */ page_size, buffer_size);
632
633 if (!buffers_[n_buffers_].start) {
634 AERROR << "Out of memory";
635 return false;
636 }
637 }
638
639 return true;
640}
641
642bool UsbCam::start_capturing(void) {
643 if (is_capturing_) {
644 return true;
645 }
646
647 unsigned int i = 0;
648 enum v4l2_buf_type type;
649
650 switch (config_->io_method()) {
651 case IO_METHOD_READ:
652 /* Nothing to do. */
653 break;
654
655 case IO_METHOD_MMAP:
656 for (i = 0; i < n_buffers_; ++i) {
657 struct v4l2_buffer buf;
658 CLEAR(buf);
659
660 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
661 buf.memory = V4L2_MEMORY_MMAP;
662 buf.index = i;
663 if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
664 AERROR << "VIDIOC_QBUF";
665 return false;
666 }
667 }
668
669 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
670
671 if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
672 AERROR << "VIDIOC_STREAMON";
673 return false;
674 }
675
676 break;
677
678 case IO_METHOD_USERPTR:
679 for (i = 0; i < n_buffers_; ++i) {
680 struct v4l2_buffer buf;
681
682 CLEAR(buf);
683
684 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
685 buf.memory = V4L2_MEMORY_USERPTR;
686 buf.index = i;
687 buf.m.userptr = reinterpret_cast<uint64_t>(buffers_[i].start);
688 buf.length = static_cast<unsigned int>(buffers_[i].length);
689
690 if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
691 AERROR << "VIDIOC_QBUF";
692 return false;
693 }
694 }
695
696 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
697
698 if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
699 AERROR << "VIDIOC_STREAMON";
700 return false;
701 }
702
703 break;
704
705 case IO_METHOD_UNKNOWN:
706 AERROR << "unknown IO";
707 return false;
708 break;
709 }
710
711 is_capturing_ = true;
712 return true;
713}
714
715void UsbCam::set_device_config() {
716 if (config_->brightness() >= 0) {
717 set_v4l_parameter("brightness", config_->brightness());
718 }
719
720 if (config_->contrast() >= 0) {
721 set_v4l_parameter("contrast", config_->contrast());
722 }
723
724 if (config_->saturation() >= 0) {
725 set_v4l_parameter("saturation", config_->saturation());
726 }
727
728 if (config_->sharpness() >= 0) {
729 set_v4l_parameter("sharpness", config_->sharpness());
730 }
731
732 if (config_->gain() >= 0) {
733 set_v4l_parameter("gain", config_->gain());
734 }
735
736 // check auto white balance
737 if (config_->auto_white_balance()) {
738 set_v4l_parameter("white_balance_temperature_auto", 1);
739 } else {
740 set_v4l_parameter("white_balance_temperature_auto", 0);
741 set_v4l_parameter("white_balance_temperature", config_->white_balance());
742 }
743
744 // check auto exposure
745 if (!config_->auto_exposure()) {
746 // turn down exposure control (from max of 3)
747 set_v4l_parameter("auto_exposure", 1);
748 // change the exposure level
749 set_v4l_parameter("exposure_absolute", config_->exposure());
750 }
751
752 // check auto focus
753 if (config_->auto_focus()) {
754 set_auto_focus(1);
755 set_v4l_parameter("focus_auto", 1);
756 } else {
757 set_v4l_parameter("focus_auto", 0);
758 if (config_->focus() >= 0) {
759 set_v4l_parameter("focus_absolute", config_->focus());
760 }
761 }
762}
763
764bool UsbCam::uninit_device(void) {
765 unsigned int i = 0;
766
767 switch (config_->io_method()) {
768 case IO_METHOD_READ:
769 free(buffers_[0].start);
770 break;
771
772 case IO_METHOD_MMAP:
773 for (i = 0; i < n_buffers_; ++i) {
774 if (-1 == munmap(buffers_[i].start, buffers_[i].length)) {
775 AERROR << "munmap";
776 return false;
777 }
778 }
779
780 break;
781
782 case IO_METHOD_USERPTR:
783 for (i = 0; i < n_buffers_; ++i) {
784 free(buffers_[i].start);
785 }
786
787 break;
788
789 case IO_METHOD_UNKNOWN:
790 AERROR << "unknown IO";
791 break;
792 }
793
794 return true;
795}
796
797bool UsbCam::close_device(void) {
798#ifdef __aarch64__
799 if (-1 == close(fd_) || -1 == close(time_compensator_fd_)) {
800#else
801 if (-1 == close(fd_)) {
802#endif
803 AERROR << "close";
804 return false;
805 }
806
807#ifdef __aarch64__
808 time_compensator_fd_ = -1;
809#endif
810 fd_ = -1;
811 return true;
812}
813
814bool UsbCam::stop_capturing(void) {
815 if (!is_capturing_) {
816 return true;
817 }
818
819 is_capturing_ = false;
820 enum v4l2_buf_type type;
821
822 switch (config_->io_method()) {
823 case IO_METHOD_READ:
824 /* Nothing to do. */
825 break;
826
827 case IO_METHOD_MMAP:
828 case IO_METHOD_USERPTR:
829 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
830
831 if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type)) {
832 AERROR << "VIDIOC_STREAMOFF";
833 return false;
834 }
835
836 break;
837 case IO_METHOD_UNKNOWN:
838 AERROR << "unknown IO";
839 return false;
840 break;
841 }
842
843 return true;
844}
845
846CameraDeviceState UsbCam::read_frame(CameraImagePtr raw_image, CameraImagePtr sensor_raw_image) {
847 struct v4l2_buffer buf;
848 unsigned int i = 0;
849 int len = 0;
850
851 timeval sample_ts_monotonic;
852 timespec current_ts_monotonic;
853 uint64_t sample_ts_monotonic_ns, current_ts_monotonic_ns, diff_ns, sample_in_systme_time_ns;
854
855 if (cyber::Time::Now().ToNanosecond() - last_error_time_ns_ < frame_drop_interval_ns_) {
856 auto d = std::chrono::microseconds(
857 (last_error_time_ns_ + frame_drop_interval_ns_) / 1000);
858 std::this_thread::sleep_until(std::chrono::system_clock::time_point(d));
859 }
860
861 switch (config_->io_method()) {
862 case IO_METHOD_READ:
863 len = static_cast<int>(read(fd_, buffers_[0].start, buffers_[0].length));
864
865 if (len == -1) {
866 switch (errno) {
867 case EAGAIN:
868 AINFO << "EAGAIN";
869 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
870 return CameraDeviceState::STATE_DEVICE_ERROR;
871
872 case EIO:
873
874 /* Could ignore EIO, see spec. */
875
876 /* fall through */
877
878 default:
879 AERROR << "read";
880 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
881 return CameraDeviceState::STATE_DEVICE_ERROR;
882 }
883 }
884
885 process_image(buffers_[0].start, len, raw_image);
886 if (sensor_raw_image->image != nullptr) {
887 memcpy(sensor_raw_image->image, buffers_[0].start, len);
888 }
889 break;
890
891 case IO_METHOD_MMAP:
892 CLEAR(buf);
893
894 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
895 buf.memory = V4L2_MEMORY_MMAP;
896
897 if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
898 switch (errno) {
899 case EAGAIN:
900 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
901 return CameraDeviceState::STATE_DEVICE_ERROR;
902
903 case EIO:
904
905 /* Could ignore EIO, see spec. */
906
907 /* fall through */
908
909 default:
910 AERROR << "VIDIOC_DQBUF";
911 reconnect();
912 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
913 return CameraDeviceState::STATE_DEVICE_ERROR;
914 }
915 }
916 assert(buf.index < n_buffers_);
917 len = buf.bytesused;
918 if (!config_->hardware_trigger()) {
919 sample_ts_monotonic.tv_sec = static_cast<int>(buf.timestamp.tv_sec);
920 sample_ts_monotonic.tv_usec = static_cast<int>(buf.timestamp.tv_usec);
921 clock_gettime(CLOCK_MONOTONIC, &current_ts_monotonic);
922 sample_ts_monotonic_ns = static_cast<uint64_t>(sample_ts_monotonic.tv_sec * (1000000000))
923 + static_cast<uint64_t>(sample_ts_monotonic.tv_usec * (1000));
924#ifdef __aarch64__
925 sample_ts_monotonic_ns = compensate_rtcpu_to_kernel_time(sample_ts_monotonic_ns);
926#endif
927 current_ts_monotonic_ns = static_cast<uint64_t>(current_ts_monotonic.tv_sec * (1000000000))
928 + static_cast<uint64_t>(current_ts_monotonic.tv_nsec);
929 diff_ns = static_cast<uint64_t>(current_ts_monotonic_ns - sample_ts_monotonic_ns);
930 sample_in_systme_time_ns = static_cast<uint64_t>(cyber::Time::Now().ToNanosecond() - diff_ns);
931 raw_image->tv_sec = static_cast<int>((sample_in_systme_time_ns) / 1000000000);
932 raw_image->tv_usec = static_cast<int>(((sample_in_systme_time_ns) % 1000000000) / 1000);
933 } else {
934 raw_image->tv_sec = static_cast<int>(buf.timestamp.tv_sec);
935 raw_image->tv_usec = static_cast<int>(buf.timestamp.tv_usec);
936 }
937 {
938 cyber::Time image_time(raw_image->tv_sec, 1000 * raw_image->tv_usec);
939 uint64_t camera_timestamp = image_time.ToNanosecond();
940 if (last_nsec_ == 0) {
941 last_nsec_ = camera_timestamp;
942 } else {
943 double diff = static_cast<double>(camera_timestamp - last_nsec_) / 1e9;
944 // drop image by frame_rate
945 if (diff < frame_drop_interval_) {
946 AINFO << "drop image:" << camera_timestamp;
947 if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
948 AERROR << "VIDIOC_QBUF ERROR";
949 }
950 return CameraDeviceState::STATE_DROP;
951 }
952 if (frame_warning_interval_ < diff) {
953 AWARN << "stamp jump.last stamp:" << last_nsec_ << " current stamp:" << camera_timestamp;
954 }
955 last_nsec_ = camera_timestamp;
956 }
957
958 double now_s = static_cast<double>(cyber::Time::Now().ToSecond());
959 double image_s = static_cast<double>(camera_timestamp) / 1e9;
960 double diff = now_s - image_s;
961 if (diff > 0.5 || diff < 0) {
962 std::stringstream warning_stream;
963 std::string warning_str;
964 warning_stream << "camera time diff exception,diff:" << diff << ";dev:" << config_->camera_dev();
965 warning_stream >> warning_str;
966 AWARN << warning_str;
967 }
968 }
969 if (len < raw_image->width * raw_image->height) {
970 AERROR << "Wrong Buffer Len: " << len << ", dev: " << config_->camera_dev();
971 } else {
972#ifdef CUDA_ZERO_COPY
973 process_image(buffers_[buf.index].start, len, raw_image, buffers_[buf.index].cuda_data);
974#else
975 process_image(buffers_[buf.index].start, len, raw_image);
976#endif
977 if (sensor_raw_image->image != nullptr) {
978 memcpy(sensor_raw_image->image, buffers_[buf.index].start, len);
979 }
980 }
981
982 if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
983 AERROR << "VIDIOC_QBUF";
984 return CameraDeviceState::STATE_OTHER;
985 }
986 break;
987
988 case IO_METHOD_USERPTR:
989 CLEAR(buf);
990
991 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
992 buf.memory = V4L2_MEMORY_USERPTR;
993
994 if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
995 switch (errno) {
996 case EAGAIN:
997 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
998 return CameraDeviceState::STATE_DEVICE_ERROR;
999
1000 case EIO:
1001
1002 /* Could ignore EIO, see spec. */
1003
1004 /* fall through */
1005
1006 default:
1007 AERROR << "VIDIOC_DQBUF";
1008 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
1009 return CameraDeviceState::STATE_DEVICE_ERROR;
1010 }
1011 }
1012
1013 for (i = 0; i < n_buffers_; ++i) {
1014 if (buf.m.userptr == reinterpret_cast<uint64_t>(buffers_[i].start) && buf.length == buffers_[i].length) {
1015 break;
1016 }
1017 }
1018
1019 assert(i < n_buffers_);
1020 len = buf.bytesused;
1021 process_image(reinterpret_cast<void*>(buf.m.userptr), len, raw_image);
1022 if (sensor_raw_image->image != nullptr) {
1023 memcpy(sensor_raw_image->image, reinterpret_cast<void*>(buf.m.userptr), len);
1024 }
1025
1026 if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
1027 AERROR << "VIDIOC_QBUF";
1028 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
1029 return CameraDeviceState::STATE_OTHER;
1030 }
1031
1032 break;
1033
1034 case IO_METHOD_UNKNOWN:
1035 AERROR << "unknown IO";
1036 last_error_time_ns_ = cyber::Time::Now().ToNanosecond();
1037 return CameraDeviceState::STATE_OTHER;
1038 }
1039
1040 return CameraDeviceState::STATE_SUCCESS;
1041}
1042
1043bool UsbCam::process_image(void* src, int len, CameraImagePtr dest, void* zero_copy_src) {
1044 if (src == nullptr || dest == nullptr) {
1045 AERROR << "process image error. src or dest is null";
1046 return false;
1047 }
1048 if (pixel_format_ == V4L2_PIX_FMT_YUYV || pixel_format_ == V4L2_PIX_FMT_UYVY) {
1049 if (pixel_format_ == V4L2_PIX_FMT_UYVY) {
1050 unsigned char yuyvbuf[len];
1051 unsigned char uyvybuf[len];
1052 memcpy(yuyvbuf, src, len);
1053 for (int index = 0; index < len; index = index + 2) {
1054 uyvybuf[index] = yuyvbuf[index + 1];
1055 uyvybuf[index + 1] = yuyvbuf[index];
1056 }
1057 memcpy(src, uyvybuf, len);
1058 }
1059 if (config_->output_type() == YUYV) {
1060 memcpy(dest->image, src, dest->width * dest->height * 2);
1061 } else if (config_->output_type() == RGB) {
1062#ifdef __aarch64__
1063 if (config_->arm_gpu_acceleration())
1064 // cuda_convert_yuv_to_rgb((unsigned char*)src,
1065 // (unsigned char*)dest->image, dest->width, dest->height);
1066 if (zero_copy_src != nullptr) {
1067 handler_.Process((unsigned char*)zero_copy_src, (unsigned char*)dest->image, true);
1068 } else {
1069 handler_.Process((unsigned char*)src, (unsigned char*)dest->image);
1070 }
1071 else
1072 convert_yuv_to_rgb_buffer((unsigned char*)src, (unsigned char*)dest->image, dest->width, dest->height);
1073#else
1074 yuyv2rgb_avx((unsigned char*)src, (unsigned char*)dest->image, dest->width * dest->height);
1075#endif
1076 } else {
1077 AERROR << "unsupported output format:" << config_->output_type();
1078 return false;
1079 }
1080 } else {
1081 AERROR << "unsupported pixel format:" << pixel_format_;
1082 return false;
1083 }
1084 return true;
1085}
1086
1087bool UsbCam::is_capturing() {
1088 return is_capturing_;
1089}
1090
1091// enables/disables auto focus
1092void UsbCam::set_auto_focus(int value) {
1093 struct v4l2_queryctrl queryctrl;
1094 struct v4l2_ext_control control;
1095
1096 memset(&queryctrl, 0, sizeof(queryctrl));
1097 queryctrl.id = V4L2_CID_FOCUS_AUTO;
1098
1099 if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl)) {
1100 if (errno != EINVAL) {
1101 perror("VIDIOC_QUERYCTRL");
1102 return;
1103 } else {
1104 AINFO << "V4L2_CID_FOCUS_AUTO is not supported";
1105 return;
1106 }
1107 } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1108 AINFO << "V4L2_CID_FOCUS_AUTO is not supported";
1109 return;
1110 } else {
1111 memset(&control, 0, sizeof(control));
1112 control.id = V4L2_CID_FOCUS_AUTO;
1113 control.value = value;
1114
1115 if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control)) {
1116 perror("VIDIOC_S_CTRL");
1117 return;
1118 }
1119 }
1120}
1121
1128void UsbCam::set_v4l_parameter(const std::string& param, int value) {
1129 set_v4l_parameter(param, std::to_string(value));
1130}
1137void UsbCam::set_v4l_parameter(const std::string& param, const std::string& value) {
1138 // build the command
1139 std::stringstream ss;
1140 ss << "v4l2-ctl --device=" << config_->camera_dev() << " -c " << param << "=" << value << " 2>&1";
1141 std::string cmd = ss.str();
1142
1143 // capture the output
1144 std::string output;
1145 char buffer[256];
1146 FILE* stream = popen(cmd.c_str(), "r");
1147 if (stream) {
1148 while (!feof(stream)) {
1149 if (fgets(buffer, 256, stream) != nullptr) {
1150 output.append(buffer);
1151 }
1152 }
1153
1154 pclose(stream);
1155 // any output should be an error
1156 if (output.length() > 0) {
1157 AERROR << output.c_str();
1158 }
1159 } else {
1160 AERROR << "usb_cam_node could not run " << cmd.c_str();
1161 }
1162}
1163
1164bool UsbCam::wait_for_device() {
1165 if (is_capturing_) {
1166 ADEBUG << "is capturing";
1167 return true;
1168 }
1169 if (!open_device()) {
1170 return false;
1171 }
1172 if (!init_device()) {
1173 close_device();
1174 return false;
1175 }
1176 if (!start_capturing()) {
1177 uninit_device();
1178 close_device();
1179 return false;
1180 }
1181#ifndef __aarch64__
1182 // will continue when trigger failed for self-trigger camera
1183 set_adv_trigger();
1184#endif
1185 return true;
1186}
1187
1188void UsbCam::reconnect() {
1189 stop_capturing();
1190 uninit_device();
1191 close_device();
1192}
1193
1194#ifdef __aarch64__
1195int UsbCam::convert_yuv_to_rgb_pixel(int y, int u, int v) {
1196 unsigned int pixel32 = 0;
1197 unsigned char* pixel = (unsigned char*)&pixel32;
1198 int r = 0;
1199 int g = 0;
1200 int b = 0;
1201
1202 // TODO(lanyongshun): Calculate loss of precision between float and int
1203
1204 // Base on ITUR BT601 Full Range, YUV => RGB
1205 // Expand 64 times to convert float to int
1206 y = std::max(y, 16);
1207 r = (75 * y + 102 * v - 14266 + 32) >> 6;
1208 g = (75 * y - 52 * v - 25 * u + 8671 + 32) >> 6;
1209 b = (75 * y + 129 * u - 17723 + 32) >> 6;
1210
1211 if (r > 255)
1212 r = 255;
1213 if (g > 255)
1214 g = 255;
1215 if (b > 255)
1216 b = 255;
1217 if (r < 0)
1218 r = 0;
1219 if (g < 0)
1220 g = 0;
1221 if (b < 0)
1222 b = 0;
1223 pixel[0] = (unsigned char)r;
1224 pixel[1] = (unsigned char)g;
1225 pixel[2] = (unsigned char)b;
1226 return pixel32;
1227}
1228
1229int UsbCam::convert_yuv_to_rgb_buffer(unsigned char* yuv, unsigned char* rgb, unsigned int width, unsigned int height) {
1230#ifdef __ARM_NEON
1231 int16x8_t v_y, v_u, v_v, v_y_prod;
1232 uint8x8x3_t v_rgb_result;
1233
1234 const int16_t factor_y = 75;
1235 const int16_t factor_v_for_r = 102;
1236 const int16_t const_for_r = -14266 + 32;
1237 const int16_t factor_v_for_g = -52;
1238 const int16_t factor_u_for_g = -25;
1239 const int16_t const_for_g = 8671 + 32;
1240 const int16_t factor_u_for_b = 129;
1241 const int16_t const_for_b = -17723 + 32;
1242
1243 int16x8_t v_const_for_r = vdupq_n_s16(const_for_r);
1244 int16x8_t v_const_for_g = vdupq_n_s16(const_for_g);
1245 int16x8_t v_const_for_b = vdupq_n_s16(const_for_b);
1246
1247#pragma omp parallel for num_threads(get_nprocs())
1248 for (unsigned int in = 0; in < width * height * 2; in += 16) {
1249 unsigned char* rgb_out = rgb + static_cast<unsigned int>(1.5 * in);
1250 int16_t y_mem[]
1251 = {yuv[in + 0],
1252 yuv[in + 2],
1253 yuv[in + 4],
1254 yuv[in + 6],
1255 yuv[in + 8],
1256 yuv[in + 10],
1257 yuv[in + 12],
1258 yuv[in + 14]};
1259 int16_t u_mem[]
1260 = {yuv[in + 1],
1261 yuv[in + 1],
1262 yuv[in + 5],
1263 yuv[in + 5],
1264 yuv[in + 9],
1265 yuv[in + 9],
1266 yuv[in + 13],
1267 yuv[in + 13]};
1268 int16_t v_mem[]
1269 = {yuv[in + 3],
1270 yuv[in + 3],
1271 yuv[in + 7],
1272 yuv[in + 7],
1273 yuv[in + 11],
1274 yuv[in + 11],
1275 yuv[in + 15],
1276 yuv[in + 15]};
1277 v_y = vld1q_s16(y_mem);
1278 v_u = vld1q_s16(u_mem);
1279 v_v = vld1q_s16(v_mem);
1280
1281 v_y_prod = vmulq_n_s16(v_y, factor_y);
1282 v_rgb_result.val[0] = vqshrun_n_s16(vaddq_s16(vmlaq_n_s16(v_y_prod, v_v, factor_v_for_r), v_const_for_r), 6);
1283
1284 v_rgb_result.val[1] = vqshrun_n_s16(
1285 vaddq_s16(vmlaq_n_s16(vmlaq_n_s16(v_y_prod, v_v, factor_v_for_g), v_u, factor_u_for_g), v_const_for_g),
1286 6);
1287
1288 v_rgb_result.val[2] = vqshrun_n_s16(vaddq_s16(vmlaq_n_s16(v_y_prod, v_u, factor_u_for_b), v_const_for_b), 6);
1289
1290 vst3_u8(rgb_out, v_rgb_result);
1291 }
1292#else
1293 unsigned int in = 0;
1294 unsigned int out = 0;
1295 unsigned int pixel_16 = 0;
1296 unsigned char pixel_24[3];
1297 unsigned int pixel32 = 0;
1298 int y0 = 0;
1299 int u = 0;
1300 int u = 0;
1301 int v = 0;
1302 for (in = 0; in < width * height * 2; in += 4) {
1303 pixel_16 = yuv[in + 3] << 24 | yuv[in + 2] << 16 | yuv[in + 1] << 8 | yuv[in + 0];
1304 y0 = (pixel_16 & 0x000000ff);
1305 u = (pixel_16 & 0x0000ff00) >> 8;
1306 y1 = (pixel_16 & 0x00ff0000) >> 16;
1307 v = (pixel_16 & 0xff000000) >> 24;
1308 pixel32 = convert_yuv_to_rgb_pixel(y0, u, v);
1309 pixel_24[0] = (unsigned char)(pixel32 & 0x000000ff);
1310 pixel_24[1] = (unsigned char)((pixel32 & 0x0000ff00) >> 8);
1311 pixel_24[2] = (unsigned char)((pixel32 & 0x00ff0000) >> 16);
1312 rgb[out++] = pixel_24[0];
1313 rgb[out++] = pixel_24[1];
1314 rgb[out++] = pixel_24[2];
1315 pixel32 = convert_yuv_to_rgb_pixel(y1, u, v);
1316 pixel_24[0] = (unsigned char)(pixel32 & 0x000000ff);
1317 pixel_24[1] = (unsigned char)((pixel32 & 0x0000ff00) >> 8);
1318 pixel_24[2] = (unsigned char)((pixel32 & 0x00ff0000) >> 16);
1319 rgb[out++] = pixel_24[0];
1320 rgb[out++] = pixel_24[1];
1321 rgb[out++] = pixel_24[2];
1322 }
1323#endif
1324 return 0;
1325}
1326
1327uint64_t UsbCam::compensate_rtcpu_to_kernel_time(uint64_t rtcpu_time_ns) {
1328 char buffer[64];
1329 uint64_t system_kernel_time_ns;
1330
1331 lseek(time_compensator_fd_, 0, SEEK_SET);
1332 auto len = static_cast<int>(read(time_compensator_fd_, buffer, 64 * sizeof(char)));
1333 if (len == -1) {
1334 AWARN << "Cannot read offset: " << config_->time_compensator_conf().compensator_fnode();
1335 return rtcpu_time_ns;
1336 }
1337
1338 buffer[len - 1] = '\0';
1339 system_kernel_time_ns = rtcpu_time_ns - boost::lexical_cast<uint64_t>(buffer);
1340 return system_kernel_time_ns;
1341}
1342
1343#endif
1344
1345} // namespace camera
1346} // namespace drivers
1347} // namespace apollo
apollo::cyber::Time::ToNanosecond
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
apollo::cyber::Time::Now
static Time Now()
get the current time.
Definition time.cc:57
apollo::cyber::Time::ToSecond
double ToSecond() const
convert time to second.
Definition time.cc:77
apollo::drivers::camera::UsbCam::init
virtual bool init(const std::shared_ptr< apollo::drivers::camera::config::Config > &camera_config)
Definition usb_cam.cc:91
apollo::drivers::camera::UsbCam::poll
virtual CameraDeviceState poll(const CameraImagePtr &raw_image, const CameraImagePtr &sensor_raw_image)
Definition usb_cam.cc:280
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
AWARN
#define AWARN
Definition log.h:43
apollo::drivers::camera::CameraImagePtr
std::shared_ptr< CameraImage > CameraImagePtr
Definition usb_cam.h:102
apollo::drivers::camera::yuyv2rgb_avx
void yuyv2rgb_avx(unsigned char *YUV, unsigned char *RGB, int NumPixels)
Definition util.cc:127
apollo::drivers::robosense::height
uint32_t height
Height of point cloud
Definition point_cloud_msg.h:53
apollo::drivers::robosense::width
uint32_t width
Width of point cloud
Definition point_cloud_msg.h:54
apollo::perception::lidar::diff
float diff(Image< float > *I, int x1, int y1, int x2, int y2)
Dissimilarity measure between pixels
Definition segment_image.h:55
apollo
class register implement
Definition arena_queue.h:37
CLEAR
#define CLEAR(x)
Definition usb_cam.cc:54
usb_cam.h
AV_CODEC_ID_MJPEG
#define AV_CODEC_ID_MJPEG
Definition usb_cam.h:60