Commit 257ccb75 authored by Furushchev's avatar Furushchev
Browse files

[libuvc_camera] support multiple video_mode

parent 745c3f20
...@@ -34,10 +34,13 @@ gen.add("height", int_t, RECONFIGURE_CLOSE, ...@@ -34,10 +34,13 @@ gen.add("height", int_t, RECONFIGURE_CLOSE,
"Image height.", 480, 0) "Image height.", 480, 0)
video_modes = gen.enum([gen.const("uncompressed", str_t, "uncompressed", "Use any uncompressed format"), 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("yuyv", str_t, "yuyv", "YUYV"),
gen.const("uyvy", str_t, "uyvy", "UYVY"), gen.const("uyvy", str_t, "uyvy", "UYVY"),
gen.const("rgb", str_t, "rgb", "RGB"), 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") "Video stream format")
gen.add("video_mode", str_t, RECONFIGURE_CLOSE, gen.add("video_mode", str_t, RECONFIGURE_CLOSE,
......
...@@ -38,6 +38,7 @@ private: ...@@ -38,6 +38,7 @@ private:
// Accept a reconfigure request from a client // Accept a reconfigure request from a client
void ReconfigureCallback(UVCCameraConfig &config, uint32_t level); void ReconfigureCallback(UVCCameraConfig &config, uint32_t level);
enum uvc_frame_format GetVideoMode(std::string vmode);
// Accept changes in values of automatically updated controls // Accept changes in values of automatically updated controls
void AutoControlsCallback(enum uvc_status_class status_class, void AutoControlsCallback(enum uvc_status_class status_class,
int event, int event,
......
...@@ -165,20 +165,40 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) { ...@@ -165,20 +165,40 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
assert(state_ == kRunning); assert(state_ == kRunning);
assert(rgb_frame_); 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()); sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
image->width = config_.width; image->width = config_.width;
image->height = config_.height; image->height = config_.height;
image->encoding = "rgb8";
image->step = image->width * 3; image->step = image->width * 3;
image->data.resize(image->step * image->height); 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( sensor_msgs::CameraInfo::Ptr cinfo(
new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo())); new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
...@@ -257,6 +277,30 @@ void CameraDriver::AutoControlsCallback( ...@@ -257,6 +277,30 @@ void CameraDriver::AutoControlsCallback(
status_attribute, data, data_len); 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) { void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
assert(state_ == kStopped); assert(state_ == kStopped);
...@@ -280,6 +324,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { ...@@ -280,6 +324,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
} }
uvc_error_t open_err = uvc_open(dev_, &devh_); uvc_error_t open_err = uvc_open(dev_, &devh_);
if (open_err != UVC_SUCCESS) { if (open_err != UVC_SUCCESS) {
switch (open_err) { switch (open_err) {
case UVC_ERROR_ACCESS: case UVC_ERROR_ACCESS:
...@@ -313,7 +358,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { ...@@ -313,7 +358,7 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
uvc_stream_ctrl_t ctrl; uvc_stream_ctrl_t ctrl;
uvc_error_t mode_err = uvc_get_stream_ctrl_format_size( uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
devh_, &ctrl, devh_, &ctrl,
UVC_COLOR_FORMAT_UNCOMPRESSED, GetVideoMode(new_config.video_mode),
new_config.width, new_config.height, new_config.width, new_config.height,
new_config.frame_rate); new_config.frame_rate);
......
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