From 257ccb75929fbedbba57675d3df3658c9b00f0cb Mon Sep 17 00:00:00 2001 From: Furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp> Date: Tue, 15 Sep 2015 15:38:51 +0900 Subject: [PATCH] [libuvc_camera] support multiple video_mode --- libuvc_camera/cfg/UVCCamera.cfg | 5 +- .../include/libuvc_camera/camera_driver.h | 1 + libuvc_camera/src/camera_driver.cpp | 65 ++++++++++++++++--- 3 files changed, 60 insertions(+), 11 deletions(-) diff --git a/libuvc_camera/cfg/UVCCamera.cfg b/libuvc_camera/cfg/UVCCamera.cfg index 7b16c45..7c73e90 100755 --- a/libuvc_camera/cfg/UVCCamera.cfg +++ b/libuvc_camera/cfg/UVCCamera.cfg @@ -34,10 +34,13 @@ gen.add("height", int_t, RECONFIGURE_CLOSE, "Image height.", 480, 0) video_modes = gen.enum([gen.const("uncompressed", str_t, "uncompressed", "Use any uncompressed format"), + gen.const("compressed", str_t, "compressed", "User any compressed format"), gen.const("yuyv", str_t, "yuyv", "YUYV"), gen.const("uyvy", str_t, "uyvy", "UYVY"), gen.const("rgb", str_t, "rgb", "RGB"), - gen.const("jpeg", str_t, "jpeg", "JPEG/MJPEG")], + gen.const("bgr", str_t, "bgr", "BGR"), + gen.const("mjpeg", str_t, "mjpeg", "MJPEG"), + gen.const("gray8", str_t, "gray8", "gray8")], "Video stream format") gen.add("video_mode", str_t, RECONFIGURE_CLOSE, diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index 9be85c7..db825fd 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -38,6 +38,7 @@ private: // Accept a reconfigure request from a client void ReconfigureCallback(UVCCameraConfig &config, uint32_t level); + enum uvc_frame_format GetVideoMode(std::string vmode); // Accept changes in values of automatically updated controls void AutoControlsCallback(enum uvc_status_class status_class, int event, diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 329e490..f7aa6f6 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -165,20 +165,40 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) { assert(state_ == kRunning); assert(rgb_frame_); - uvc_error_t conv_ret = uvc_any2rgb(frame, rgb_frame_); - - if (conv_ret != UVC_SUCCESS) { - uvc_perror(conv_ret, "Couldn't convert frame to RGB"); - return; - } - sensor_msgs::Image::Ptr image(new sensor_msgs::Image()); image->width = config_.width; image->height = config_.height; - image->encoding = "rgb8"; image->step = image->width * 3; image->data.resize(image->step * image->height); - memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); + + if (frame->frame_format == UVC_FRAME_FORMAT_BGR){ + image->encoding = "bgr8"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_RGB){ + image->encoding = "rgb8"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_UYVY) { + image->encoding = "yuv422"; + memcpy(&(image->data[0]), frame->data, frame->data_bytes); + } else if (frame->frame_format == UVC_FRAME_FORMAT_YUYV) { + // FIXME: uvc_any2bgr does not work on "yuyv" format, so use uvc_yuyv2bgr directly. + uvc_error_t conv_ret = uvc_yuyv2bgr(frame, rgb_frame_); + if (conv_ret != UVC_SUCCESS) { + uvc_perror(conv_ret, "Couldn't convert frame to RGB"); + return; + } + image->encoding = "bgr8"; + memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); + } else { + uvc_error_t conv_ret = uvc_any2bgr(frame, rgb_frame_); + if (conv_ret != UVC_SUCCESS) { + uvc_perror(conv_ret, "Couldn't convert frame to RGB"); + return; + } + image->encoding = "bgr8"; + memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes); + } + sensor_msgs::CameraInfo::Ptr cinfo( new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo())); @@ -257,6 +277,30 @@ void CameraDriver::AutoControlsCallback( status_attribute, data, data_len); } +enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){ + if(vmode == "uncompressed") { + return UVC_COLOR_FORMAT_UNCOMPRESSED; + } else if (vmode == "compressed") { + return UVC_COLOR_FORMAT_COMPRESSED; + } else if (vmode == "yuyv") { + return UVC_COLOR_FORMAT_YUYV; + } else if (vmode == "uyvy") { + return UVC_COLOR_FORMAT_UYVY; + } else if (vmode == "rgb") { + return UVC_COLOR_FORMAT_RGB; + } else if (vmode == "bgr") { + return UVC_COLOR_FORMAT_BGR; + } else if (vmode == "mjpeg") { + return UVC_COLOR_FORMAT_MJPEG; + } else if (vmode == "gray8") { + return UVC_COLOR_FORMAT_GRAY8; + } else { + ROS_ERROR_STREAM("Invalid Video Mode: " << vmode); + ROS_WARN_STREAM("Continue using video mode: uncompressed"); + return UVC_COLOR_FORMAT_UNCOMPRESSED; + } +}; + void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { assert(state_ == kStopped); @@ -280,6 +324,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { } uvc_error_t open_err = uvc_open(dev_, &devh_); + if (open_err != UVC_SUCCESS) { switch (open_err) { case UVC_ERROR_ACCESS: @@ -313,7 +358,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { uvc_stream_ctrl_t ctrl; uvc_error_t mode_err = uvc_get_stream_ctrl_format_size( devh_, &ctrl, - UVC_COLOR_FORMAT_UNCOMPRESSED, + GetVideoMode(new_config.video_mode), new_config.width, new_config.height, new_config.frame_rate); -- GitLab