Add parameter to skip throwing an error on format check

Signed-off-by: Evan Flynn <evanflynn.msu@gmail.com>
ros-drivers · Jan 31, 2024 · d3248c2 · d3248c2
1 parent e2b11ab
commit d3248c2
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 5 deletions.
diff --git a/config/params_1.yaml b/config/params_1.yaml
@@ -4,7 +4,8 @@
       framerate: 30.0
       io_method: "mmap"
       frame_id: "camera"
-      pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
+      pixel_format: "mjpeg2rgb"
+      skip_format_check: false  # not recommended for use, but might be useful in corner cases (e.g. camera firmware bug)
       av_device_format: "YUV422P"
       image_width: 640
       image_height: 480

diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp
@@ -115,6 +115,7 @@ typedef struct
   // to discover them,
   // or guvcview
   std::string pixel_format_name;
+  bool skip_format_check;
   std::string av_device_format;
   int image_width;
   int image_height;
@@ -369,11 +370,15 @@ class UsbCam
       });
 
     // Look for specified pixel format
-    if (!this->set_pixel_format(format_args)) {
+    if (!this->set_pixel_format(format_args) && !parameters.skip_format_check) {
       throw std::invalid_argument(
               "Specified format `" + parameters.pixel_format_name + "` is unsupported by the " +
               "selected device `" + parameters.device_name + "`"
       );
+    } else if (parameters.skip_format_check) {
+      // print out a warning to users that the `skip_format_check` is on in case they didnt mean it
+      std::cout << "Skipping format check because the `skip_format_check` variable is `true`";
+      std::cout << std::endl;
     }
 
     return m_image.pixel_format;

diff --git a/src/ros2/usb_cam_node.cpp b/src/ros2/usb_cam_node.cpp
@@ -70,6 +70,7 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options)
   this->declare_parameter("image_width", 640);
   this->declare_parameter("io_method", "mmap");
   this->declare_parameter("pixel_format", "yuyv");
+  this->declare_parameter("skip_format_check", false);
   this->declare_parameter("av_device_format", "YUV422P");
   this->declare_parameter("video_device", "/dev/video0");
   this->declare_parameter("brightness", 50);  // 0-255, -1 "leave alone"
@@ -222,9 +223,9 @@ void UsbCamNode::get_params()
   auto parameters = parameters_client->get_parameters(
     {
       "camera_name", "camera_info_url", "frame_id", "framerate", "image_height", "image_width",
-      "io_method", "pixel_format", "av_device_format", "video_device", "brightness", "contrast",
-      "saturation", "sharpness", "gain", "auto_white_balance", "white_balance", "autoexposure",
-      "exposure", "autofocus", "focus"
+      "io_method", "pixel_format", "skip_format_check", "av_device_format", "video_device",
+      "brightness", "contrast", "saturation", "sharpness", "gain", "auto_white_balance",
+      "white_balance", "autoexposure", "exposure", "autofocus", "focus"
     }
   );
 
@@ -252,6 +253,8 @@ void UsbCamNode::assign_params(const std::vector<rclcpp::Parameter> & parameters
       m_parameters.io_method_name = parameter.value_to_string();
     } else if (parameter.get_name() == "pixel_format") {
       m_parameters.pixel_format_name = parameter.value_to_string();
+    } else if (parameter.get_name() == "skip_format_check") {
+      m_parameters.skip_format_check = parameter.as_bool();
     } else if (parameter.get_name() == "av_device_format") {
       m_parameters.av_device_format = parameter.value_to_string();
     } else if (parameter.get_name() == "video_device") {