Request to Add Support for YUV Format in visp_bridge

[m11112089]

Hello,

I encountered an issue when using usb_cam to launch my Logitech camera(C9xx) and converting the ROS image to ViSP format with visp_bridge. The converted image appears distorted.

This limitation is similar to the issue discussed in ros2/common_interfaces #76.

Upon reviewing the visp_bridge source code, I noticed it does not support the YUV format.

Could you consider adding support for this format in visp_bridge? I suggest incorporating the YUV422_YUY2 format specifically. This addition would align visp_bridge with the capabilities of sensor_msgs, the vpImageConvert Class, and usb_cam, all of which support the YUV422_YUY2 format.

Would it be acceptable for me to create a Pull Request to introduce this feature, or would you prefer to implement it differently?
Below is a proposed implementation snippet for handling YUV422_YUY2 format images in the toVispImageRGBa function:

vpImage<vpRGBa> toVispImageRGBa(const sensor_msgs::msg::Image &src)
{
  using sensor_msgs::image_encodings::BGR8;
  using sensor_msgs::image_encodings::BGRA8;
  using sensor_msgs::image_encodings::MONO8;
  using sensor_msgs::image_encodings::RGB8;
  using sensor_msgs::image_encodings::RGBA8;
  using sensor_msgs::image_encodings::YUV422_YUY2;

  vpImage<vpRGBa> dst(src.height, src.width);

  if (src.encoding == sensor_msgs::image_encodings::MONO8)
    for (unsigned i = 0; i < dst.getWidth(); ++i) {
      for (unsigned j = 0; j < dst.getHeight(); ++j) {

        dst[j][i] = vpRGBa(src.data[j * src.step + i], src.data[j * src.step + i], src.data[j * src.step + i]);
      }
    }
  else if (src.encoding == sensor_msgs::image_encodings::YUV422_YUY2) {
    int index = 0;
    unsigned char Y0, U, Y1, V;
    for (unsigned i = 0; i < src.width; i += 2) {
      for (unsigned j = 0; j < src.height; j++) {
        index = j * src.step + i * 2;
        Y0 = src.data[index];
        U = src.data[index + 1];
        Y1 = src.data[index + 2];
        V = src.data[index + 3];

        auto convertYUVtoRGB = [](unsigned char Y, unsigned char U, unsigned char V) -> vpRGBa {
          int C = Y - 16;
          int D = U - 128;
          int E = V - 128;
          unsigned char R = std::min(std::max((298 * C + 409 * E + 128) >> 8, 0), 255);
          unsigned char G = std::min(std::max((298 * C - 100 * D - 208 * E + 128) >> 8, 0), 255);
          unsigned char B = std::min(std::max((298 * C + 516 * D + 128) >> 8, 0), 255);
          return vpRGBa(R, G, B);
        };

        dst[j][i] = convertYUVtoRGB(Y0, U, V);
        if (i + 1 < src.width) { // Make sure not to exceed the image width
          dst[j][i + 1] = convertYUVtoRGB(Y1, U, V);
        }
      }
    }
  } else {
    unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);

    for (unsigned i = 0; i < dst.getWidth(); ++i) {
      for (unsigned j = 0; j < dst.getHeight(); ++j) {
        dst[j][i] = vpRGBa(src.data[j * src.step + i * nc + 0], src.data[j * src.step + i * nc + 1],
                           src.data[j * src.step + i * nc + 2]);
      }
    }
  }
  return dst;
}