Commit c96d26bf authored by Ken Tossell's avatar Ken Tossell
Browse files

Merge pull request #22 from furushchev/passthrough-if-possible

[libuvc_camera] support multiple video mode
parents 745c3f20 09a21ab2
......@@ -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,
......
......@@ -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,
......
......@@ -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);
......@@ -321,6 +366,8 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_);
uvc_unref_device(dev_);
ROS_ERROR("check video_mode/width/height/frame_rate are available");
uvc_print_diag(devh_, NULL);
return;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment