From 8c9400b94b1fe79fb9749dec41a259d64ebc7729 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Tue, 22 Nov 2022 11:34:09 -0800 Subject: [PATCH 01/12] Restructure usb_cam code into more digestible pieces Signed-off-by: Evan Flynn --- include/usb_cam/constants.hpp | 177 ++++++++++++++ include/usb_cam/conversions.hpp | 232 ++++++++++++++++++ include/usb_cam/usb_cam.hpp | 73 ++---- include/usb_cam/usb_cam_utils.hpp | 393 ------------------------------ include/usb_cam/utils.hpp | 221 +++++++++++++++++ src/usb_cam.cpp | 314 +++++++----------------- src/usb_cam_node.cpp | 13 +- 7 files changed, 739 insertions(+), 684 deletions(-) create mode 100644 include/usb_cam/constants.hpp create mode 100644 include/usb_cam/conversions.hpp delete mode 100644 include/usb_cam/usb_cam_utils.hpp create mode 100644 include/usb_cam/utils.hpp diff --git a/include/usb_cam/constants.hpp b/include/usb_cam/constants.hpp new file mode 100644 index 00000000..5a3df3c6 --- /dev/null +++ b/include/usb_cam/constants.hpp @@ -0,0 +1,177 @@ +#ifndef USB_CAM__CONSTANTS_HPP_ +#define USB_CAM__CONSTANTS_HPP_ + + +namespace usb_cam +{ +namespace constants +{ + + +const unsigned char uchar_clipping_table[] = { + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -128 - -121 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -120 - -113 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -112 - -105 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -104 - -97 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -96 - -89 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -88 - -81 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -80 - -73 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -72 - -65 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -64 - -57 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -56 - -49 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -48 - -41 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -40 - -33 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -32 - -25 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -24 - -17 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -16 - -9 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -8 - -1 + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, + 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, + 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, + 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, + 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, + 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, + 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, + 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, + 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, + 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, + 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, + 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, + 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, + 251, 252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263 + 255, 255, 255, 255, 255, 255, 255, 255, // 264-271 + 255, 255, 255, 255, 255, 255, 255, 255, // 272-279 + 255, 255, 255, 255, 255, 255, 255, 255, // 280-287 + 255, 255, 255, 255, 255, 255, 255, 255, // 288-295 + 255, 255, 255, 255, 255, 255, 255, 255, // 296-303 + 255, 255, 255, 255, 255, 255, 255, 255, // 304-311 + 255, 255, 255, 255, 255, 255, 255, 255, // 312-319 + 255, 255, 255, 255, 255, 255, 255, 255, // 320-327 + 255, 255, 255, 255, 255, 255, 255, 255, // 328-335 + 255, 255, 255, 255, 255, 255, 255, 255, // 336-343 + 255, 255, 255, 255, 255, 255, 255, 255, // 344-351 + 255, 255, 255, 255, 255, 255, 255, 255, // 352-359 + 255, 255, 255, 255, 255, 255, 255, 255, // 360-367 + 255, 255, 255, 255, 255, 255, 255, 255, // 368-375 + 255, 255, 255, 255, 255, 255, 255, 255, // 376-383 +}; +const int clipping_table_offset = 128; + +} // namespace constants +} // namespace usb_cam + +#endif // USB_CAM__CONSTANTS_HPP_ diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp new file mode 100644 index 00000000..9a103fbe --- /dev/null +++ b/include/usb_cam/conversions.hpp @@ -0,0 +1,232 @@ +#ifndef USB_CAM__CONVERSIONS_HPP_ +#define USB_CAM__CONVERSIONS_HPP_ + +#include "usb_cam/constants.hpp" +#include "usb_cam/utils.hpp" + + +namespace usb_cam +{ +namespace conversions +{ + + +/// @brief Convert a given MJPEG image to RGB +/// +/// @param MJPEG Input MJPEG image to convert +/// @param len Length of MJPEG image in memory +/// @param RGB Output RGB image to fill +/// @param NumPixels Length of output RGB image in memory +inline bool MJPEG2RGB(char * MJPEG, int len, char * RGB, int NumPixels) +{ + (void)MJPEG; + (void)len; + (void)RGB; + (void)NumPixels; +// int got_picture = 1; +// +// (void)len; +// +// // clear the picture +// memset(RGB, 0, NumPixels); +// +// #if LIBAVCODEC_VERSION_MAJOR > 52 +// // TODO(flynneva): what is this checking? +// if (avcodec_context_->codec_type == AVMEDIA_TYPE_VIDEO) +// #else +// avcodec_decode_video( +// avcodec_context_, avframe_camera_, &got_picture, reinterpret_cast(MJPEG), len); +// #endif +// +// {if (!got_picture) { +// RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Webcam: expected picture but didn't get it..."); +// return false; +// } +// } +// +// int xsize = avcodec_context_->width; +// int ysize = avcodec_context_->height; +// #if LIBAVCODEC_VERSION_MAJOR > 52 +// int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize, 1); +// #else +// // TODO(lucasw) avpicture_get_size corrupts the pix_fmt +// int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize); +// #endif +// +// // int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize); +// if (pic_size != avframe_camera_size_) { +// RCLCPP_ERROR( +// rclcpp::get_logger("usb_cam"), +// "outbuf size mismatch. pic_size: %d bufsize: %d", pic_size, avframe_camera_size_); +// return false; +// } +// +// // TODO(lucasw) why does the image need to be scaled? Does it also convert formats? +// // RCLCPP_INFO_STREAM( +// // rclcpp::get_logger("usb_cam"), "sw scaler " << xsize << " " << ysize << " " +// // << avcodec_context_->pix_fmt << ", linesize " << avframe_rgb_->linesize); +// // TODO(lucasw) only do if xsize and ysize or pix fmt is different from last time +// video_sws_ = sws_getContext( +// xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, +// AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL); +// sws_scale( +// video_sws_, avframe_camera_->data, avframe_camera_->linesize, +// 0, ysize, avframe_rgb_->data, avframe_rgb_->linesize); +// // TODO(lucasw) keep around until parameters change +// sws_freeContext(video_sws_); +// +// #if LIBAVCODEC_VERSION_MAJOR > 52 +// int size = av_image_copy_to_buffer( +// reinterpret_cast(avframe_rgb_), pic_size, avframe_camera_->data, +// avframe_camera_->linesize, avcodec_context_->pix_fmt, xsize, ysize, 1); +// #else +// int size = avpicture_layout( +// reinterpret_cast(avframe_rgb_), AV_PIX_FMT_RGB24, +// xsize, ysize, reinterpret_cast(RGB), avframe_rgb_size_); +// #endif +// +// if (size != avframe_rgb_size_) { +// RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "webcam: avpicture_layout error: %d", size); +// return false; +// } + return true; +} + +/** + * Conversion from YUV to RGB. + * The normal conversion matrix is due to Julien (surname unknown): + * + * [ R ] [ 1.0 0.0 1.403 ] [ Y ] + * [ G ] = [ 1.0 -0.344 -0.714 ] [ U ] + * [ B ] [ 1.0 1.770 0.0 ] [ V ] + * + * and the firewire one is similar: + * + * [ R ] [ 1.0 0.0 0.700 ] [ Y ] + * [ G ] = [ 1.0 -0.198 -0.291 ] [ U ] + * [ B ] [ 1.0 1.015 0.0 ] [ V ] + * + * Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB + * do not get you back to the same RGB!) + * [ R ] [ 1.0 0.0 1.136 ] [ Y ] + * [ G ] = [ 1.0 -0.396 -0.578 ] [ U ] + * [ B ] [ 1.0 2.041 0.002 ] [ V ] + * + */ +inline bool YUV2RGB( + const unsigned char & y, const unsigned char & u, const unsigned char & v, + unsigned char * r, unsigned char * g, unsigned char * b) +{ + const int y2 = static_cast(y); + const int u2 = static_cast(u - 128); + const int v2 = static_cast(v - 128); + // std::cerr << "YUV=("<> 16); + // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 ); + // int b2 = y2 + ( (u2*115999) >> 16); + // This is an adjusted version (UV spread out a bit) + int r2 = y2 + ((v2 * 37221) >> 15); + int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15); + int b2 = y2 + ((u2 * 66883) >> 15); + // std::cerr << " RGB=("<> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0)); + } + return true; +} + +inline bool YUYV2RGB(const char * YUV, char * & RGB, const int & NumPixels) +{ + int i, j; + unsigned char y0, y1, u, v; + unsigned char r, g, b; + + for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) { + y0 = (unsigned char)YUV[i + 0]; + u = (unsigned char)YUV[i + 1]; + y1 = (unsigned char)YUV[i + 2]; + v = (unsigned char)YUV[i + 3]; + YUV2RGB(y0, u, v, &r, &g, &b); + RGB[j + 0] = r; + RGB[j + 1] = g; + RGB[j + 2] = b; + YUV2RGB(y1, u, v, &r, &g, &b); + RGB[j + 3] = r; + RGB[j + 4] = g; + RGB[j + 5] = b; + } + return true; +} + +inline bool COPY2RGB(const char * input, char * & output, const int & NumPixels) +{ + memcpy(output, input, NumPixels * 3); + return true; +} + +inline bool YUV4202RGB(char * YUV, char * & RGB, const int & width, const int & height) +{ + cv::Size size(height, width); + cv::Mat cv_img(height * 1.5, width, CV_8UC1, YUV); + cv::Mat cv_out(height, width, CV_8UC3, RGB); + + cvtColor(cv_img, cv_out, cv::COLOR_YUV420p2BGR); + return true; +} + +std::string FCC2S(const unsigned int & val) +{ + std::string s; + + s += val & 0x7f; + s += (val >> 8) & 0x7f; + s += (val >> 16) & 0x7f; + s += (val >> 24) & 0x7f; + if (val & (1 << 31)) { + s += "-BE"; + } + return s; +} + +} // namespace conversions +} // namespace usb_cam + +#endif // USB_CAM__CONVERSIONS_HPP_ diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 4b0287a9..958f28da 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -29,7 +29,7 @@ #ifndef USB_CAM__USB_CAM_HPP_ #define USB_CAM__USB_CAM_HPP_ -#include "usb_cam/usb_cam_utils.hpp" +#include "usb_cam/utils.hpp" #include /* for videodev2.h */ @@ -60,38 +60,26 @@ extern "C" namespace usb_cam { +using utils::io_method; +using utils::pixel_format; +using utils::color_format; + +// TODO(lucasw) just store an Image shared_ptr here +typedef struct +{ + uint32_t width; + uint32_t height; + int bytes_per_pixel; + int image_size; + builtin_interfaces::msg::Time stamp; + char * image; + int is_new; +} camera_image_t; + class UsbCam { public: - typedef enum - { - IO_METHOD_READ, - IO_METHOD_MMAP, - IO_METHOD_USERPTR, - IO_METHOD_UNKNOWN, - } io_method; - - typedef enum - { - PIXEL_FORMAT_YUYV, - PIXEL_FORMAT_UYVY, - PIXEL_FORMAT_MJPEG, - PIXEL_FORMAT_YUVMONO10, - PIXEL_FORMAT_RGB24, - PIXEL_FORMAT_GREY, - PIXEL_FORMAT_YU12, - PIXEL_FORMAT_H264, - PIXEL_FORMAT_UNKNOWN - } pixel_format; - - typedef enum - { - COLOR_FORMAT_YUV420P, - COLOR_FORMAT_YUV422P, - COLOR_FORMAT_UNKNOWN, - } color_format; - UsbCam(); ~UsbCam(); @@ -117,34 +105,11 @@ class UsbCam bool set_v4l_parameter(const std::string & param, int value); bool set_v4l_parameter(const std::string & param, const std::string & value); - static io_method io_method_from_string(const std::string & str); - static pixel_format pixel_format_from_string(const std::string & str); - static color_format color_format_from_string(const std::string & str); - bool stop_capturing(void); bool start_capturing(void); bool is_capturing(); private: - // TODO(lucasw) just store an Image shared_ptr here - typedef struct - { - uint32_t width; - uint32_t height; - int bytes_per_pixel; - int image_size; - builtin_interfaces::msg::Time stamp; - char * image; - int is_new; - } camera_image_t; - - struct buffer - { - void * start; - size_t length; - }; - - int init_decoder( int image_width, int image_height, color_format color_format, AVCodecID codec_id, const char * codec_name); @@ -166,9 +131,9 @@ class UsbCam std::string camera_dev_; unsigned int pixelformat_; bool monochrome_; - io_method io_; + usb_cam::utils::io_method io_; int fd_; - buffer * buffers_; + usb_cam::utils::buffer * buffers_; unsigned int n_buffers_; AVFrame * avframe_camera_; AVFrame * avframe_rgb_; diff --git a/include/usb_cam/usb_cam_utils.hpp b/include/usb_cam/usb_cam_utils.hpp deleted file mode 100644 index 8d47de73..00000000 --- a/include/usb_cam/usb_cam_utils.hpp +++ /dev/null @@ -1,393 +0,0 @@ -// Copyright 2014 Robert Bosch, LLC -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Robert Bosch, LLC nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef USB_CAM__USB_CAM_UTILS_HPP_ -#define USB_CAM__USB_CAM_UTILS_HPP_ - -#include - -#include -#include -#include - -#include "opencv2/opencv.hpp" - - -namespace usb_cam -{ - -void monotonicToRealTime(const timespec & monotonic_time, timespec & real_time) -{ - struct timespec real_sample1, real_sample2, monotonic_sample; - - // TODO(lucasw) Disable interrupts here? - // otherwise what if there is a delay/interruption between sampling the times? - clock_gettime(CLOCK_REALTIME, &real_sample1); - clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); - clock_gettime(CLOCK_REALTIME, &real_sample2); - - timespec time_diff; - time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; - time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; - - // This isn't available outside of the kernel - // real_time = timespec_add(monotonic_time, time_diff); - - const int64_t NSEC_PER_SEC = 1000000000; - real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; - real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; - if (real_time.tv_nsec >= NSEC_PER_SEC) { - ++real_time.tv_sec; - real_time.tv_nsec -= NSEC_PER_SEC; - } else if (real_time.tv_nsec < 0) { - --real_time.tv_sec; - real_time.tv_nsec += NSEC_PER_SEC; - } -} - -inline int xioctl(int fd, int request, void * arg) -{ - int r; - - do { - r = ioctl(fd, request, arg); - continue; - } while (-1 == r && EINTR == errno); - - return r; -} - -const unsigned char uchar_clipping_table[] = { - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -128 - -121 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -120 - -113 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -112 - -105 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -104 - -97 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -96 - -89 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -88 - -81 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -80 - -73 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -72 - -65 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -64 - -57 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -56 - -49 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -48 - -41 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -40 - -33 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -32 - -25 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -24 - -17 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -16 - -9 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -8 - -1 - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, - 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, - 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, - 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, - 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, - 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, - 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, - 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, - 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, - 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, - 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, - 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, - 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, - 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, - 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, - 251, 252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263 - 255, 255, 255, 255, 255, 255, 255, 255, // 264-271 - 255, 255, 255, 255, 255, 255, 255, 255, // 272-279 - 255, 255, 255, 255, 255, 255, 255, 255, // 280-287 - 255, 255, 255, 255, 255, 255, 255, 255, // 288-295 - 255, 255, 255, 255, 255, 255, 255, 255, // 296-303 - 255, 255, 255, 255, 255, 255, 255, 255, // 304-311 - 255, 255, 255, 255, 255, 255, 255, 255, // 312-319 - 255, 255, 255, 255, 255, 255, 255, 255, // 320-327 - 255, 255, 255, 255, 255, 255, 255, 255, // 328-335 - 255, 255, 255, 255, 255, 255, 255, 255, // 336-343 - 255, 255, 255, 255, 255, 255, 255, 255, // 344-351 - 255, 255, 255, 255, 255, 255, 255, 255, // 352-359 - 255, 255, 255, 255, 255, 255, 255, 255, // 360-367 - 255, 255, 255, 255, 255, 255, 255, 255, // 368-375 - 255, 255, 255, 255, 255, 255, 255, 255, // 376-383 -}; -const int clipping_table_offset = 128; - -/** Clip a value to the range 0 255 ? 255 : val; */ - - // New method (array) - return uchar_clipping_table[val + clipping_table_offset]; -} - -/** - * Conversion from YUV to RGB. - * The normal conversion matrix is due to Julien (surname unknown): - * - * [ R ] [ 1.0 0.0 1.403 ] [ Y ] - * [ G ] = [ 1.0 -0.344 -0.714 ] [ U ] - * [ B ] [ 1.0 1.770 0.0 ] [ V ] - * - * and the firewire one is similar: - * - * [ R ] [ 1.0 0.0 0.700 ] [ Y ] - * [ G ] = [ 1.0 -0.198 -0.291 ] [ U ] - * [ B ] [ 1.0 1.015 0.0 ] [ V ] - * - * Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB - * do not get you back to the same RGB!) - * [ R ] [ 1.0 0.0 1.136 ] [ Y ] - * [ G ] = [ 1.0 -0.396 -0.578 ] [ U ] - * [ B ] [ 1.0 2.041 0.002 ] [ V ] - * - */ -inline bool YUV2RGB( - const unsigned char y, const unsigned char u, const unsigned char v, - unsigned char * r, unsigned char * g, unsigned char * b) -{ - const int y2 = static_cast(y); - const int u2 = static_cast(u - 128); - const int v2 = static_cast(v - 128); - // std::cerr << "YUV=("<> 16); - // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 ); - // int b2 = y2 + ( (u2*115999) >> 16); - // This is an adjusted version (UV spread out a bit) - int r2 = y2 + ((v2 * 37221) >> 15); - int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15); - int b2 = y2 + ((u2 * 66883) >> 15); - // std::cerr << " RGB=("<> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0)); - } - return true; -} - -inline bool yuyv2rgb(char * YUV, char * RGB, int NumPixels) -{ - int i, j; - unsigned char y0, y1, u, v; - unsigned char r, g, b; - - for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) { - y0 = (unsigned char)YUV[i + 0]; - u = (unsigned char)YUV[i + 1]; - y1 = (unsigned char)YUV[i + 2]; - v = (unsigned char)YUV[i + 3]; - YUV2RGB(y0, u, v, &r, &g, &b); - RGB[j + 0] = r; - RGB[j + 1] = g; - RGB[j + 2] = b; - YUV2RGB(y1, u, v, &r, &g, &b); - RGB[j + 3] = r; - RGB[j + 4] = g; - RGB[j + 5] = b; - } - return true; -} - -void rgb242rgb(char * YUV, char * RGB, int NumPixels) -{ - memcpy(RGB, YUV, NumPixels * 3); -} - -inline void yuv4202rgb(char * YUV, char * RGB, int width, int height) -{ - cv::Size size(height, width); - cv::Mat cv_img(height * 1.5, width, CV_8UC1, YUV); - cv::Mat cv_out(height, width, CV_8UC3, RGB); - - cvtColor(cv_img, cv_out, cv::COLOR_YUV420p2BGR); -} - -std::string fcc2s(unsigned int val) -{ - std::string s; - - s += val & 0x7f; - s += (val >> 8) & 0x7f; - s += (val >> 16) & 0x7f; - s += (val >> 24) & 0x7f; - if (val & (1 << 31)) { - s += "-BE"; - } - return s; -} -} // namespace usb_cam -#endif // USB_CAM__USB_CAM_UTILS_HPP_ diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp new file mode 100644 index 00000000..4998c724 --- /dev/null +++ b/include/usb_cam/utils.hpp @@ -0,0 +1,221 @@ +// Copyright 2014 Robert Bosch, LLC +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Robert Bosch, LLC nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef USB_CAM__UTILS_HPP_ +#define USB_CAM__UTILS_HPP_ + +#include "usb_cam/constants.hpp" + +#include + +#include +#include +#include + +#include "opencv2/opencv.hpp" + + +namespace usb_cam +{ +namespace utils +{ + + +typedef enum +{ + IO_METHOD_READ, + IO_METHOD_MMAP, + IO_METHOD_USERPTR, + IO_METHOD_UNKNOWN, +} io_method; + + +typedef enum +{ + PIXEL_FORMAT_YUYV, + PIXEL_FORMAT_UYVY, + PIXEL_FORMAT_MJPEG, + PIXEL_FORMAT_YUVMONO10, + PIXEL_FORMAT_RGB24, + PIXEL_FORMAT_GREY, + PIXEL_FORMAT_YU12, + PIXEL_FORMAT_H264, + PIXEL_FORMAT_UNKNOWN +} pixel_format; + + +typedef enum +{ + COLOR_FORMAT_YUV420P, + COLOR_FORMAT_YUV422P, + COLOR_FORMAT_UNKNOWN, +} color_format; + + +struct buffer +{ + void * start; + size_t length; +}; + + +void monotonicToRealTime(const timespec & monotonic_time, timespec & real_time) +{ + struct timespec real_sample1, real_sample2, monotonic_sample; + + // TODO(lucasw) Disable interrupts here? + // otherwise what if there is a delay/interruption between sampling the times? + clock_gettime(CLOCK_REALTIME, &real_sample1); + clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); + clock_gettime(CLOCK_REALTIME, &real_sample2); + + timespec time_diff; + time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; + time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; + + // This isn't available outside of the kernel + // real_time = timespec_add(monotonic_time, time_diff); + + const int64_t NSEC_PER_SEC = 1000000000; + real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; + real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; + if (real_time.tv_nsec >= NSEC_PER_SEC) { + ++real_time.tv_sec; + real_time.tv_nsec -= NSEC_PER_SEC; + } else if (real_time.tv_nsec < 0) { + --real_time.tv_sec; + real_time.tv_nsec += NSEC_PER_SEC; + } +} + + +inline int xioctl(int fd, int request, void * arg) +{ + int r; + + do { + r = ioctl(fd, request, arg); + continue; + } while (-1 == r && EINTR == errno); + + return r; +} + +/** Clip a value to the range 0 255 ? 255 : val; */ + + // New method (array) + return usb_cam::constants::uchar_clipping_table[val + usb_cam::constants::clipping_table_offset]; +} + + +inline io_method io_method_from_string(const std::string & str) +{ + if (str == "mmap") { + return io_method::IO_METHOD_MMAP; + } else if (str == "read") { + return io_method::IO_METHOD_READ; + } else if (str == "userptr") { + return io_method::IO_METHOD_USERPTR; + } else { + return io_method::IO_METHOD_UNKNOWN; + } +} + + +inline pixel_format pixel_format_from_string(const std::string & str) +{ + if (str == "yuyv") { + return PIXEL_FORMAT_YUYV; + } else if (str == "uyvy") { + return PIXEL_FORMAT_UYVY; + } else if (str == "mjpeg") { + return PIXEL_FORMAT_MJPEG; + } else if (str == "h264") { + return PIXEL_FORMAT_H264; + } else if (str == "yuvmono10") { + return PIXEL_FORMAT_YUVMONO10; + } else if (str == "rgb24") { + return PIXEL_FORMAT_RGB24; + } else if (str == "grey") { + return PIXEL_FORMAT_GREY; + } else if (str == "yu12") { + return PIXEL_FORMAT_YU12; + } else { + return PIXEL_FORMAT_UNKNOWN; + } +} + + +inline std::string pixel_format_to_string(const uint32_t & pixelformat) +{ + switch (pixelformat) { + case PIXEL_FORMAT_YUYV: + return "yuyv"; + case PIXEL_FORMAT_UYVY: + return "uyvy"; + case PIXEL_FORMAT_MJPEG: + return "mjpeg"; + case PIXEL_FORMAT_H264: + return "h264"; + case PIXEL_FORMAT_YUVMONO10: + return "yuvmono10"; + case PIXEL_FORMAT_RGB24: + return "rgb24"; + case PIXEL_FORMAT_GREY: + return "grey"; + case PIXEL_FORMAT_YU12: + return "yu12"; + case PIXEL_FORMAT_UNKNOWN: + default: + return "unknown"; + } +} + + +color_format color_format_from_string(const std::string & str) +{ + if (str == "yuv420p") { + return COLOR_FORMAT_YUV420P; + } else if (str == "yuv422p") { + return COLOR_FORMAT_YUV422P; + } else { + return COLOR_FORMAT_UNKNOWN; + } +} + +} // namespace utils +} // namespace usb_cam + +#endif // USB_CAM__UTILS_HPP_ diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index b8412609..aaed8871 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -29,6 +29,8 @@ #define __STDC_CONSTANT_MACROS #include "usb_cam/usb_cam.hpp" +#include "usb_cam/conversions.hpp" +#include "usb_cam/utils.hpp" #include #include @@ -60,8 +62,13 @@ namespace usb_cam { +using utils::io_method; +using utils::pixel_format; +using utils::color_format; + + UsbCam::UsbCam() -: io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), +: io_(io_method::IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL), avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL), avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL), is_capturing_(false) @@ -118,7 +125,7 @@ int UsbCam::init_decoder( #if LIBAVCODEC_VERSION_MAJOR > 52 // TODO(lucasw) it gets set correctly here, but then changed later to deprecated J422P format - if (color_format == COLOR_FORMAT_YUV420P) { + if (color_format == color_format::COLOR_FORMAT_YUV420P) { avcodec_context_->pix_fmt = AV_PIX_FMT_YUV420P; RCLCPP_INFO_STREAM( rclcpp::get_logger("usb_cam"), @@ -162,127 +169,40 @@ int UsbCam::init_h264_decoder(int image_width, int image_height, color_format cf return init_decoder(image_width, image_height, cf, AV_CODEC_ID_H264, "H264"); } -bool UsbCam::mjpeg2rgb(char * MJPEG, int len, char * RGB, int NumPixels) -{ - // RCLCPP_INFO_STREAM( - // rclcpp::get_logger("usb_cam"), - // "mjpeg2rgb " << len << ", image 0x" << std::hex << (unsigned long int)RGB - // << std::dec << " " << NumPixels << ", avframe_rgb_size_ " << avframe_rgb_size_); - int got_picture = 1; - - // TODO(flynneva): unused parameters not needed? - (void)MJPEG; - (void)len; - (void)NumPixels; - - // clear the picture - memset(RGB, 0, avframe_rgb_size_); - -#if LIBAVCODEC_VERSION_MAJOR > 52 - // TODO(flynneva): what is this checking? - if (avcodec_context_->codec_type == AVMEDIA_TYPE_VIDEO) -#else - avcodec_decode_video( - avcodec_context_, avframe_camera_, &got_picture, reinterpret_cast(MJPEG), len); -#endif - - {if (!got_picture) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Webcam: expected picture but didn't get it..."); - return false; - } - } - - int xsize = avcodec_context_->width; - int ysize = avcodec_context_->height; -#if LIBAVCODEC_VERSION_MAJOR > 52 - int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize, 1); -#else - // TODO(lucasw) avpicture_get_size corrupts the pix_fmt - int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize); -#endif - - // int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize); - if (pic_size != avframe_camera_size_) { - RCLCPP_ERROR( - rclcpp::get_logger("usb_cam"), - "outbuf size mismatch. pic_size: %d bufsize: %d", pic_size, avframe_camera_size_); - return false; - } - - // TODO(lucasw) why does the image need to be scaled? Does it also convert formats? - // RCLCPP_INFO_STREAM( - // rclcpp::get_logger("usb_cam"), "sw scaler " << xsize << " " << ysize << " " - // << avcodec_context_->pix_fmt << ", linesize " << avframe_rgb_->linesize); - // TODO(lucasw) only do if xsize and ysize or pix fmt is different from last time - video_sws_ = sws_getContext( - xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, - AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL); - sws_scale( - video_sws_, avframe_camera_->data, avframe_camera_->linesize, - 0, ysize, avframe_rgb_->data, avframe_rgb_->linesize); - // TODO(lucasw) keep around until parameters change - sws_freeContext(video_sws_); - -#if LIBAVCODEC_VERSION_MAJOR > 52 - int size = av_image_copy_to_buffer( - reinterpret_cast(avframe_rgb_), pic_size, avframe_camera_->data, - avframe_camera_->linesize, avcodec_context_->pix_fmt, xsize, ysize, 1); -#else - int size = avpicture_layout( - reinterpret_cast(avframe_rgb_), AV_PIX_FMT_RGB24, - xsize, ysize, reinterpret_cast(RGB), avframe_rgb_size_); -#endif - - if (size != avframe_rgb_size_) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "webcam: avpicture_layout error: %d", size); - return false; - } - return true; -} - bool UsbCam::process_image(const void * src, int len, camera_image_t * dest) { - // TODO(lucasw) return bool from all these + bool result = false; if (pixelformat_ == V4L2_PIX_FMT_YUYV) { if (monochrome_) { - // actually format V4L2_PIX_FMT_Y16, but xioctl gets unhappy + // actually format V4L2_PIX_FMT_Y16, but usb_cam::utils::xioctl gets unhappy // if you don't use the advertised type (yuyv) - mono102mono8( + result = conversions::MONO102MONO8( const_cast( reinterpret_cast(src)), dest->image, dest->width * dest->height); } else { - yuyv2rgb( + result = conversions::YUYV2RGB( const_cast( reinterpret_cast(src)), dest->image, dest->width * dest->height); } } else if (pixelformat_ == V4L2_PIX_FMT_UYVY) { - uyvy2rgb( + result = conversions::UYVY2RGB( const_cast( reinterpret_cast(src)), dest->image, dest->width * dest->height); - } else if (pixelformat_ == V4L2_PIX_FMT_MJPEG) { - return mjpeg2rgb( - const_cast( - reinterpret_cast(src)), len, dest->image, dest->width * dest->height); - } else if (pixelformat_ == V4L2_PIX_FMT_H264) { - // libav handles the decoding, so reusing the same function is fine - return mjpeg2rgb( + } else if (pixelformat_ == V4L2_PIX_FMT_MJPEG || pixelformat_ == V4L2_PIX_FMT_H264) { + result = conversions::MJPEG2RGB( const_cast( reinterpret_cast(src)), len, dest->image, dest->width * dest->height); - } else if (pixelformat_ == V4L2_PIX_FMT_RGB24) { - rgb242rgb( + } else if (pixelformat_ == V4L2_PIX_FMT_RGB24 || pixelformat_ == V4L2_PIX_FMT_GREY) { + result = conversions::COPY2RGB( const_cast( reinterpret_cast(src)), dest->image, dest->width * dest->height); - } else if (pixelformat_ == V4L2_PIX_FMT_GREY) { - memcpy( - dest->image, - const_cast(reinterpret_cast(src)), dest->width * dest->height); } else if (pixelformat_ == V4L2_PIX_FMT_YUV420) { - yuv4202rgb( + result = conversions::YUV4202RGB( const_cast( reinterpret_cast(src)), dest->image, dest->width, dest->height); } - return true; + return result; } bool UsbCam::read_frame() @@ -295,7 +215,7 @@ bool UsbCam::read_frame() timespec real_time; switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: len = read(fd_, buffers_[0].start, buffers_[0].length); if (len == -1) { switch (errno) { @@ -320,13 +240,13 @@ bool UsbCam::read_frame() break; - case IO_METHOD_MMAP: + case io_method::IO_METHOD_MMAP: CLEAR(buf); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_MMAP; - if (-1 == xioctl(fd_, static_cast(VIDIOC_DQBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_DQBUF), &buf)) { switch (errno) { case EAGAIN: return false; @@ -344,7 +264,7 @@ bool UsbCam::read_frame() // need to get buf time here otherwise process_image will zero it TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); - monotonicToRealTime(buf_time, real_time); + usb_cam::utils::monotonicToRealTime(buf_time, real_time); stamp.sec = real_time.tv_sec; stamp.nanosec = real_time.tv_nsec; @@ -354,7 +274,7 @@ bool UsbCam::read_frame() return false; } - if (-1 == xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_QBUF"); } @@ -363,13 +283,13 @@ bool UsbCam::read_frame() break; - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_USERPTR: CLEAR(buf); buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; buf.memory = V4L2_MEMORY_USERPTR; - if (-1 == xioctl(fd_, static_cast(VIDIOC_DQBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_DQBUF), &buf)) { switch (errno) { case EAGAIN: return false; @@ -386,7 +306,7 @@ bool UsbCam::read_frame() } TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); - monotonicToRealTime(buf_time, real_time); + usb_cam::utils::monotonicToRealTime(buf_time, real_time); stamp.sec = real_time.tv_sec; stamp.nanosec = real_time.tv_nsec; @@ -404,14 +324,14 @@ bool UsbCam::read_frame() return false; } - if (-1 == xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_QBUF"); } image_->stamp = stamp; break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something to indicate IO method unknown break; } @@ -432,21 +352,21 @@ bool UsbCam::stop_capturing(void) enum v4l2_buf_type type; switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: /* Nothing to do. */ break; - case IO_METHOD_MMAP: - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_MMAP: + case io_method::IO_METHOD_USERPTR: type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type)) { + if (-1 == usb_cam::utils::xioctl(fd_, VIDIOC_STREAMOFF, &type)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_STREAMOFF"); } break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something indicating IO method unknown break; } @@ -461,11 +381,11 @@ bool UsbCam::start_capturing(void) enum v4l2_buf_type type; switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: /* Nothing to do. */ break; - case IO_METHOD_MMAP: + case io_method::IO_METHOD_MMAP: for (i = 0; i < n_buffers_; ++i) { struct v4l2_buffer buf; @@ -475,7 +395,7 @@ bool UsbCam::start_capturing(void) buf.memory = V4L2_MEMORY_MMAP; buf.index = i; - if (-1 == xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_QBUF"); } @@ -483,13 +403,13 @@ bool UsbCam::start_capturing(void) type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) { + if (-1 == usb_cam::utils::xioctl(fd_, VIDIOC_STREAMON, &type)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_STREAMON"); } break; - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_USERPTR: for (i = 0; i < n_buffers_; ++i) { struct v4l2_buffer buf; @@ -501,7 +421,7 @@ bool UsbCam::start_capturing(void) buf.m.userptr = reinterpret_cast(buffers_[i].start); buf.length = buffers_[i].length; - if (-1 == xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QBUF), &buf)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_QBUF"); } @@ -509,13 +429,13 @@ bool UsbCam::start_capturing(void) type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) { + if (-1 == usb_cam::utils::xioctl(fd_, VIDIOC_STREAMON, &type)) { std::cerr << "error, quitting " << errno << std::endl; return false; // ("VIDIOC_STREAMON"); } break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something indicating IO method unknown break; } @@ -528,11 +448,11 @@ bool UsbCam::uninit_device(void) unsigned int i; switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: free(buffers_[0].start); break; - case IO_METHOD_MMAP: + case io_method::IO_METHOD_MMAP: for (i = 0; i < n_buffers_; ++i) { if (-1 == munmap(buffers_[i].start, buffers_[i].length)) { std::cerr << "error, quitting, TODO throw " << errno << std::endl; @@ -541,12 +461,12 @@ bool UsbCam::uninit_device(void) } break; - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_USERPTR: for (i = 0; i < n_buffers_; ++i) { free(buffers_[i].start); } break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something break; } @@ -557,7 +477,7 @@ bool UsbCam::uninit_device(void) bool UsbCam::init_read(unsigned int buffer_size) { - buffers_ = reinterpret_cast(calloc(1, sizeof(*buffers_))); + buffers_ = reinterpret_cast(calloc(1, sizeof(*buffers_))); if (!buffers_) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); @@ -584,7 +504,7 @@ bool UsbCam::init_mmap(void) req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; req.memory = V4L2_MEMORY_MMAP; - if (-1 == xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { if (EINVAL == errno) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("usb_cam"), @@ -603,7 +523,7 @@ bool UsbCam::init_mmap(void) return false; } - buffers_ = reinterpret_cast(calloc(req.count, sizeof(*buffers_))); + buffers_ = reinterpret_cast(calloc(req.count, sizeof(*buffers_))); if (!buffers_) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); @@ -619,7 +539,7 @@ bool UsbCam::init_mmap(void) buf.memory = V4L2_MEMORY_MMAP; buf.index = n_buffers_; - if (-1 == xioctl(fd_, static_cast(VIDIOC_QUERYBUF), &buf)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QUERYBUF), &buf)) { std::cerr << "error, quitting, TODO throw " << errno << std::endl; return false; // ("VIDIOC_QUERYBUF"); } @@ -652,7 +572,7 @@ bool UsbCam::init_userp(unsigned int buffer_size) req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; req.memory = V4L2_MEMORY_USERPTR; - if (-1 == xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { if (EINVAL == errno) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("usb_cam"), @@ -664,7 +584,7 @@ bool UsbCam::init_userp(unsigned int buffer_size) } } - buffers_ = reinterpret_cast(calloc(4, sizeof(*buffers_))); + buffers_ = reinterpret_cast(calloc(4, sizeof(*buffers_))); if (!buffers_) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); @@ -691,7 +611,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer struct v4l2_format fmt; unsigned int min; - if (-1 == xioctl(fd_, static_cast(VIDIOC_QUERYCAP), &cap)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QUERYCAP), &cap)) { if (EINVAL == errno) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("usb_cam"), @@ -711,7 +631,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer } switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: if (!(cap.capabilities & V4L2_CAP_READWRITE)) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("usb_cam"), @@ -721,8 +641,8 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer break; - case IO_METHOD_MMAP: - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_MMAP: + case io_method::IO_METHOD_USERPTR: if (!(cap.capabilities & V4L2_CAP_STREAMING)) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("usb_cam"), @@ -731,7 +651,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer } break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something break; } @@ -742,11 +662,11 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (0 == xioctl(fd_, static_cast(VIDIOC_CROPCAP), &cropcap)) { + if (0 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_CROPCAP), &cropcap)) { crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; crop.c = cropcap.defrect; /* reset to default */ - if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop)) { + if (-1 == usb_cam::utils::xioctl(fd_, VIDIOC_S_CROP, &crop)) { switch (errno) { case EINVAL: /* Cropping not supported. */ @@ -768,7 +688,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer fmt.fmt.pix.pixelformat = pixelformat_; fmt.fmt.pix.field = V4L2_FIELD_INTERLACED; - if (-1 == xioctl(fd_, static_cast(VIDIOC_S_FMT), &fmt)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_S_FMT), &fmt)) { std::cerr << "error, quitting, TODO throw " << errno << std::endl; return false; // ("VIDIOC_S_FMT"); } @@ -792,7 +712,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer struct v4l2_streamparm stream_params; memset(&stream_params, 0, sizeof(stream_params)); stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (xioctl(fd_, static_cast(VIDIOC_G_PARM), &stream_params) < 0) { + if (usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_G_PARM), &stream_params) < 0) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("usb_cam"), "can't set stream params " << errno); return false; // ("Couldn't query v4l fps!"); } @@ -807,25 +727,25 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer // and match closest to what user put in. stream_params.parm.capture.timeperframe.numerator = 1; stream_params.parm.capture.timeperframe.denominator = framerate; - if (xioctl(fd_, static_cast(VIDIOC_S_PARM), &stream_params) < 0) { + if (usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_S_PARM), &stream_params) < 0) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Couldn't set camera framerate"); } else { RCLCPP_INFO_STREAM(rclcpp::get_logger("usb_cam"), "Set framerate to be " << framerate); } switch (io_) { - case IO_METHOD_READ: + case io_method::IO_METHOD_READ: init_read(fmt.fmt.pix.sizeimage); break; - case IO_METHOD_MMAP: + case io_method::IO_METHOD_MMAP: init_mmap(); break; - case IO_METHOD_USERPTR: + case io_method::IO_METHOD_USERPTR: init_userp(fmt.fmt.pix.sizeimage); break; - case IO_METHOD_UNKNOWN: + case io_method::IO_METHOD_UNKNOWN: // TODO(flynneva): log something break; } @@ -878,27 +798,27 @@ bool UsbCam::start( io_ = io_method; monochrome_ = false; - if (pixel_format == PIXEL_FORMAT_YUYV) { + if (pixel_format == pixel_format::PIXEL_FORMAT_YUYV) { pixelformat_ = V4L2_PIX_FMT_YUYV; - } else if (pixel_format == PIXEL_FORMAT_UYVY) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_UYVY) { pixelformat_ = V4L2_PIX_FMT_UYVY; - } else if (pixel_format == PIXEL_FORMAT_MJPEG) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_MJPEG) { pixelformat_ = V4L2_PIX_FMT_MJPEG; init_mjpeg_decoder(image_width, image_height, cf); - } else if (pixel_format == PIXEL_FORMAT_H264) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_H264) { pixelformat_ = V4L2_PIX_FMT_H264; init_h264_decoder(image_width, image_height, cf); - } else if (pixel_format == PIXEL_FORMAT_YUVMONO10) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_YUVMONO10) { // actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels) // but we need to use the advertised type (yuyv) pixelformat_ = V4L2_PIX_FMT_YUYV; monochrome_ = true; - } else if (pixel_format == PIXEL_FORMAT_RGB24) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_RGB24) { pixelformat_ = V4L2_PIX_FMT_RGB24; - } else if (pixel_format == PIXEL_FORMAT_GREY) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_GREY) { pixelformat_ = V4L2_PIX_FMT_GREY; monochrome_ = true; - } else if (pixel_format == PIXEL_FORMAT_YU12) { + } else if (pixel_format == pixel_format::PIXEL_FORMAT_YU12) { pixelformat_ = V4L2_PIX_FMT_YUV420; } else { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Unknown pixel format."); @@ -998,7 +918,9 @@ void UsbCam::get_formats() // std::vector& formats) struct v4l2_fmtdesc fmt; fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; fmt.index = 0; - for (fmt.index = 0; xioctl(fd_, static_cast(VIDIOC_ENUM_FMT), &fmt) == 0; ++fmt.index) { + for (fmt.index = 0; usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FMT), &fmt) == 0; + ++fmt.index) + { RCLCPP_INFO_STREAM( rclcpp::get_logger("usb_cam"), " " << fmt.description << "[Index: " << fmt.index << ", Type: " << fmt.type << @@ -1008,7 +930,8 @@ void UsbCam::get_formats() // std::vector& formats) size.index = 0; size.pixel_format = fmt.pixelformat; - for (size.index = 0; xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMESIZES), &size) == 0; + for (size.index = 0; + usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMESIZES), &size) == 0; ++size.index) { RCLCPP_INFO_STREAM( @@ -1020,7 +943,7 @@ void UsbCam::get_formats() // std::vector& formats) interval.width = size.discrete.width; interval.height = size.discrete.height; for (interval.index = 0; - xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMEINTERVALS), &interval) == 0; + usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMEINTERVALS), &interval) == 0; ++interval.index) { if (interval.type == V4L2_FRMIVAL_TYPE_DISCRETE) { @@ -1086,7 +1009,7 @@ bool UsbCam::set_auto_focus(int value) memset(&queryctrl, 0, sizeof(queryctrl)); queryctrl.id = V4L2_CID_FOCUS_AUTO; - if (-1 == xioctl(fd_, static_cast(VIDIOC_QUERYCTRL), &queryctrl)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QUERYCTRL), &queryctrl)) { if (errno != EINVAL) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "VIDIOC_QUERYCTRL"); return false; @@ -1102,7 +1025,7 @@ bool UsbCam::set_auto_focus(int value) control.id = V4L2_CID_FOCUS_AUTO; control.value = value; - if (-1 == xioctl(fd_, static_cast(VIDIOC_S_CTRL), &control)) { + if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_S_CTRL), &control)) { RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "VIDIOC_S_CTRL"); return false; } @@ -1161,75 +1084,4 @@ bool UsbCam::set_v4l_parameter(const std::string & param, const std::string & va return retcode; } -UsbCam::io_method UsbCam::io_method_from_string(const std::string & str) -{ - if (str == "mmap") { - return IO_METHOD_MMAP; - } else if (str == "read") { - return IO_METHOD_READ; - } else if (str == "userptr") { - return IO_METHOD_USERPTR; - } else { - return IO_METHOD_UNKNOWN; - } -} - -UsbCam::pixel_format UsbCam::pixel_format_from_string(const std::string & str) -{ - if (str == "yuyv") { - return PIXEL_FORMAT_YUYV; - } else if (str == "uyvy") { - return PIXEL_FORMAT_UYVY; - } else if (str == "mjpeg") { - return PIXEL_FORMAT_MJPEG; - } else if (str == "h264") { - return PIXEL_FORMAT_H264; - } else if (str == "yuvmono10") { - return PIXEL_FORMAT_YUVMONO10; - } else if (str == "rgb24") { - return PIXEL_FORMAT_RGB24; - } else if (str == "grey") { - return PIXEL_FORMAT_GREY; - } else if (str == "yu12") { - return PIXEL_FORMAT_YU12; - } else { - return PIXEL_FORMAT_UNKNOWN; - } -} -#if 0 -std::string UsbCam::pixel_format_to_string(__u32 pixelformat) -{ - if (str == "yuyv") { - return PIXEL_FORMAT_YUYV; - } else if (str == "uyvy") { - return PIXEL_FORMAT_UYVY; - } else if (str == "mjpeg") { - return PIXEL_FORMAT_MJPEG; - } else if (str == "h264") { - return PIXEL_FORMAT_H264; - } else if (str == "yuvmono10") { - return PIXEL_FORMAT_YUVMONO10; - } else if (str == "rgb24") { - return PIXEL_FORMAT_RGB24; - } else if (str == "grey") { - return PIXEL_FORMAT_GREY; - } else if (str == "yu12") { - return PIXEL_FORMAT_YU12; - } else { - return PIXEL_FORMAT_UNKNOWN; - } -} -#endif - -UsbCam::color_format UsbCam::color_format_from_string(const std::string & str) -{ - if (str == "yuv420p") { - return COLOR_FORMAT_YUV420P; - } else if (str == "yuv422p") { - return COLOR_FORMAT_YUV422P; - } else { - return COLOR_FORMAT_UNKNOWN; - } -} - } // namespace usb_cam diff --git a/src/usb_cam_node.cpp b/src/usb_cam_node.cpp index add0a0ce..567a6d5a 100644 --- a/src/usb_cam_node.cpp +++ b/src/usb_cam_node.cpp @@ -28,6 +28,7 @@ #include "usb_cam/usb_cam_node.hpp" +#include "usb_cam/utils.hpp" #include // #include @@ -125,22 +126,22 @@ void UsbCamNode::init() image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), color_format_name_.c_str(), framerate_); // set the IO method - UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_); - if (io_method == UsbCam::IO_METHOD_UNKNOWN) { + io_method io_method = usb_cam::utils::io_method_from_string(io_method_name_); + if (io_method == usb_cam::utils::IO_METHOD_UNKNOWN) { RCLCPP_ERROR_ONCE(this->get_logger(), "Unknown IO method '%s'", io_method_name_.c_str()); rclcpp::shutdown(); return; } // set the pixel format - UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_); - if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN) { + pixel_format pixel_format = usb_cam::utils::pixel_format_from_string(pixel_format_name_); + if (pixel_format == usb_cam::utils::PIXEL_FORMAT_UNKNOWN) { RCLCPP_ERROR_ONCE(this->get_logger(), "Unknown pixel format '%s'", pixel_format_name_.c_str()); rclcpp::shutdown(); return; } // set the color format - UsbCam::color_format color_format = UsbCam::color_format_from_string(color_format_name_); - if (color_format == UsbCam::COLOR_FORMAT_UNKNOWN) { + color_format color_format = usb_cam::utils::color_format_from_string(color_format_name_); + if (color_format == usb_cam::utils::COLOR_FORMAT_UNKNOWN) { RCLCPP_ERROR_ONCE(this->get_logger(), "Unknown color format '%s'", color_format_name_.c_str()); rclcpp::shutdown(); return; From cbd245a10f03fbb7f095ed45a98a1da45271972c Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Tue, 22 Nov 2022 12:31:31 -0800 Subject: [PATCH 02/12] Remove ROS dep from usb_cam by rewriting timestamping of frames Signed-off-by: Evan Flynn --- include/usb_cam/usb_cam.hpp | 17 +++++++++-------- src/usb_cam.cpp | 14 +++++++------- src/usb_cam_node.cpp | 8 +++++++- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 958f28da..363f6039 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -47,15 +47,11 @@ extern "C" #define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG #endif +#include #include #include #include -#include "builtin_interfaces/msg/time.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -// #include "sensor_msgs/msg/image.hpp" - namespace usb_cam { @@ -65,13 +61,18 @@ using utils::pixel_format; using utils::color_format; // TODO(lucasw) just store an Image shared_ptr here +// TOOD(flynnev) can new Image shared_ptr be used for both ROS 1 and ROS 2? +// we should try and eliminate all ROS-specific code from this `usb_cam` lib +// so we can reuse it for all ROS distros. I think this clock is the only thing +// left...other than the loggers. typedef struct { uint32_t width; uint32_t height; int bytes_per_pixel; int image_size; - builtin_interfaces::msg::Time stamp; + // TODO(flynneva) + std::chrono::time_point stamp; char * image; int is_new; } camera_image_t; @@ -93,7 +94,7 @@ class UsbCam // grabs a new image from the camera // bool get_image(sensor_msgs::msg::Image:::SharedPtr image); bool get_image( - builtin_interfaces::msg::Time & stamp, std::string & encoding, + std::chrono::time_point & stamp, std::string & encoding, uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data); void get_formats(); // std::vector& formats); @@ -127,7 +128,7 @@ class UsbCam bool open_device(void); bool grab_image(); - rclcpp::Clock::SharedPtr clock_; + std::shared_ptr clock_; std::string camera_dev_; unsigned int pixelformat_; bool monochrome_; diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index aaed8871..f29d003b 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -73,7 +74,7 @@ UsbCam::UsbCam() avcodec_context_(NULL), avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL), is_capturing_(false) { - clock_ = std::make_shared(RCL_SYSTEM_TIME); + clock_ = std::make_shared(); } UsbCam::~UsbCam() @@ -210,7 +211,7 @@ bool UsbCam::read_frame() struct v4l2_buffer buf; unsigned int i; int len; - builtin_interfaces::msg::Time stamp; + std::chrono::time_point stamp; timespec buf_time; timespec real_time; @@ -265,8 +266,7 @@ bool UsbCam::read_frame() // need to get buf time here otherwise process_image will zero it TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); usb_cam::utils::monotonicToRealTime(buf_time, real_time); - stamp.sec = real_time.tv_sec; - stamp.nanosec = real_time.tv_nsec; + stamp = clock_->from_time_t(real_time.tv_sec); assert(buf.index < n_buffers_); len = buf.bytesused; @@ -307,8 +307,7 @@ bool UsbCam::read_frame() TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); usb_cam::utils::monotonicToRealTime(buf_time, real_time); - stamp.sec = real_time.tv_sec; - stamp.nanosec = real_time.tv_nsec; + stamp = clock_->from_time_t(real_time.tv_sec); for (i = 0; i < n_buffers_; ++i) { if (buf.m.userptr == reinterpret_cast(buffers_[i].start) && \ @@ -876,7 +875,7 @@ bool UsbCam::shutdown(void) } bool UsbCam::get_image( - builtin_interfaces::msg::Time & stamp, + std::chrono::time_point & stamp, std::string & encoding, uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data) { @@ -975,6 +974,7 @@ bool UsbCam::grab_image() r = select(fd_ + 1, &fds, NULL, NULL, &tv); // if the v4l2_buffer timestamp isn't available use this time, though // it may be 10s of milliseconds after the frame acquisition. + // image_->stamp = clock_->now(); image_->stamp = clock_->now(); if (-1 == r) { diff --git a/src/usb_cam_node.cpp b/src/usb_cam_node.cpp index 567a6d5a..03973702 100644 --- a/src/usb_cam_node.cpp +++ b/src/usb_cam_node.cpp @@ -203,15 +203,21 @@ void UsbCamNode::assign_params(const std::vector & parameters bool UsbCamNode::take_and_send_image() { + // TODO(flynneva): this is disgusting. We should find a better way + // to get the timestamp of the image during capture and put it into + // whatever format we want (not ROS specific like ROS-message) + std::chrono::time_point image_time; // grab the image if (!cam_.get_image( - img_->header.stamp, img_->encoding, img_->height, img_->width, + image_time, img_->encoding, img_->height, img_->width, img_->step, img_->data)) { RCLCPP_ERROR(this->get_logger(), "grab failed"); return false; } + img_->header.stamp.sec = std::chrono::system_clock::to_time_t(image_time); + // INFO(img_->data.size() << " " << img_->width << " " << img_->height << " " << img_->step); auto ci = std::make_unique(cinfo_->getCameraInfo()); ci->header = img_->header; From 0a79d363c41a2b8e6e68e44c812be1772a762600 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 23 Nov 2022 03:07:11 -0800 Subject: [PATCH 03/12] Add some basic unit tests to usb_cam Signed-off-by: Evan Flynn --- CMakeLists.txt | 7 +- include/usb_cam/constants.hpp | 29 ++++++ include/usb_cam/conversions.hpp | 34 ++++++- include/usb_cam/usb_cam.hpp | 14 +-- include/usb_cam/usb_cam_node.hpp | 3 +- include/usb_cam/utils.hpp | 48 ++++------ package.xml | 1 + src/usb_cam.cpp | 47 +++++---- src/usb_cam_node.cpp | 16 ++-- test/gtest_main.cpp | 38 ++++++++ test/test_usb_cam_utils.cpp | 157 +++++++++++++++++++++++++++++++ 11 files changed, 324 insertions(+), 70 deletions(-) create mode 100644 test/gtest_main.cpp create mode 100644 test/test_usb_cam_utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 00aba0a0..b6fc32a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,12 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - # TODO(flynneva): add some unit and integration tests + find_package(ament_cmake_gtest) + + # unit tests + ament_add_gtest(test_usb_cam_utils + test/test_usb_cam_utils.cpp) + target_link_libraries(test_usb_cam_utils ${PROJECT_NAME}) endif() install( diff --git a/include/usb_cam/constants.hpp b/include/usb_cam/constants.hpp index 5a3df3c6..954a5adc 100644 --- a/include/usb_cam/constants.hpp +++ b/include/usb_cam/constants.hpp @@ -1,3 +1,32 @@ +// Copyright 2022 Evan Flynn +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Evan Flynn nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + #ifndef USB_CAM__CONSTANTS_HPP_ #define USB_CAM__CONSTANTS_HPP_ diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp index 9a103fbe..9961ee0c 100644 --- a/include/usb_cam/conversions.hpp +++ b/include/usb_cam/conversions.hpp @@ -1,6 +1,37 @@ +// Copyright 2022 Evan Flynn +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Evan Flynn nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + #ifndef USB_CAM__CONVERSIONS_HPP_ #define USB_CAM__CONVERSIONS_HPP_ +#include + #include "usb_cam/constants.hpp" #include "usb_cam/utils.hpp" @@ -39,7 +70,8 @@ inline bool MJPEG2RGB(char * MJPEG, int len, char * RGB, int NumPixels) // #endif // // {if (!got_picture) { -// RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Webcam: expected picture but didn't get it..."); +// RCLCPP_ERROR( +// rclcpp::get_logger("usb_cam"), "Webcam: expected picture but didn't get it..."); // return false; // } // } diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 363f6039..8dbcfa1d 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -29,9 +29,9 @@ #ifndef USB_CAM__USB_CAM_HPP_ #define USB_CAM__USB_CAM_HPP_ -#include "usb_cam/utils.hpp" #include /* for videodev2.h */ +#include extern "C" { @@ -48,9 +48,12 @@ extern "C" #endif #include +#include +#include #include #include -#include + +#include "usb_cam/utils.hpp" namespace usb_cam @@ -71,8 +74,7 @@ typedef struct uint32_t height; int bytes_per_pixel; int image_size; - // TODO(flynneva) - std::chrono::time_point stamp; + struct timespec stamp; char * image; int is_new; } camera_image_t; @@ -94,7 +96,7 @@ class UsbCam // grabs a new image from the camera // bool get_image(sensor_msgs::msg::Image:::SharedPtr image); bool get_image( - std::chrono::time_point & stamp, std::string & encoding, + struct timespec & stamp, std::string & encoding, uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data); void get_formats(); // std::vector& formats); @@ -128,7 +130,6 @@ class UsbCam bool open_device(void); bool grab_image(); - std::shared_ptr clock_; std::string camera_dev_; unsigned int pixelformat_; bool monochrome_; @@ -146,6 +147,7 @@ class UsbCam struct SwsContext * video_sws_; camera_image_t * image_; bool is_capturing_; + const time_t epoch_time_shift_; }; } // namespace usb_cam diff --git a/include/usb_cam/usb_cam_node.hpp b/include/usb_cam/usb_cam_node.hpp index 4241b0f5..c9074789 100644 --- a/include/usb_cam/usb_cam_node.hpp +++ b/include/usb_cam/usb_cam_node.hpp @@ -29,7 +29,6 @@ #ifndef USB_CAM__USB_CAM_NODE_HPP_ #define USB_CAM__USB_CAM_NODE_HPP_ -#include "usb_cam/usb_cam.hpp" #include #include @@ -41,6 +40,8 @@ #include "sensor_msgs/msg/image.hpp" #include "std_srvs/srv/set_bool.hpp" +#include "usb_cam/usb_cam.hpp" + std::ostream & operator<<(std::ostream & ostr, const rclcpp::Time & tm) { diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp index 4998c724..4a4f5eb4 100644 --- a/include/usb_cam/utils.hpp +++ b/include/usb_cam/utils.hpp @@ -29,9 +29,9 @@ #ifndef USB_CAM__UTILS_HPP_ #define USB_CAM__UTILS_HPP_ -#include "usb_cam/constants.hpp" - #include +#include +#include #include #include @@ -39,6 +39,8 @@ #include "opencv2/opencv.hpp" +#include "usb_cam/constants.hpp" + namespace usb_cam { @@ -84,33 +86,23 @@ struct buffer }; -void monotonicToRealTime(const timespec & monotonic_time, timespec & real_time) +/// @brief Get epoch time shift +/// @details Run this at start of process to calculate epoch time shift +/// @ref https://stackoverflow.com/questions/10266451/where-does-v4l2-buffer-timestamp-value-starts-counting +time_t get_epoch_time_shift() { - struct timespec real_sample1, real_sample2, monotonic_sample; - - // TODO(lucasw) Disable interrupts here? - // otherwise what if there is a delay/interruption between sampling the times? - clock_gettime(CLOCK_REALTIME, &real_sample1); - clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); - clock_gettime(CLOCK_REALTIME, &real_sample2); - - timespec time_diff; - time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; - time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; - - // This isn't available outside of the kernel - // real_time = timespec_add(monotonic_time, time_diff); - - const int64_t NSEC_PER_SEC = 1000000000; - real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; - real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; - if (real_time.tv_nsec >= NSEC_PER_SEC) { - ++real_time.tv_sec; - real_time.tv_nsec -= NSEC_PER_SEC; - } else if (real_time.tv_nsec < 0) { - --real_time.tv_sec; - real_time.tv_nsec += NSEC_PER_SEC; - } + struct timeval epoch_time; + struct timespec monotonic_time; + + gettimeofday(&epoch_time, NULL); + clock_gettime(CLOCK_MONOTONIC, &monotonic_time); + + const int64_t uptime_ms = + monotonic_time.tv_sec * 1000 + static_cast(round(monotonic_time.tv_nsec / 1000000.0)); + const int64_t epoch_ms = + epoch_time.tv_sec * 1000 + static_cast(round(epoch_time.tv_usec / 1000.0)); + + return static_cast((epoch_ms - uptime_ms) / 1000); } diff --git a/package.xml b/package.xml index 09f701bc..a772ad5a 100644 --- a/package.xml +++ b/package.xml @@ -29,6 +29,7 @@ ffmpeg + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index f29d003b..b74a0b9f 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -28,9 +28,6 @@ #define __STDC_CONSTANT_MACROS -#include "usb_cam/usb_cam.hpp" -#include "usb_cam/conversions.hpp" -#include "usb_cam/utils.hpp" #include #include @@ -57,6 +54,11 @@ #include "rclcpp/rclcpp.hpp" +#include "usb_cam/usb_cam.hpp" +#include "usb_cam/conversions.hpp" +#include "usb_cam/utils.hpp" + + #define CLEAR(x) memset(&(x), 0, sizeof(x)) @@ -72,10 +74,9 @@ UsbCam::UsbCam() : io_(io_method::IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL), avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL), avframe_camera_size_(0), avframe_rgb_size_(0), - video_sws_(NULL), image_(NULL), is_capturing_(false) -{ - clock_ = std::make_shared(); -} + video_sws_(NULL), image_(NULL), is_capturing_(false), + epoch_time_shift_(usb_cam::utils::get_epoch_time_shift()) +{} UsbCam::~UsbCam() { @@ -211,9 +212,8 @@ bool UsbCam::read_frame() struct v4l2_buffer buf; unsigned int i; int len; - std::chrono::time_point stamp; - timespec buf_time; - timespec real_time; + struct timespec stamp; + int64_t buffer_time_s; switch (io_) { case io_method::IO_METHOD_READ: @@ -263,10 +263,11 @@ bool UsbCam::read_frame() } } - // need to get buf time here otherwise process_image will zero it - TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); - usb_cam::utils::monotonicToRealTime(buf_time, real_time); - stamp = clock_->from_time_t(real_time.tv_sec); + buffer_time_s = + buf.timestamp.tv_sec + static_cast(round(buf.timestamp.tv_usec / 1000000.0)); + + stamp.tv_sec = static_cast(round(buffer_time_s)) + epoch_time_shift_; + stamp.tv_nsec = static_cast(buf.timestamp.tv_usec * 1000.0); assert(buf.index < n_buffers_); len = buf.bytesused; @@ -305,9 +306,11 @@ bool UsbCam::read_frame() } } - TIMEVAL_TO_TIMESPEC(&buf.timestamp, &buf_time); - usb_cam::utils::monotonicToRealTime(buf_time, real_time); - stamp = clock_->from_time_t(real_time.tv_sec); + buffer_time_s = + buf.timestamp.tv_sec + static_cast(round(buf.timestamp.tv_usec / 1000000.0)); + + stamp.tv_sec = static_cast(round(buffer_time_s)) + epoch_time_shift_; + stamp.tv_nsec = static_cast(buf.timestamp.tv_usec / 1000.0); for (i = 0; i < n_buffers_; ++i) { if (buf.m.userptr == reinterpret_cast(buffers_[i].start) && \ @@ -875,7 +878,7 @@ bool UsbCam::shutdown(void) } bool UsbCam::get_image( - std::chrono::time_point & stamp, + struct timespec & stamp, std::string & encoding, uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data) { @@ -886,11 +889,7 @@ bool UsbCam::get_image( if (!grab_image()) { return false; } - // TODO(lucasw) check if stamp is valid) - // RCLCPP_INFO_STREAM( - // rclcpp::get_logger("usb_cam"), - // "stamp " << image_->stamp.sec << " " << image_->stamp.nanosec - // << " to " << stamp.sec << " " << stamp.nanosec); + // stamp the image stamp = image_->stamp; // fill in the info @@ -975,7 +974,7 @@ bool UsbCam::grab_image() // if the v4l2_buffer timestamp isn't available use this time, though // it may be 10s of milliseconds after the frame acquisition. // image_->stamp = clock_->now(); - image_->stamp = clock_->now(); + timespec_get(&image_->stamp, TIME_UTC); if (-1 == r) { if (EINTR == errno) { diff --git a/src/usb_cam_node.cpp b/src/usb_cam_node.cpp index 03973702..29c48b2f 100644 --- a/src/usb_cam_node.cpp +++ b/src/usb_cam_node.cpp @@ -26,17 +26,14 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. - -#include "usb_cam/usb_cam_node.hpp" -#include "usb_cam/utils.hpp" - +#include #include -// #include - #include -#include #include +#include "usb_cam/usb_cam_node.hpp" +#include "usb_cam/utils.hpp" + namespace usb_cam { @@ -206,7 +203,7 @@ bool UsbCamNode::take_and_send_image() // TODO(flynneva): this is disgusting. We should find a better way // to get the timestamp of the image during capture and put it into // whatever format we want (not ROS specific like ROS-message) - std::chrono::time_point image_time; + struct timespec image_time; // grab the image if (!cam_.get_image( image_time, img_->encoding, img_->height, img_->width, @@ -216,7 +213,8 @@ bool UsbCamNode::take_and_send_image() return false; } - img_->header.stamp.sec = std::chrono::system_clock::to_time_t(image_time); + img_->header.stamp.sec = image_time.tv_sec; + img_->header.stamp.nanosec = image_time.tv_nsec; // INFO(img_->data.size() << " " << img_->width << " " << img_->height << " " << img_->step); auto ci = std::make_unique(cinfo_->getCameraInfo()); diff --git a/test/gtest_main.cpp b/test/gtest_main.cpp new file mode 100644 index 00000000..87209a09 --- /dev/null +++ b/test/gtest_main.cpp @@ -0,0 +1,38 @@ +// Copyright 2022 Evan Flynn +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Evan Flynn nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +#include "gtest/gtest.h" +#include + + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_usb_cam_utils.cpp b/test/test_usb_cam_utils.cpp new file mode 100644 index 00000000..2277d1ca --- /dev/null +++ b/test/test_usb_cam_utils.cpp @@ -0,0 +1,157 @@ +// Copyright 2022 Evan Flynn +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Evan Flynn nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include "usb_cam/utils.hpp" + + +using usb_cam::utils::io_method_from_string; +using usb_cam::utils::pixel_format_from_string; + + +TEST(test_usb_cam_utils, test_io_method_from_string) { + usb_cam::utils::io_method test_io; + + test_io = io_method_from_string("mmap"); + EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_MMAP); + + test_io = io_method_from_string("read"); + EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_READ); + + test_io = io_method_from_string("userptr"); + EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_USERPTR); + + test_io = io_method_from_string("bananas"); + EXPECT_EQ(test_io, usb_cam::utils::IO_METHOD_UNKNOWN); +} + +TEST(test_usb_cam_utils, test_pixel_format_from_string) { + usb_cam::utils::pixel_format test_fmt; + + test_fmt = pixel_format_from_string("yuyv"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_YUYV); + + test_fmt = pixel_format_from_string("uyvy"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_UYVY); + + test_fmt = pixel_format_from_string("mjpeg"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_MJPEG); + + test_fmt = pixel_format_from_string("h264"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_H264); + + test_fmt = pixel_format_from_string("yuvmono10"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_YUVMONO10); + + test_fmt = pixel_format_from_string("rgb24"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_RGB24); + + test_fmt = pixel_format_from_string("grey"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_GREY); + + test_fmt = pixel_format_from_string("yu12"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_YU12); + + test_fmt = pixel_format_from_string("apples"); + EXPECT_EQ(test_fmt, usb_cam::utils::PIXEL_FORMAT_UNKNOWN); +} + +TEST(test_usb_cam_utils, test_pixel_format_to_string) { + std::string test_fmt; + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_YUYV); + EXPECT_EQ(test_fmt, "yuyv"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_UYVY); + EXPECT_EQ(test_fmt, "uyvy"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_MJPEG); + EXPECT_EQ(test_fmt, "mjpeg"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_H264); + EXPECT_EQ(test_fmt, "h264"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_YUVMONO10); + EXPECT_EQ(test_fmt, "yuvmono10"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_RGB24); + EXPECT_EQ(test_fmt, "rgb24"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_GREY); + EXPECT_EQ(test_fmt, "grey"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_YU12); + EXPECT_EQ(test_fmt, "yu12"); + + test_fmt = pixel_format_to_string(usb_cam::utils::PIXEL_FORMAT_UNKNOWN); + EXPECT_EQ(test_fmt, "unknown"); +} + +TEST(test_usb_cam_utils, test_color_format) { + usb_cam::utils::color_format test_fmt; + + test_fmt = usb_cam::utils::color_format_from_string("yuv420p"); + EXPECT_EQ(test_fmt, usb_cam::utils::COLOR_FORMAT_YUV420P); + + test_fmt = usb_cam::utils::color_format_from_string("yuv422p"); + EXPECT_EQ(test_fmt, usb_cam::utils::COLOR_FORMAT_YUV422P); + + test_fmt = usb_cam::utils::color_format_from_string("pears"); + EXPECT_EQ(test_fmt, usb_cam::utils::COLOR_FORMAT_UNKNOWN); +} + +TEST(test_usb_cam_utils, test_clip_value) { + // Clip values to 0 if -128<=val<0 + for (int i = -128; i < 0; i++) { + EXPECT_EQ(0, usb_cam::utils::CLIPVALUE(i)); + } + // Do not clip values between 0 383 + // these will use the old method (non-array method) + EXPECT_EQ(0, usb_cam::utils::CLIPVALUE(-129)); + EXPECT_EQ(255, usb_cam::utils::CLIPVALUE(400)); +} + +TEST(test_usb_cam_utils, test_monotonic_to_real_time) { + // Get timeval to use for test + + const time_t test_time_t = usb_cam::utils::get_epoch_time_shift(); + + EXPECT_NE(test_time_t, 0); +} From 0691819b9337799639c49e863d7861416f1dff5a Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 23 Nov 2022 03:41:28 -0800 Subject: [PATCH 04/12] Fix humble CI name Signed-off-by: Evan Flynn --- .github/workflows/build_test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index f4c52701..b94ea7ef 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -15,6 +15,7 @@ jobs: ros_distribution: - foxy - galactic + - humble - rolling include: # Foxy Fitzroy (June 2020 - May 2023) From 5843cfe27668044e173406551a03716995f57735 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 23 Nov 2022 06:22:20 -0800 Subject: [PATCH 05/12] Improve CLIPVALUE method, add unit test for it Signed-off-by: Evan Flynn --- include/usb_cam/constants.hpp | 138 +++------------------------------- include/usb_cam/utils.hpp | 13 +++- 2 files changed, 19 insertions(+), 132 deletions(-) diff --git a/include/usb_cam/constants.hpp b/include/usb_cam/constants.hpp index 954a5adc..3aef0072 100644 --- a/include/usb_cam/constants.hpp +++ b/include/usb_cam/constants.hpp @@ -30,6 +30,7 @@ #ifndef USB_CAM__CONSTANTS_HPP_ #define USB_CAM__CONSTANTS_HPP_ +#include namespace usb_cam { @@ -37,135 +38,14 @@ namespace constants { -const unsigned char uchar_clipping_table[] = { - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -128 - -121 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -120 - -113 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -112 - -105 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -104 - -97 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -96 - -89 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -88 - -81 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -80 - -73 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -72 - -65 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -64 - -57 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -56 - -49 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -48 - -41 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -40 - -33 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -32 - -25 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -24 - -17 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -16 - -9 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -8 - -1 +const std::vector uchar_clipping_table = { + 0, 0, 0, 0, 0, 0, 0, 0, // -128 - -121 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -120 - -101 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -100 - -81 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -80 - -61 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -60 - -41 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -40 - -21 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // -20 - -1 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp index 4a4f5eb4..dbc6ae7c 100644 --- a/include/usb_cam/utils.hpp +++ b/include/usb_cam/utils.hpp @@ -119,7 +119,7 @@ inline int xioctl(int fd, int request, void * arg) } /** Clip a value to the range 0 255 ? 255 : val; */ - // New method (array) - return usb_cam::constants::uchar_clipping_table[val + usb_cam::constants::clipping_table_offset]; + try { + // New method array + return usb_cam::constants::uchar_clipping_table.at( + val + usb_cam::constants::clipping_table_offset); + } catch (std::out_of_range const &) { + // fall back to old method + unsigned char clipped_val = val < 0 ? 0 : static_cast(val); + return val > 255 ? 255 : clipped_val; + } } From 1d7fda448659e391986ee6fa20a2b74cb04b220e Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 23 Nov 2022 06:23:20 -0800 Subject: [PATCH 06/12] Bump default framerate to 30hz Signed-off-by: Evan Flynn --- src/usb_cam_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/usb_cam_node.cpp b/src/usb_cam_node.cpp index 29c48b2f..6595eecc 100644 --- a/src/usb_cam_node.cpp +++ b/src/usb_cam_node.cpp @@ -57,7 +57,7 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options) // declare params this->declare_parameter("camera_name", "default_cam"); this->declare_parameter("camera_info_url", ""); - this->declare_parameter("framerate", 10.0); + this->declare_parameter("framerate", 30.0); this->declare_parameter("frame_id", "default_cam"); this->declare_parameter("image_height", 480); this->declare_parameter("image_width", 640); From 19a86420be24a2ebeee4cd7ef510f7c5b76ca47b Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 30 Nov 2022 22:29:44 -0800 Subject: [PATCH 07/12] Clean up usb_cam lib, remove rclcpp dep Signed-off-by: Evan Flynn --- CMakeLists.txt | 59 ++++++++----- include/usb_cam/conversions.hpp | 2 + include/usb_cam/usb_cam.hpp | 14 +--- include/usb_cam/utils.hpp | 16 ++-- src/usb_cam.cpp | 141 ++++++++++++-------------------- 5 files changed, 104 insertions(+), 128 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b6fc32a7..fa6e8c2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,37 +13,50 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(OpenCV REQUIRED) - -## pkg-config libraries find_package(PkgConfig REQUIRED) -pkg_check_modules(avcodec libavcodec REQUIRED) -pkg_check_modules(swscale libswscale REQUIRED) +pkg_check_modules(avcodec REQUIRED libavcodec) +pkg_check_modules(avutil REQUIRED libavutil) +pkg_check_modules(swscale REQUIRED libswscale) -include_directories(include - ${avcodec_INCLUDE_DIRS} - ${swscale_INCLUDE_DIRS} -) +if(EXISTS ${avcodec}) + message(STATUS "Found libavcodec: ${avcodec}") +endif() + +if(EXISTS ${avutil}) + message(STATUS "Found libavutil: ${avutil}") +endif() + +if(EXISTS ${swscale}) + message(STATUS "Found libswscale: ${swscale}") +endif() ## Build the USB camera library -ament_auto_add_library(${PROJECT_NAME} SHARED +## Do not use ament_auto here so as to not link to rclcpp +add_library(${PROJECT_NAME} SHARED src/usb_cam.cpp ) -## Declare a cpp executable -ament_auto_add_library(${PROJECT_NAME}_node SHARED - src/usb_cam_node.cpp +target_include_directories(${PROJECT_NAME} PUBLIC + "include" ) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME} ${avcodec_LIBRARIES} + ${avutil_LIBRARIES} ${swscale_LIBRARIES} - ${OpenCV_LIBRARIES} - # TODO(lucasw) should this have been in libavcodec? - #avutil + ${OpenCV_LIBRARIES}) + +ament_export_libraries(${PROJECT_NAME}) + +## Declare a ROS 2 composible node as a library +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/usb_cam_node.cpp ) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME}) + +## Use node to generate an executable rclcpp_components_register_node(${PROJECT_NAME}_node PLUGIN "usb_cam::UsbCamNode" EXECUTABLE ${PROJECT_NAME}_node_exe @@ -55,10 +68,16 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest) - # unit tests + # Unit tests ament_add_gtest(test_usb_cam_utils test/test_usb_cam_utils.cpp) - target_link_libraries(test_usb_cam_utils ${PROJECT_NAME}) + target_link_libraries(test_usb_cam_utils + ${PROJECT_NAME}) + # Integration tests + ament_add_gtest(test_usb_cam_lib + test/test_usb_cam_lib.cpp) + target_link_libraries(test_usb_cam_lib + ${PROJECT_NAME}) endif() install( diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp index 9961ee0c..bef27af9 100644 --- a/include/usb_cam/conversions.hpp +++ b/include/usb_cam/conversions.hpp @@ -32,6 +32,8 @@ #include +#include + #include "usb_cam/constants.hpp" #include "usb_cam/utils.hpp" diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 8dbcfa1d..30a9e035 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -30,23 +30,11 @@ #ifndef USB_CAM__USB_CAM_HPP_ #define USB_CAM__USB_CAM_HPP_ -#include /* for videodev2.h */ -#include - -extern "C" -{ -#include +extern "C" { #include #include -#include } -// legacy reasons -#include -#if LIBAVCODEC_VERSION_MAJOR < 55 -#define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG -#endif - #include #include #include diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp index dbc6ae7c..677e4924 100644 --- a/include/usb_cam/utils.hpp +++ b/include/usb_cam/utils.hpp @@ -31,14 +31,12 @@ #include #include -#include - +#include +#include #include #include #include -#include "opencv2/opencv.hpp" - #include "usb_cam/constants.hpp" @@ -89,7 +87,7 @@ struct buffer /// @brief Get epoch time shift /// @details Run this at start of process to calculate epoch time shift /// @ref https://stackoverflow.com/questions/10266451/where-does-v4l2-buffer-timestamp-value-starts-counting -time_t get_epoch_time_shift() +inline time_t get_epoch_time_shift() { struct timeval epoch_time; struct timespec monotonic_time; @@ -98,9 +96,11 @@ time_t get_epoch_time_shift() clock_gettime(CLOCK_MONOTONIC, &monotonic_time); const int64_t uptime_ms = - monotonic_time.tv_sec * 1000 + static_cast(round(monotonic_time.tv_nsec / 1000000.0)); + monotonic_time.tv_sec * 1000 + static_cast( + std::round(monotonic_time.tv_nsec / 1000000.0)); const int64_t epoch_ms = - epoch_time.tv_sec * 1000 + static_cast(round(epoch_time.tv_usec / 1000.0)); + epoch_time.tv_sec * 1000 + static_cast( + std::round(epoch_time.tv_usec / 1000.0)); return static_cast((epoch_ms - uptime_ms) / 1000); } @@ -203,7 +203,7 @@ inline std::string pixel_format_to_string(const uint32_t & pixelformat) } -color_format color_format_from_string(const std::string & str) +inline color_format color_format_from_string(const std::string & str) { if (str == "yuv420p") { return COLOR_FORMAT_YUV420P; diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index b74a0b9f..32453029 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -26,42 +26,32 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#define CLEAR(x) memset(&(x), 0, sizeof(x)) -#define __STDC_CONSTANT_MACROS - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include /* low-level i/o */ - +extern "C" { +#include // Defines V4L2 format constants +#include // for memalign +#include // for mmap +#include // for stat +#define __STDC_CONSTANT_MACROS // Required for libavutil #include +#include // for O_* constants +} +#include +#include #include #include #include #include -#include -#include "rclcpp/rclcpp.hpp" +#include "opencv4/opencv2/imgproc.hpp" #include "usb_cam/usb_cam.hpp" #include "usb_cam/conversions.hpp" #include "usb_cam/utils.hpp" -#define CLEAR(x) memset(&(x), 0, sizeof(x)) - - namespace usb_cam { @@ -89,7 +79,7 @@ int UsbCam::init_decoder( { avcodec_ = avcodec_find_decoder(codec_id); if (!avcodec_) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Could not find %s decoder", codec_name); + std::cout << "Could not find " << codec_name << " decoder"; return 0; } @@ -129,14 +119,12 @@ int UsbCam::init_decoder( // TODO(lucasw) it gets set correctly here, but then changed later to deprecated J422P format if (color_format == color_format::COLOR_FORMAT_YUV420P) { avcodec_context_->pix_fmt = AV_PIX_FMT_YUV420P; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - "using YUV420P " << AV_PIX_FMT_YUV420P << " " << avcodec_context_->pix_fmt); + std::cout << "using YUV420P " << AV_PIX_FMT_YUV420P << " "; + std::cout << avcodec_context_->pix_fmt << std::endl; } else { avcodec_context_->pix_fmt = AV_PIX_FMT_YUV422P; - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - "using YUV422P " << AV_PIX_FMT_YUV422P << " " << avcodec_context_->pix_fmt); + std::cout << "using YUV422P " << AV_PIX_FMT_YUV422P << " "; + std::cout << avcodec_context_->pix_fmt << std::endl; } avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO; @@ -146,18 +134,19 @@ int UsbCam::init_decoder( avframe_camera_size_ = avpicture_get_size(AV_PIX_FMT_YUV422P, image_width, image_height); avframe_rgb_size_ = avpicture_get_size(AV_PIX_FMT_RGB24, image_width, image_height); #else - avframe_camera_size_ = av_image_get_buffer_size(AV_PIX_FMT_YUV422P, image_width, image_height, 1); - avframe_rgb_size_ = av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); + avframe_camera_size_ = + av_image_get_buffer_size(AV_PIX_FMT_YUV422P, image_width, image_height, 1); + avframe_rgb_size_ = + av_image_get_buffer_size(AV_PIX_FMT_RGB24, image_width, image_height, 1); #endif /* open it */ if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Could not open %s Decoder", codec_name); + std::cout << "Could not open " << codec_name << " decoder" << std::endl; return 0; } - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - "pixel format " << AV_PIX_FMT_YUV422P << " " << avcodec_context_->pix_fmt); + std::cout << "pixel format " << AV_PIX_FMT_YUV422P << " "; + std::cout << avcodec_context_->pix_fmt << std::endl; return 1; } @@ -482,7 +471,7 @@ bool UsbCam::init_read(unsigned int buffer_size) buffers_ = reinterpret_cast(calloc(1, sizeof(*buffers_))); if (!buffers_) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); + std::cerr << "Out of memory" << std::endl; return false; } @@ -490,7 +479,7 @@ bool UsbCam::init_read(unsigned int buffer_size) buffers_[0].start = malloc(buffer_size); if (!buffers_[0].start) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); + std::cerr << "Out of memory" << std::endl; return false; } return true; @@ -508,9 +497,7 @@ bool UsbCam::init_mmap(void) if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { if (EINVAL == errno) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " does not support memory mapping"); + std::cerr << camera_dev_ << " does not support memory mapping" << std::endl; return false; } else { std::cerr << "error, quitting, TODO throw " << errno << std::endl; @@ -519,16 +506,14 @@ bool UsbCam::init_mmap(void) } if (req.count < 2) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - "Insufficient buffer memory on " << camera_dev_); + std::cerr << "Insufficient buffer memory on " << camera_dev_ << std::endl; return false; } buffers_ = reinterpret_cast(calloc(req.count, sizeof(*buffers_))); if (!buffers_) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); + std::cerr << "Out of memory" << std::endl; return false; } @@ -576,9 +561,7 @@ bool UsbCam::init_userp(unsigned int buffer_size) if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_REQBUFS), &req)) { if (EINVAL == errno) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " does not support user pointer i/o"); + std::cerr << camera_dev_ << " does not support user pointer i/o" << std::endl; return false; // (EXIT_FAILURE); } else { std::cerr << "error, quitting, TODO throw " << errno << std::endl; @@ -589,7 +572,7 @@ bool UsbCam::init_userp(unsigned int buffer_size) buffers_ = reinterpret_cast(calloc(4, sizeof(*buffers_))); if (!buffers_) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); + std::cerr << "Out of memory" << std::endl; return false; // (EXIT_FAILURE); } @@ -598,7 +581,7 @@ bool UsbCam::init_userp(unsigned int buffer_size) buffers_[n_buffers_].start = memalign(/* boundary */ page_size, buffer_size); if (!buffers_[n_buffers_].start) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Out of memory"); + std::cerr << "Out of memory" << std::endl; return false; } } @@ -615,9 +598,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QUERYCAP), &cap)) { if (EINVAL == errno) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " is no V4L2 device"); + std::cerr << camera_dev_ << " is no V4L2 device" << std::endl; return false; // (EXIT_FAILURE); } else { std::cerr << "error, quitting, TODO throw " << errno << std::endl; @@ -626,18 +607,14 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer } if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " is no video capture device"); + std::cerr << camera_dev_ << " is no video capture device" << std::endl; return false; // (EXIT_FAILURE); } switch (io_) { case io_method::IO_METHOD_READ: if (!(cap.capabilities & V4L2_CAP_READWRITE)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " does not support read i/o"); + std::cerr << camera_dev_ << " does not support read i/o" << std::endl; return false; // (EXIT_FAILURE); } @@ -646,9 +623,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer case io_method::IO_METHOD_MMAP: case io_method::IO_METHOD_USERPTR: if (!(cap.capabilities & V4L2_CAP_STREAMING)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - camera_dev_ << " does not support streaming i/o"); + std::cout << camera_dev_ << " does not support streaming i/o" << std::endl; return false; // (EXIT_FAILURE); } @@ -715,14 +690,12 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer memset(&stream_params, 0, sizeof(stream_params)); stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; if (usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_G_PARM), &stream_params) < 0) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("usb_cam"), "can't set stream params " << errno); + std::cerr << "can't set stream params " << errno << std::endl; return false; // ("Couldn't query v4l fps!"); } - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - "Capability flag: 0x" << std::hex << stream_params.parm.capture.capability << std::dec); + if (!stream_params.parm.capture.capability && V4L2_CAP_TIMEPERFRAME) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "V4L2_CAP_TIMEPERFRAME not supported"); + std::cerr << "V4L2_CAP_TIMEPERFRAME not supported" << std::endl; } // TODO(lucasw) need to get list of valid numerator/denominator pairs @@ -730,9 +703,7 @@ bool UsbCam::init_device(uint32_t image_width, uint32_t image_height, int framer stream_params.parm.capture.timeperframe.numerator = 1; stream_params.parm.capture.timeperframe.denominator = framerate; if (usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_S_PARM), &stream_params) < 0) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Couldn't set camera framerate"); - } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger("usb_cam"), "Set framerate to be " << framerate); + std::cerr << "Couldn't set camera framerate" << std::endl; } switch (io_) { @@ -770,23 +741,21 @@ bool UsbCam::open_device(void) struct stat st; if (-1 == stat(camera_dev_.c_str(), &st)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - "Cannot identify '" << camera_dev_ << "': " << errno << ", " << strerror(errno)); + std::cerr << "Cannot identify '" << camera_dev_ << "': " << errno << ", "; + std::cerr << strerror(errno) << std::endl; return false; // (EXIT_FAILURE); } if (!S_ISCHR(st.st_mode)) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("usb_cam"), camera_dev_ << " is no device"); + std::cerr << camera_dev_ << " is no device" << std::endl; return false; // (EXIT_FAILURE); } fd_ = open(camera_dev_.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0); if (-1 == fd_) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - "Cannot open '" << camera_dev_ << "': " << errno << ", " << strerror(errno)); + std::cerr << "Cannot open '" << camera_dev_ << "': " << errno << ", "; + std::cerr << strerror(errno) << std::endl; return false; // (EXIT_FAILURE); } return true; @@ -823,7 +792,7 @@ bool UsbCam::start( } else if (pixel_format == pixel_format::PIXEL_FORMAT_YU12) { pixelformat_ = V4L2_PIX_FMT_YUV420; } else { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "Unknown pixel format."); + std::cerr << "Unknown pixel format" << std::endl; return false; // (EXIT_FAILURE); } @@ -981,14 +950,12 @@ bool UsbCam::grab_image() return false; } - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("usb_cam"), - "Something went wrong, exiting..." << errno); + std::cerr << "Something went wrong, exiting..." << errno << std::endl; return false; // ("select"); } if (0 == r) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "select timeout"); + std::cerr << "Select timeout, exiting..." << std::endl; return false; } @@ -1010,14 +977,14 @@ bool UsbCam::set_auto_focus(int value) if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_QUERYCTRL), &queryctrl)) { if (errno != EINVAL) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "VIDIOC_QUERYCTRL"); + std::cerr << "VIDIOC_QUERYCTRL" << std::endl; return false; } else { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "V4L2_CID_FOCUS_AUTO is not supported"); + std::cerr << "V4L2_CID_FOCUS_AUTO is not supported" << std::endl; return false; } } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "V4L2_CID_FOCUS_AUTO is not supported"); + std::cerr << "V4L2_CID_FOCUS_AUTO is not supported" << std::endl; return false; } else { memset(&control, 0, sizeof(control)); @@ -1025,7 +992,7 @@ bool UsbCam::set_auto_focus(int value) control.value = value; if (-1 == usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_S_CTRL), &control)) { - RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "VIDIOC_S_CTRL"); + std::cerr << "VIDIOC_S_CTRL" << std::endl; return false; } } @@ -1073,11 +1040,11 @@ bool UsbCam::set_v4l_parameter(const std::string & param, const std::string & va pclose(stream); // any output should be an error if (output.length() > 0) { - RCLCPP_WARN(rclcpp::get_logger("usb_cam"), "%s", output.c_str()); + std::cout << output.c_str() << std::endl; retcode = 1; } } else { - RCLCPP_WARN(rclcpp::get_logger("usb_cam"), "usb_cam_node could not run '%s'", cmd.c_str()); + std::cerr << "usb_cam_node could not run '" << cmd.c_str() << "'" << std::endl; retcode = 1; } return retcode; From 639d41692fc83a42d61aca033592a3d2a2d6ecd4 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Wed, 30 Nov 2022 22:30:12 -0800 Subject: [PATCH 08/12] Add integration test for usb_cam lib Signed-off-by: Evan Flynn --- CMakeLists.txt | 2 ++ include/usb_cam/conversions.hpp | 2 +- src/usb_cam.cpp | 1 + test/test_usb_cam_lib.cpp | 49 +++++++++++++++++++++++++++++++++ 4 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 test/test_usb_cam_lib.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fa6e8c2e..c723c197 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,8 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +find_package(OpenCV REQUIRED) + find_package(PkgConfig REQUIRED) pkg_check_modules(avcodec REQUIRED libavcodec) pkg_check_modules(avutil REQUIRED libavutil) diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp index bef27af9..b30c404e 100644 --- a/include/usb_cam/conversions.hpp +++ b/include/usb_cam/conversions.hpp @@ -32,7 +32,7 @@ #include -#include +#include "opencv4/opencv2/imgproc.hpp" #include "usb_cam/constants.hpp" #include "usb_cam/utils.hpp" diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index 32453029..c72ab25f 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -36,6 +36,7 @@ extern "C" { #define __STDC_CONSTANT_MACROS // Required for libavutil #include #include // for O_* constants +#include // for getpagesize() } #include diff --git a/test/test_usb_cam_lib.cpp b/test/test_usb_cam_lib.cpp new file mode 100644 index 00000000..de814dba --- /dev/null +++ b/test/test_usb_cam_lib.cpp @@ -0,0 +1,49 @@ +// Copyright 2022 Evan Flynn +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Evan Flynn nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "usb_cam/usb_cam.hpp" + + +TEST(test_usb_cam_lib, test_usb_cam_class) { + usb_cam::UsbCam test_usb_cam; + + test_usb_cam.start( + "/dev/video0", + usb_cam::utils::IO_METHOD_MMAP, + usb_cam::utils::PIXEL_FORMAT_YUYV, + usb_cam::utils::COLOR_FORMAT_YUV420P, + 640, 480, 10); + + test_usb_cam.get_formats(); + + // TODO(flynneva): uncomment once /dev/video0 can be simulated in CI + // EXPECT_TRUE(test_usb_cam.is_capturing()); + // EXPECT_TRUE(test_usb_cam.shutdown()); +} From e030b10263387f6e4b73a3fff62d579571c5de81 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Thu, 1 Dec 2022 19:06:07 -0800 Subject: [PATCH 09/12] Enable code coverage using lcov Signed-off-by: Evan Flynn --- .github/workflows/build_test.yml | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index b94ea7ef..5191eebb 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -44,8 +44,28 @@ jobs: with: path: ros_ws/src - name: build and test + id: build_and_test_step uses: ros-tooling/action-ros-ci@master with: package-name: usb_cam target-ros2-distro: ${{ matrix.ros_distribution }} vcs-repo-file-url: "" + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + # If possible, pin the repository in the workflow to a specific commit to avoid + # changes in colcon-mixin-repository from breaking your tests. + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/1ddb69bedfd1f04c2f000e95452f7c24a4d6176b/index.yaml + - uses: actions/upload-artifact@v1 + with: + name: colcon-logs-${{ matrix.ros_distribution }} + path: ${{ steps.build_and_test_step.outputs.ros-workspace-directory-name }}/log + if: always() + - uses: actions/upload-artifact@v1 + with: + name: lcov-logs-${{ matrix.ros_distribution }} + path: ${{ steps.build_and_test_step.outputs.ros-workspace-directory-name }}/lcov + if: always() \ No newline at end of file From e32a6e7f862f4bba336e40ed813e5c7b060e7723 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Fri, 2 Dec 2022 18:27:35 -0800 Subject: [PATCH 10/12] Fix MJPEG2RGB conversion function Signed-off-by: Evan Flynn --- include/usb_cam/conversions.hpp | 134 +++++++++++++++----------------- src/usb_cam.cpp | 1 + 2 files changed, 63 insertions(+), 72 deletions(-) diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp index b30c404e..c4d411ed 100644 --- a/include/usb_cam/conversions.hpp +++ b/include/usb_cam/conversions.hpp @@ -50,79 +50,69 @@ namespace conversions /// @param len Length of MJPEG image in memory /// @param RGB Output RGB image to fill /// @param NumPixels Length of output RGB image in memory -inline bool MJPEG2RGB(char * MJPEG, int len, char * RGB, int NumPixels) +inline bool MJPEG2RGB( + usb_cam::UsbCam * usb_cam_obj, char * MJPEG, int len, char * RGB, const int & NumPixels) { - (void)MJPEG; - (void)len; - (void)RGB; - (void)NumPixels; -// int got_picture = 1; -// -// (void)len; -// -// // clear the picture -// memset(RGB, 0, NumPixels); -// -// #if LIBAVCODEC_VERSION_MAJOR > 52 -// // TODO(flynneva): what is this checking? -// if (avcodec_context_->codec_type == AVMEDIA_TYPE_VIDEO) -// #else -// avcodec_decode_video( -// avcodec_context_, avframe_camera_, &got_picture, reinterpret_cast(MJPEG), len); -// #endif -// -// {if (!got_picture) { -// RCLCPP_ERROR( -// rclcpp::get_logger("usb_cam"), "Webcam: expected picture but didn't get it..."); -// return false; -// } -// } -// -// int xsize = avcodec_context_->width; -// int ysize = avcodec_context_->height; -// #if LIBAVCODEC_VERSION_MAJOR > 52 -// int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize, 1); -// #else -// // TODO(lucasw) avpicture_get_size corrupts the pix_fmt -// int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize); -// #endif -// -// // int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize); -// if (pic_size != avframe_camera_size_) { -// RCLCPP_ERROR( -// rclcpp::get_logger("usb_cam"), -// "outbuf size mismatch. pic_size: %d bufsize: %d", pic_size, avframe_camera_size_); -// return false; -// } -// -// // TODO(lucasw) why does the image need to be scaled? Does it also convert formats? -// // RCLCPP_INFO_STREAM( -// // rclcpp::get_logger("usb_cam"), "sw scaler " << xsize << " " << ysize << " " -// // << avcodec_context_->pix_fmt << ", linesize " << avframe_rgb_->linesize); -// // TODO(lucasw) only do if xsize and ysize or pix fmt is different from last time -// video_sws_ = sws_getContext( -// xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, -// AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL); -// sws_scale( -// video_sws_, avframe_camera_->data, avframe_camera_->linesize, -// 0, ysize, avframe_rgb_->data, avframe_rgb_->linesize); -// // TODO(lucasw) keep around until parameters change -// sws_freeContext(video_sws_); -// -// #if LIBAVCODEC_VERSION_MAJOR > 52 -// int size = av_image_copy_to_buffer( -// reinterpret_cast(avframe_rgb_), pic_size, avframe_camera_->data, -// avframe_camera_->linesize, avcodec_context_->pix_fmt, xsize, ysize, 1); -// #else -// int size = avpicture_layout( -// reinterpret_cast(avframe_rgb_), AV_PIX_FMT_RGB24, -// xsize, ysize, reinterpret_cast(RGB), avframe_rgb_size_); -// #endif -// -// if (size != avframe_rgb_size_) { -// RCLCPP_ERROR(rclcpp::get_logger("usb_cam"), "webcam: avpicture_layout error: %d", size); -// return false; -// } + int got_picture = 1; + + // clear the picture + memset(RGB, 0, NumPixels); + + auto avcodec_context_ = usb_cam_obj->get_avcodec_context(); + auto avframe_camera_ = usb_cam_obj->get_avframe_camera(); + auto avframe_camera_size_ = usb_cam_obj->get_avframe_camera_size(); + auto avframe_rgb_ = usb_cam_obj->get_avframe_rgb(); + auto avframe_rgb_size_ = usb_cam_obj->get_avframe_rgb_size(); + +#if LIBAVCODEC_VERSION_MAJOR > 52 + avcodec_receive_frame(avcodec_context_, avframe_camera_); + // avcodec_send_packet(avcodec_context_, ); +#else + avcodec_decode_video2( + avcodec_context_, avframe_camera_, &got_picture, reinterpret_cast(MJPEG), len); +#endif + + if (!got_picture) { + std::cerr << "Webcam: expected picture but didn't get it..." << std::endl; + return false; + } + + int xsize = avcodec_context_->width; + int ysize = avcodec_context_->height; +#if LIBAVCODEC_VERSION_MAJOR > 52 + int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize, 1); +#else + // TODO(lucasw) avpicture_get_size corrupts the pix_fmt + int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize); +#endif + // int pic_size = av_image_get_buffer_size(avcodec_context_->pix_fmt, xsize, ysize); + if (pic_size != avframe_camera_size_) { + std::cerr << "Outbuf size mismatch. pic_size: " << pic_size; + std::cerr << " bufsize: " << avframe_camera_size_ << std::endl; + return false; + } + // TODO(lucasw) why does the image need to be scaled? Does it also convert formats? + // RCLCPP_INFO_STREAM( + // std::cout << "sw scaler " << xsize << " " << ysize << " " + // << avcodec_context_->pix_fmt << ", linesize " << avframe_rgb_->linesize << std::endl; + // TODO(lucasw) only do if xsize and ysize or pix fmt is different from last time + usb_cam_obj->get_video_sws(); + usb_cam_obj->scale(); +#if LIBAVCODEC_VERSION_MAJOR > 52 + int size = av_image_copy_to_buffer( + reinterpret_cast(avframe_rgb_), pic_size, avframe_camera_->data, + avframe_camera_->linesize, avcodec_context_->pix_fmt, xsize, ysize, 1); +#else + int size = avpicture_layout( + reinterpret_cast(avframe_rgb_), AV_PIX_FMT_RGB24, + xsize, ysize, reinterpret_cast(RGB), avframe_rgb_size_); +#endif + + if (size != avframe_rgb_size_) { + std::cerr << "avpicture_layout error: " << size << std::endl; + return false; + } + return true; } diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index c72ab25f..2c91ec04 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -182,6 +182,7 @@ bool UsbCam::process_image(const void * src, int len, camera_image_t * dest) reinterpret_cast(src)), dest->image, dest->width * dest->height); } else if (pixelformat_ == V4L2_PIX_FMT_MJPEG || pixelformat_ == V4L2_PIX_FMT_H264) { result = conversions::MJPEG2RGB( + this, const_cast( reinterpret_cast(src)), len, dest->image, dest->width * dest->height); } else if (pixelformat_ == V4L2_PIX_FMT_RGB24 || pixelformat_ == V4L2_PIX_FMT_GREY) { From 8bd77756fd92c24242cc23c245052a307aec81ba Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Fri, 2 Dec 2022 18:28:22 -0800 Subject: [PATCH 11/12] Improve supported formats method for UsbCam object Signed-off-by: Evan Flynn --- include/usb_cam/usb_cam.hpp | 116 +++++++++++++++++++++++++++++++++++- src/usb_cam.cpp | 76 +++++++++++------------ src/usb_cam_node.cpp | 15 ++++- test/test_usb_cam_lib.cpp | 30 ++++++---- 4 files changed, 180 insertions(+), 57 deletions(-) diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 30a9e035..5bb181fe 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -33,6 +33,7 @@ extern "C" { #include #include +#include } #include @@ -68,6 +69,14 @@ typedef struct } camera_image_t; +typedef struct +{ + struct v4l2_fmtdesc format; + struct v4l2_frmsizeenum size; + struct v4l2_frmivalenum interval; +} capture_format_t; + + class UsbCam { public: @@ -87,7 +96,7 @@ class UsbCam struct timespec & stamp, std::string & encoding, uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data); - void get_formats(); // std::vector& formats); + std::vector get_supported_formats(); // enables/disable auto focus bool set_auto_focus(int value); @@ -98,7 +107,109 @@ class UsbCam bool stop_capturing(void); bool start_capturing(void); - bool is_capturing(); + + inline void scale() + { + sws_scale( + video_sws_, avframe_camera_->data, avframe_camera_->linesize, + 0, avcodec_context_->height, avframe_rgb_->data, avframe_rgb_->linesize); + // TODO(lucasw) keep around until parameters change + sws_freeContext(video_sws_); + } + + inline std::string get_camera_dev() + { + return camera_dev_; + } + + inline unsigned int get_pixelformat() + { + return pixelformat_; + } + + inline bool is_monochrome() + { + return monochrome_; + } + + inline usb_cam::utils::io_method get_io_method() + { + return io_; + } + + inline int get_fd() + { + return fd_; + } + + inline usb_cam::utils::buffer * get_buffers() + { + return buffers_; + } + + inline unsigned int number_of_buffers() + { + return n_buffers_; + } + + inline AVFrame * get_avframe_camera() + { + return avframe_camera_; + } + + inline AVFrame * get_avframe_rgb() + { + return avframe_rgb_; + } + + inline AVCodec * get_avcodec() + { + return avcodec_; + } + + inline AVDictionary * get_avoptions() + { + return avoptions_; + } + + inline AVCodecContext * get_avcodec_context() + { + return avcodec_context_; + } + + inline int get_avframe_camera_size() + { + return avframe_camera_size_; + } + + inline int get_avframe_rgb_size() + { + return avframe_rgb_size_; + } + + inline struct SwsContext * get_video_sws() + { + video_sws_ = sws_getContext( + avcodec_context_->width, avcodec_context_->height, avcodec_context_->pix_fmt, + avcodec_context_->width, avcodec_context_->height, + AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL); + return video_sws_; + } + + inline camera_image_t * get_current_image() + { + return image_; + } + + inline bool is_capturing() + { + return is_capturing_; + } + + inline time_t get_epoch_time_shift() + { + return epoch_time_shift_; + } private: int init_decoder( @@ -106,7 +217,6 @@ class UsbCam AVCodecID codec_id, const char * codec_name); int init_h264_decoder(int image_width, int image_height, color_format cf); int init_mjpeg_decoder(int image_width, int image_height, color_format cf); - bool mjpeg2rgb(char * MJPEG, int len, char * RGB, int NumPixels); bool process_image(const void * src, int len, camera_image_t * dest); bool read_frame(); bool uninit_device(void); diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index 2c91ec04..0962324e 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -332,11 +332,6 @@ bool UsbCam::read_frame() return true; } -bool UsbCam::is_capturing() -{ - return is_capturing_; -} - bool UsbCam::stop_capturing(void) { if (!is_capturing_) {return false;} @@ -881,51 +876,48 @@ bool UsbCam::get_image( return true; } -void UsbCam::get_formats() // std::vector& formats) +std::vector UsbCam::get_supported_formats() { - RCLCPP_INFO(rclcpp::get_logger("usb_cam"), "This Cameras Supported Formats:"); - struct v4l2_fmtdesc fmt; - fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - fmt.index = 0; - for (fmt.index = 0; usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FMT), &fmt) == 0; - ++fmt.index) + std::vector supported_formats; + struct v4l2_fmtdesc current_format; + current_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + current_format.index = 0; + for (current_format.index = 0; + usb_cam::utils::xioctl( + fd_, static_cast(VIDIOC_ENUM_FMT), ¤t_format) == 0; + ++current_format.index) { - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - " " << fmt.description << "[Index: " << fmt.index << ", Type: " << fmt.type << - ", Flags: " << fmt.flags << ", PixelFormat: " << std::hex << fmt.pixelformat << "]"); - - struct v4l2_frmsizeenum size; - size.index = 0; - size.pixel_format = fmt.pixelformat; - - for (size.index = 0; - usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMESIZES), &size) == 0; - ++size.index) + struct v4l2_frmsizeenum current_size; + current_size.index = 0; + current_size.pixel_format = current_format.pixelformat; + + for (current_size.index = 0; + usb_cam::utils::xioctl( + fd_, static_cast(VIDIOC_ENUM_FRAMESIZES), ¤t_size) == 0; + ++current_size.index) { - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - " width: " << size.discrete.width << " x height: " << size.discrete.height); - struct v4l2_frmivalenum interval; - interval.index = 0; - interval.pixel_format = size.pixel_format; - interval.width = size.discrete.width; - interval.height = size.discrete.height; - for (interval.index = 0; - usb_cam::utils::xioctl(fd_, static_cast(VIDIOC_ENUM_FRAMEINTERVALS), &interval) == 0; - ++interval.index) + struct v4l2_frmivalenum current_interval; + current_interval.index = 0; + current_interval.pixel_format = current_size.pixel_format; + current_interval.width = current_size.discrete.width; + current_interval.height = current_size.discrete.height; + for (current_interval.index = 0; + usb_cam::utils::xioctl( + fd_, static_cast(VIDIOC_ENUM_FRAMEINTERVALS), ¤t_interval) == 0; + ++current_interval.index) { - if (interval.type == V4L2_FRMIVAL_TYPE_DISCRETE) { - RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), - " " << interval.type << " " << interval.discrete.numerator << " / " << - interval.discrete.denominator); - } else { - RCLCPP_INFO(rclcpp::get_logger("usb_cam"), "other type"); + if (current_interval.type == V4L2_FRMIVAL_TYPE_DISCRETE) { + capture_format_t capture_format; + capture_format.format = current_format; + capture_format.size = current_size; + capture_format.interval = current_interval; + supported_formats.push_back(capture_format); } } // interval loop } // size loop } // fmt loop + + return supported_formats; } bool UsbCam::grab_image() diff --git a/src/usb_cam_node.cpp b/src/usb_cam_node.cpp index 6595eecc..9a285f9b 100644 --- a/src/usb_cam_node.cpp +++ b/src/usb_cam_node.cpp @@ -147,7 +147,18 @@ void UsbCamNode::init() cam_.start( video_device_name_.c_str(), io_method, pixel_format, color_format, image_width_, image_height_, framerate_); - cam_.get_formats(); + auto supported_formats = cam_.get_supported_formats(); + + RCLCPP_INFO(this->get_logger(), "This devices supproted formats:"); + for (auto fmt : supported_formats) { + RCLCPP_INFO( + this->get_logger(), + "\t%s: %d x %d (%d Hz)", + fmt.format.description, + fmt.interval.width, + fmt.interval.height, + fmt.interval.discrete.denominator / fmt.interval.discrete.numerator); + } // TODO(lucasw) should this check a little faster than expected frame rate? // TODO(lucasw) how to do small than ms, or fractional ms- std::chrono::nanoseconds? @@ -155,7 +166,7 @@ void UsbCamNode::init() timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(period_ms)), std::bind(&UsbCamNode::update, this)); - RCLCPP_INFO_STREAM(this->get_logger(), "starting timer " << period_ms); + RCLCPP_INFO_STREAM(this->get_logger(), "Timer triggering every " << period_ms << " ms"); } void UsbCamNode::get_params() diff --git a/test/test_usb_cam_lib.cpp b/test/test_usb_cam_lib.cpp index de814dba..6492796d 100644 --- a/test/test_usb_cam_lib.cpp +++ b/test/test_usb_cam_lib.cpp @@ -28,22 +28,32 @@ #include +#include +#include +#include + #include "usb_cam/usb_cam.hpp" TEST(test_usb_cam_lib, test_usb_cam_class) { usb_cam::UsbCam test_usb_cam; - test_usb_cam.start( - "/dev/video0", - usb_cam::utils::IO_METHOD_MMAP, - usb_cam::utils::PIXEL_FORMAT_YUYV, - usb_cam::utils::COLOR_FORMAT_YUV420P, - 640, 480, 10); + auto supported_fmts = test_usb_cam.get_supported_formats(); - test_usb_cam.get_formats(); + // TODO(flynneva): iterate over availble formats with test_usb_cam obj + for (auto fmt : supported_fmts) { + std::cerr << "format: " << fmt.format.type << std::endl; + } - // TODO(flynneva): uncomment once /dev/video0 can be simulated in CI - // EXPECT_TRUE(test_usb_cam.is_capturing()); - // EXPECT_TRUE(test_usb_cam.shutdown()); + { + test_usb_cam.start( + "/dev/video0", + usb_cam::utils::IO_METHOD_MMAP, + usb_cam::utils::PIXEL_FORMAT_YUYV, + usb_cam::utils::COLOR_FORMAT_YUV422P, + 640, 480, 30); + // TODO(flynneva): uncomment once /dev/video0 can be simulated in CI + // EXPECT_TRUE(test_usb_cam.is_capturing()); + test_usb_cam.shutdown(); + } } From 9b8b41366e7addcf066d5579485bb9c1619cfacd Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Fri, 2 Dec 2022 20:31:52 -0800 Subject: [PATCH 12/12] Add back in missing copyrights Signed-off-by: Evan Flynn --- include/usb_cam/constants.hpp | 1 + include/usb_cam/conversions.hpp | 1 + include/usb_cam/usb_cam_node.hpp | 1 + 3 files changed, 3 insertions(+) diff --git a/include/usb_cam/constants.hpp b/include/usb_cam/constants.hpp index 3aef0072..32a7d0df 100644 --- a/include/usb_cam/constants.hpp +++ b/include/usb_cam/constants.hpp @@ -1,4 +1,5 @@ // Copyright 2022 Evan Flynn +// Copyright 2014 Robert Bosch, LLC // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: diff --git a/include/usb_cam/conversions.hpp b/include/usb_cam/conversions.hpp index c4d411ed..feb32b05 100644 --- a/include/usb_cam/conversions.hpp +++ b/include/usb_cam/conversions.hpp @@ -1,4 +1,5 @@ // Copyright 2022 Evan Flynn +// Copyright 2014 Robert Bosch, LLC // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: diff --git a/include/usb_cam/usb_cam_node.hpp b/include/usb_cam/usb_cam_node.hpp index c9074789..0df91589 100644 --- a/include/usb_cam/usb_cam_node.hpp +++ b/include/usb_cam/usb_cam_node.hpp @@ -1,4 +1,5 @@ // Copyright 2021 Evan Flynn +// Copyright 2014 Robert Bosch, LLC // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: