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