ros-drivers/usb_cam

[bug] When the parameter `pixel_format`: "uyvy2rgb" is in the configuration file, the No image with `rqt_image_view` software

zymouse opened this issue · 4 comments

branch: ros2
commit: 64c417f

settled

  • pixel_format: "uyvy":
    image_raw is displayed normally
    image_raw/compressed is not displayed correctly.
  • pixel_format: "uyvy2rgb":
    image_raw display exception
    image_raw/compressed is not displayed correctly.
  • Using opencv API, vyvu to rgb:
    image_raw is displayed normally
    image_raw/compressed is displayed normally

Error prompt

ros2 run rqt_image_view rqt_image_view

while trying to convert image from 'rgb8' to 'rgb8' an exception was thrown (Image is wrongly formed: step < width * byte_depth * num_channels or 3840 != 1920 * 1 * 3)

image

config file

/**:
    ros__parameters:
      video_device: "/dev/video0"
      framerate: 30.0
      io_method: "mmap"
      frame_id: "camera1"
      # pixel_format: "yuyv2rgb"  
      pixel_format: "uyvy2rgb"  
      # pixel_format: "uyvy"  
      image_width: 1920
      image_height: 1080
      camera_name: "camera1"
      # reusing same camera intrinsics only for demo, should really be unique for camera2"
      camera_info_url: "package://usb_cam/config/camera_info.yaml"
      brightness: -1
      contrast: -1
      saturation: -1
      sharpness: -1
      gain: -1
      auto_white_balance: true
      white_balance: 4000
      autoexposure: true
      exposure: 100
      autofocus: false
      focus: -1

Description of the problem

pixel_format: "uyvy" display normal
pixel_format: "uyvy2rgb" displays incorrectly

My modifications

I chose pixel_format: "uyvy", and then converted m_image_msg to rgb format, and that worked!

void convertUYVYtoRGB(sensor_msgs::msg::Image::SharedPtr m_image_msg) {

  int width = m_image_msg->width;
  int height = m_image_msg->height;
  cv::Mat uyvy_img(height, width, CV_8UC2, const_cast<uint8_t *>(m_image_msg->data.data()));
  cv::Mat rgb_img;
  cv::cvtColor(uyvy_img, rgb_img, cv::COLOR_YUV2RGB_UYVY);

  m_image_msg->encoding = "rgb8";
  m_image_msg->step = rgb_img.step;
  m_image_msg->data.assign(rgb_img.data, rgb_img.data + rgb_img.total() * rgb_img.elemSize());
}

Converted step using opencv.

[usb_cam_node_exe-1] Before conversion: 
[usb_cam_node_exe-1] Step: 3840
[usb_cam_node_exe-1] After conversion: 
[usb_cam_node_exe-1] Step: 5760

The image size changes after conversion using opencv, but the image size does not change after conversion according to the following formula.
image

This results in step not matching the formula step = width * byte_depth * num_channels.

@zymouse the logic was sound, just the base config value for number_of_channels was incorrect (2 vs 3) I think because of a copy & paste error.

Could you try again with #292 and let me know if that fixes your issue?

@flynneva
Thank you very much, problem solved!
image
#292