From 53927746809642679fa9da2f65d333875d14eab9 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Tue, 28 Nov 2023 21:33:15 -0800 Subject: [PATCH 1/3] Add common arguments strut to pixel format classes - Use common arguments struct in all format classes Signed-off-by: Evan Flynn --- .../formats/av_pixel_format_helper.hpp | 33 +++++++++++++++++++ include/usb_cam/formats/m420.hpp | 6 ++-- include/usb_cam/formats/mjpeg.hpp | 31 +++++++++-------- include/usb_cam/formats/mono.hpp | 16 +++++---- include/usb_cam/formats/pixel_format_base.hpp | 15 +++++++++ include/usb_cam/formats/rgb.hpp | 6 ++-- include/usb_cam/formats/uyvy.hpp | 10 +++--- include/usb_cam/formats/yuyv.hpp | 10 +++--- 8 files changed, 92 insertions(+), 35 deletions(-) diff --git a/include/usb_cam/formats/av_pixel_format_helper.hpp b/include/usb_cam/formats/av_pixel_format_helper.hpp index 4b0db755..ad1792ec 100644 --- a/include/usb_cam/formats/av_pixel_format_helper.hpp +++ b/include/usb_cam/formats/av_pixel_format_helper.hpp @@ -960,6 +960,7 @@ inline AVPixelFormat get_av_pixel_format_from_string(const std::string & str) return STR_2_AVPIXFMT.find(fullFmtStr)->second; } + /// @brief Get ROS PixelFormat from AVPixelFormat. /// @param avPixelFormat AVPixelFormat /// @return String specifying the ROS pixel format. @@ -1032,6 +1033,17 @@ inline std::string get_ros_pixel_format_from_av_format(const AVPixelFormat & avP return ros_format; } + +/// @brief Overload to support av pixel formats being passed as strings +/// @param avPixelFormat AvPixelFormat as string +/// @return String specifying the ROS pixel format. +inline std::string get_ros_pixel_format_from_av_format(const std::string avPixelFormatStr) +{ + auto fmt = get_av_pixel_format_from_string(avPixelFormatStr); + return get_ros_pixel_format_from_av_format(fmt); +} + + /// @brief Get the number of channels from AVPixelFormat. /// @param avPixelFormat AVPixelFormat /// @return Number of channels as uint8 @@ -1074,6 +1086,17 @@ inline uint8_t get_channels_from_av_format(const AVPixelFormat & avPixelFormat) return channels; } + +/// @brief Overload of function below to support av pixel formats as strings +/// @param avPixelFormatStr AvPixelFormat as string +/// @return Number of channels as uint8_t +inline uint8_t get_channels_from_av_format(const std::string & avPixelFormatStr) +{ + auto fmt = get_av_pixel_format_from_string(avPixelFormatStr); + return get_channels_from_av_format(fmt); +} + + /// @brief Get the pixel bit depth from AVPixelFormat. /// @param avPixelFormat AVPixelFormat /// @return Bit depth as uint8 @@ -1106,6 +1129,16 @@ inline uint8_t get_bit_depth_from_av_format(const AVPixelFormat & avPixelFormat) return bit_depth; } + +/// @brief Overload of function below to support passing av pixel formats as strings +/// @param avPixelFormatStr AVPixelFormat as string +/// @return Bit depth as uint8 +inline uint8_t get_bit_depth_from_av_format(const std::string & avPixelFormatStr) +{ + auto fmt = get_av_pixel_format_from_string(avPixelFormatStr); + return get_bit_depth_from_av_format(fmt); +} + } // namespace formats } // namespace usb_cam diff --git a/include/usb_cam/formats/m420.hpp b/include/usb_cam/formats/m420.hpp index b374acc9..e490330f 100644 --- a/include/usb_cam/formats/m420.hpp +++ b/include/usb_cam/formats/m420.hpp @@ -46,7 +46,7 @@ namespace formats class M4202RGB : public pixel_format_base { public: - M4202RGB(const int & width, const int & height) + explicit M4202RGB(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "m4202rgb", V4L2_PIX_FMT_M420, @@ -54,8 +54,8 @@ class M4202RGB : public pixel_format_base 3, 8, true), - m_width(width), - m_height(height) + m_width(args.width), + m_height(args.height) {} /// @brief Convert a YUV420 (aka M420) image to RGB8 diff --git a/include/usb_cam/formats/mjpeg.hpp b/include/usb_cam/formats/mjpeg.hpp index da3b4b15..2faff235 100644 --- a/include/usb_cam/formats/mjpeg.hpp +++ b/include/usb_cam/formats/mjpeg.hpp @@ -63,22 +63,21 @@ namespace formats class RAW_MJPEG : public pixel_format_base { public: - explicit RAW_MJPEG(const AVPixelFormat & avDeviceFormat) + explicit RAW_MJPEG(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "raw_mjpeg", V4L2_PIX_FMT_MJPEG, - get_ros_pixel_format_from_av_format(avDeviceFormat), - get_channels_from_av_format(avDeviceFormat), - get_bit_depth_from_av_format(avDeviceFormat), + get_ros_pixel_format_from_av_format(args.av_device_format_str), + get_channels_from_av_format(args.av_device_format_str), + get_bit_depth_from_av_format(args.av_device_format_str), false) - { - } + {} }; class MJPEG2RGB : public pixel_format_base { public: - MJPEG2RGB(const int & width, const int & height, const AVPixelFormat & avDeviceFormat) + explicit MJPEG2RGB(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "mjpeg2rgb", V4L2_PIX_FMT_MJPEG, @@ -103,18 +102,18 @@ class MJPEG2RGB : public pixel_format_base m_avcodec_context = avcodec_alloc_context3(m_avcodec); - m_avframe_device->width = width; - m_avframe_device->height = height; + m_avframe_device->width = args.width; + m_avframe_device->height = args.height; m_avframe_device->format = AV_PIX_FMT_YUV422P; - m_avframe_device->format = avDeviceFormat; + m_avframe_device->format = formats::get_av_pixel_format_from_string(args.av_device_format_str); - m_avframe_rgb->width = width; - m_avframe_rgb->height = height; + m_avframe_rgb->width = args.width; + m_avframe_rgb->height = args.height; m_avframe_rgb->format = AV_PIX_FMT_RGB24; m_sws_context = sws_getContext( - width, height, (AVPixelFormat)m_avframe_device->format, - width, height, (AVPixelFormat)m_avframe_rgb->format, SWS_FAST_BILINEAR, + args.width, args.height, (AVPixelFormat)m_avframe_device->format, + args.width, args.height, (AVPixelFormat)m_avframe_rgb->format, SWS_FAST_BILINEAR, NULL, NULL, NULL); // Suppress warnings from ffmpeg libraries to avoid spamming the console @@ -122,8 +121,8 @@ class MJPEG2RGB : public pixel_format_base av_log_set_flags(AV_LOG_SKIP_REPEATED); // av_log_set_flags(AV_LOG_PRINT_LEVEL); - m_avcodec_context->width = width; - m_avcodec_context->height = height; + m_avcodec_context->width = args.width; + m_avcodec_context->height = args.height; m_avcodec_context->pix_fmt = (AVPixelFormat)m_avframe_device->format; m_avcodec_context->codec_type = AVMEDIA_TYPE_VIDEO; diff --git a/include/usb_cam/formats/mono.hpp b/include/usb_cam/formats/mono.hpp index e932ea94..507988f7 100644 --- a/include/usb_cam/formats/mono.hpp +++ b/include/usb_cam/formats/mono.hpp @@ -44,7 +44,7 @@ namespace formats class MONO8 : public pixel_format_base { public: - MONO8() + explicit MONO8(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "mono8", V4L2_PIX_FMT_GREY, @@ -52,14 +52,16 @@ class MONO8 : public pixel_format_base 1, 8, false) - {} + { + (void)args; + } }; class MONO16 : public pixel_format_base { public: - MONO16() + explicit MONO16(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "mono16", V4L2_PIX_FMT_Y16, @@ -67,7 +69,9 @@ class MONO16 : public pixel_format_base 1, 16, false) - {} + { + (void)args; + } }; @@ -75,7 +79,7 @@ class MONO16 : public pixel_format_base class Y102MONO8 : public pixel_format_base { public: - explicit Y102MONO8(const int & number_of_pixels) + explicit Y102MONO8(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "y102mono8", V4L2_PIX_FMT_Y10, @@ -83,7 +87,7 @@ class Y102MONO8 : public pixel_format_base 1, 8, true), - m_number_of_pixels(number_of_pixels) + m_number_of_pixels(args.pixels) {} /// @brief Convert a Y10 (MONO10) image to MONO8 diff --git a/include/usb_cam/formats/pixel_format_base.hpp b/include/usb_cam/formats/pixel_format_base.hpp index c7fc599b..610affa1 100644 --- a/include/usb_cam/formats/pixel_format_base.hpp +++ b/include/usb_cam/formats/pixel_format_base.hpp @@ -43,6 +43,21 @@ namespace formats { +/// @brief Helper structure to standardize all pixel_format_base +/// constructors, so that they all have the same argument type. +/// +/// This should also be easily extendable in the future if we need to add additional +/// arguments for future pixel format(s) that are added. +typedef struct +{ + std::string name = ""; + int width = 640; + int height = 480; + size_t pixels = 640 * 480; + std::string av_device_format_str = "AV_PIX_FMT_YUV422P"; +} format_arguments_t; + + /// @brief Base pixel format class. Provide all necessary information for converting between /// V4L2 and ROS formats. Meant to be overridden if conversion function is required. class pixel_format_base diff --git a/include/usb_cam/formats/rgb.hpp b/include/usb_cam/formats/rgb.hpp index 750af588..d2c13490 100644 --- a/include/usb_cam/formats/rgb.hpp +++ b/include/usb_cam/formats/rgb.hpp @@ -44,7 +44,7 @@ namespace formats class RGB8 : public pixel_format_base { public: - RGB8() + explicit RGB8(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "rgb8", V4L2_PIX_FMT_RGB332, @@ -52,7 +52,9 @@ class RGB8 : public pixel_format_base 3, 8, false) - {} + { + (void)args; + } }; } // namespace formats diff --git a/include/usb_cam/formats/uyvy.hpp b/include/usb_cam/formats/uyvy.hpp index afd5d0b2..ef7f136d 100644 --- a/include/usb_cam/formats/uyvy.hpp +++ b/include/usb_cam/formats/uyvy.hpp @@ -44,7 +44,7 @@ namespace formats class UYVY : public pixel_format_base { public: - UYVY() + explicit UYVY(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "uyvy", V4L2_PIX_FMT_UYVY, @@ -52,14 +52,16 @@ class UYVY : public pixel_format_base 2, 8, false) - {} + { + (void)args; + } }; class UYVY2RGB : public pixel_format_base { public: - explicit UYVY2RGB(const int & number_of_pixels) + explicit UYVY2RGB(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "uyvy2rgb", V4L2_PIX_FMT_UYVY, @@ -67,7 +69,7 @@ class UYVY2RGB : public pixel_format_base 2, 8, true), - m_number_of_pixels(number_of_pixels) + m_number_of_pixels(args.pixels) {} /// @brief In this format each four bytes is two pixels. diff --git a/include/usb_cam/formats/yuyv.hpp b/include/usb_cam/formats/yuyv.hpp index abd8c230..bb2505a4 100644 --- a/include/usb_cam/formats/yuyv.hpp +++ b/include/usb_cam/formats/yuyv.hpp @@ -44,7 +44,7 @@ namespace formats class YUYV : public pixel_format_base { public: - YUYV() + explicit YUYV(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "yuyv", V4L2_PIX_FMT_YUYV, @@ -52,14 +52,16 @@ class YUYV : public pixel_format_base 2, 8, false) - {} + { + (void)args; + } }; class YUYV2RGB : public pixel_format_base { public: - explicit YUYV2RGB(const int & number_of_pixels) + explicit YUYV2RGB(const format_arguments_t & args = format_arguments_t()) : pixel_format_base( "yuyv2rgb", V4L2_PIX_FMT_YUYV, @@ -67,7 +69,7 @@ class YUYV2RGB : public pixel_format_base 3, 8, true), - m_number_of_pixels(number_of_pixels) + m_number_of_pixels(args.pixels) {} /// @brief In this format each four bytes is two pixels. From e4afe621fae4aac1756e6273bcf2971ee2e8bf1b Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Tue, 28 Nov 2023 21:34:02 -0800 Subject: [PATCH 2/3] Add v4l2_str method to pixel_format_base class Signed-off-by: Evan Flynn --- include/usb_cam/formats/pixel_format_base.hpp | 8 ++++++-- include/usb_cam/utils.hpp | 3 --- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/usb_cam/formats/pixel_format_base.hpp b/include/usb_cam/formats/pixel_format_base.hpp index 610affa1..62d09b84 100644 --- a/include/usb_cam/formats/pixel_format_base.hpp +++ b/include/usb_cam/formats/pixel_format_base.hpp @@ -35,7 +35,7 @@ #include "linux/videodev2.h" #include "usb_cam/constants.hpp" - +#include "usb_cam/conversions.hpp" namespace usb_cam { @@ -79,9 +79,13 @@ class pixel_format_base inline std::string name() {return m_name;} /// @brief Integer value of V4L2 capture pixel format - /// @return + /// @return uint32_t V4L2 capture pixel format inline uint32_t v4l2() {return m_v4l2;} + /// @brief String value of V4L2 capture pixel format + /// @return std::string V4L2 capture pixel format + inline std::string v4l2_str() { return usb_cam::conversions::FCC2S(m_v4l2); } + /// @brief Name of output pixel (encoding) format to ROS /// @return inline std::string ros() {return m_ros;} diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp index b847b4f2..bf2dbf65 100644 --- a/include/usb_cam/utils.hpp +++ b/include/usb_cam/utils.hpp @@ -46,7 +46,6 @@ extern "C" { #include "linux/videodev2.h" #include "usb_cam/constants.hpp" -#include "usb_cam/formats/pixel_format_base.hpp" namespace usb_cam @@ -54,8 +53,6 @@ namespace usb_cam namespace utils { -using usb_cam::formats::pixel_format_base; - /// @brief Read more on IO methods here: https://lwn.net/Articles/240667/ typedef enum From d9d12d4d7a59ff67a975a88889f0e2ca921be5e1 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Tue, 28 Nov 2023 21:47:31 -0800 Subject: [PATCH 3/3] Check if specified pixel format is supported by driver and by device - Also fix typo in params_2 yaml file - fix linter errors too Signed-off-by: Evan Flynn --- config/params_2.yaml | 4 +- include/usb_cam/formats/pixel_format_base.hpp | 2 +- include/usb_cam/usb_cam.hpp | 136 ++++++++++++------ src/ros2/usb_cam_node.cpp | 12 +- 4 files changed, 97 insertions(+), 57 deletions(-) diff --git a/config/params_2.yaml b/config/params_2.yaml index a4d2cca5..b3c6cb88 100644 --- a/config/params_2.yaml +++ b/config/params_2.yaml @@ -4,8 +4,8 @@ framerate: 15.0 io_method: "mmap" frame_id: "camera2" - pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats - av_device_format: "YUV422P" + pixel_format: "mjpeg2rgb" + av_device_format: "YUV422P" image_width: 640 image_height: 480 camera_name: "test_camera2" diff --git a/include/usb_cam/formats/pixel_format_base.hpp b/include/usb_cam/formats/pixel_format_base.hpp index 62d09b84..7524cadf 100644 --- a/include/usb_cam/formats/pixel_format_base.hpp +++ b/include/usb_cam/formats/pixel_format_base.hpp @@ -84,7 +84,7 @@ class pixel_format_base /// @brief String value of V4L2 capture pixel format /// @return std::string V4L2 capture pixel format - inline std::string v4l2_str() { return usb_cam::conversions::FCC2S(m_v4l2); } + inline std::string v4l2_str() {return usb_cam::conversions::FCC2S(m_v4l2);} /// @brief Name of output pixel (encoding) format to ROS /// @return diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index 3568b350..1c8ef91d 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -61,6 +61,40 @@ namespace usb_cam using usb_cam::utils::io_method_t; using usb_cam::formats::pixel_format_base; +/// @brief Add more formats here and to driver_supported_formats below as +/// they are added to this library +using usb_cam::formats::RGB8; +using usb_cam::formats::YUYV; +using usb_cam::formats::YUYV2RGB; +using usb_cam::formats::UYVY; +using usb_cam::formats::UYVY2RGB; +using usb_cam::formats::MONO8; +using usb_cam::formats::MONO16; +using usb_cam::formats::Y102MONO8; +using usb_cam::formats::RAW_MJPEG; +using usb_cam::formats::MJPEG2RGB; +using usb_cam::formats::M4202RGB; + + +/// @brief list all supported formats that this driver supports +std::vector> driver_supported_formats( + const formats::format_arguments_t & args = formats::format_arguments_t()) +{ + std::vector> fmts = { + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + std::make_shared(args), + }; + return fmts; +} typedef struct { @@ -273,6 +307,51 @@ class UsbCam return m_supported_formats; } + /// @brief Check if the given format is supported by this device + /// If it is supported, set the m_pixel_format variable to it. + /// @param format the format to check if it is supported + /// @return bool true if the given format is supported, false otherwise + inline bool set_pixel_format(const formats::format_arguments_t & args) + { + bool result = false; + + std::shared_ptr found_driver_format = nullptr; + + // First check if given format is supported by this driver + for (auto driver_fmt : driver_supported_formats(args)) { + if (driver_fmt->name() == args.name) { + found_driver_format = driver_fmt; + } + } + + if (found_driver_format == nullptr) { + // List the supported formats of this driver for the user before throwing + std::cerr << "This driver supports the following formats:" << std::endl; + for (auto driver_fmt : driver_supported_formats(args)) { + std::cerr << "\t" << driver_fmt->name() << std::endl; + } + throw std::invalid_argument( + "Specified format `" + args.name + "` is unsupported by this ROS driver" + ); + } + + std::cout << "This device supports the following formats:" << std::endl; + for (auto fmt : this->supported_formats()) { + // Always list the devices supported formats for the user + std::cout << "\t" << fmt.format.description << " "; + std::cout << fmt.v4l2_fmt.width << " x " << fmt.v4l2_fmt.height << " ("; + std::cout << fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator << " Hz)"; + std::cout << std::endl; + + if (fmt.v4l2_fmt.pixel_format == found_driver_format->v4l2()) { + result = true; + m_image.pixel_format = found_driver_format; + } + } + + return result; + } + /// @brief Set pixel format from parameter list. Required to have logic within UsbCam object /// in case pixel format class requires additional information for conversion function /// (e.g. number of pixels, width, height, etc.) @@ -280,50 +359,21 @@ class UsbCam /// @return pixel format structure corresponding to a given name inline std::shared_ptr set_pixel_format(const parameters_t & parameters) { - using usb_cam::formats::RGB8; - using usb_cam::formats::YUYV; - using usb_cam::formats::YUYV2RGB; - using usb_cam::formats::UYVY; - using usb_cam::formats::UYVY2RGB; - using usb_cam::formats::MONO8; - using usb_cam::formats::MONO16; - using usb_cam::formats::Y102MONO8; - using usb_cam::formats::RAW_MJPEG; - using usb_cam::formats::MJPEG2RGB; - using usb_cam::formats::M4202RGB; - - if (parameters.pixel_format_name == "rgb8") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "yuyv") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "yuyv2rgb") { - // number of pixels required for conversion method - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else if (parameters.pixel_format_name == "uyvy") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "uyvy2rgb") { - // number of pixels required for conversion method - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else if (parameters.pixel_format_name == "mjpeg") { - m_image.pixel_format = std::make_shared( - formats::get_av_pixel_format_from_string(parameters.av_device_format)); - } else if (parameters.pixel_format_name == "mjpeg2rgb") { - m_image.pixel_format = std::make_shared( - m_image.width, m_image.height, - formats::get_av_pixel_format_from_string(parameters.av_device_format)); - } else if (parameters.pixel_format_name == "m4202rgb") { - m_image.pixel_format = std::make_shared( - m_image.width, m_image.height); - } else if (parameters.pixel_format_name == "mono8") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "mono16") { - m_image.pixel_format = std::make_shared(); - } else if (parameters.pixel_format_name == "y102mono8") { - m_image.pixel_format = std::make_shared(m_image.number_of_pixels); - } else { + // create format arguments structure + formats::format_arguments_t format_args({ + parameters.pixel_format_name, + parameters.image_width, + parameters.image_height, + m_image.number_of_pixels, + parameters.av_device_format, + }); + + // Look for specified pixel format + if (!this->set_pixel_format(format_args)) { throw std::invalid_argument( - "Unsupported pixel format specified: " + - parameters.pixel_format_name); + "Specified format `" + parameters.pixel_format_name + "` is unsupported by the " + + "selected device `" + parameters.device_name + "`" + ); } return m_image.pixel_format; diff --git a/src/ros2/usb_cam_node.cpp b/src/ros2/usb_cam_node.cpp index 0133ba31..1a1cca97 100644 --- a/src/ros2/usb_cam_node.cpp +++ b/src/ros2/usb_cam_node.cpp @@ -189,20 +189,10 @@ void UsbCamNode::init() rclcpp::shutdown(); return; } + // configure the camera m_camera->configure(m_parameters, io_method); - RCLCPP_INFO(this->get_logger(), "This devices supproted formats:"); - for (auto fmt : m_camera->supported_formats()) { - RCLCPP_INFO( - this->get_logger(), - "\t%s: %d x %d (%d Hz)", - fmt.format.description, - fmt.v4l2_fmt.width, - fmt.v4l2_fmt.height, - fmt.v4l2_fmt.discrete.denominator / fmt.v4l2_fmt.discrete.numerator); - } - set_v4l2_params(); // start the camera