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 <[email protected]>
ros-drivers · Dec 1, 2023 · d9d12d4 · d9d12d4
1 parent e4afe62
commit d9d12d4
Show file tree

Hide file tree

Showing 4 changed files with 97 additions and 57 deletions.
diff --git 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
@@ -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
@@ -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<std::shared_ptr<pixel_format_base>> driver_supported_formats(
+  const formats::format_arguments_t & args = formats::format_arguments_t())
+{
+  std::vector<std::shared_ptr<pixel_format_base>> fmts = {
+    std::make_shared<RGB8>(args),
+    std::make_shared<YUYV>(args),
+    std::make_shared<YUYV2RGB>(args),
+    std::make_shared<UYVY>(args),
+    std::make_shared<UYVY2RGB>(args),
+    std::make_shared<MONO8>(args),
+    std::make_shared<MONO16>(args),
+    std::make_shared<Y102MONO8>(args),
+    std::make_shared<RAW_MJPEG>(args),
+    std::make_shared<MJPEG2RGB>(args),
+    std::make_shared<M4202RGB>(args),
+  };
+  return fmts;
+}
 
 typedef struct
 {
@@ -273,57 +307,73 @@ 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<pixel_format_base> 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.)
   /// @param parameters list of parameters from which the pixel format is to be set
   /// @return pixel format structure corresponding to a given name
   inline std::shared_ptr<pixel_format_base> 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<RGB8>();
-    } else if (parameters.pixel_format_name == "yuyv") {
-      m_image.pixel_format = std::make_shared<YUYV>();
-    } else if (parameters.pixel_format_name == "yuyv2rgb") {
-      // number of pixels required for conversion method
-      m_image.pixel_format = std::make_shared<YUYV2RGB>(m_image.number_of_pixels);
-    } else if (parameters.pixel_format_name == "uyvy") {
-      m_image.pixel_format = std::make_shared<UYVY>();
-    } else if (parameters.pixel_format_name == "uyvy2rgb") {
-      // number of pixels required for conversion method
-      m_image.pixel_format = std::make_shared<UYVY2RGB>(m_image.number_of_pixels);
-    } else if (parameters.pixel_format_name == "mjpeg") {
-      m_image.pixel_format = std::make_shared<RAW_MJPEG>(
-        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<MJPEG2RGB>(
-        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<M4202RGB>(
-        m_image.width, m_image.height);
-    } else if (parameters.pixel_format_name == "mono8") {
-      m_image.pixel_format = std::make_shared<MONO8>();
-    } else if (parameters.pixel_format_name == "mono16") {
-      m_image.pixel_format = std::make_shared<MONO16>();
-    } else if (parameters.pixel_format_name == "y102mono8") {
-      m_image.pixel_format = std::make_shared<Y102MONO8>(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
@@ -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