/* This callback function runs once per frame. Use it to perform any
* quick processing you need, or have it put the frame into your application's
* input queue. If this function takes too long, you'll start losing frames. */
void cb_rgb(uvc_frame_t *frame, void *ptr) {
uvc_frame_t *bgr;
uvc_error_t ret;
/* We'll convert the image from YUV/JPEG to BGR, so allocate space */
bgr = uvc_allocate_frame(frame->width * frame->height * 3);
if (!bgr) {
printf("unable to allocate bgr frame!");
return;
}
/* Do the BGR conversion */
ret = uvc_any2bgr(frame, bgr);
if (ret) {
uvc_perror(ret, "uvc_any2bgr");
uvc_free_frame(bgr);
return;
}
/* Call a user function:
*
* my_type *my_obj = (*my_type) ptr;
* my_user_function(ptr, bgr);
* my_other_function(ptr, bgr->data, bgr->width, bgr->height);
*/
/* Call a C++ method:
*
* my_type *my_obj = (*my_type) ptr;
* my_obj->my_func(bgr);
*/
/* Use opencv.highgui to display the image: */
IplImage* cvImg;
cvImg = cvCreateImageHeader(
cvSize(bgr->width, bgr->height),
IPL_DEPTH_8U,
3);
cvSetData(cvImg, bgr->data, bgr->width * 3);
cvNamedWindow("Test", CV_WINDOW_AUTOSIZE);
cvShowImage("Test", cvImg);
cvWaitKey(10);
cvReleaseImageHeader(&cvImg);
uvc_free_frame(bgr);
}
void CameraDriver::ImageCallback(uvc_frame_t *frame) {
ros::Time timestamp = ros::Time::now();
boost::recursive_mutex::scoped_lock(mutex_);
assert(state_ == kRunning);
assert(rgb_frame_);
sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
image->width = config_.width;
image->height = config_.height;
image->step = image->width * 3;
image->data.resize(image->step * image->height);
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);
#if libuvc_VERSION > 00005 /* version > 0.0.5 */
} else if (frame->frame_format == UVC_FRAME_FORMAT_MJPEG) {
// Enable mjpeg support despite uvs_any2bgr shortcoming
// https://github.com/ros-drivers/libuvc_ros/commit/7508a09f
uvc_error_t conv_ret = uvc_mjpeg2rgb(frame, rgb_frame_);
if (conv_ret != UVC_SUCCESS) {
uvc_perror(conv_ret, "Couldn't convert frame to RGB");
return;
}
image->encoding = "rgb8";
memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
#endif
} 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()));
image->header.frame_id = config_.frame_id;
image->header.stamp = timestamp;
cinfo->header.frame_id = config_.frame_id;
cinfo->header.stamp = timestamp;
cam_pub_.publish(image, cinfo);
if (config_changed_) {
config_server_.updateConfig(config_);
config_changed_ = false;
}
}
/* Tests a subset of the USBDEVFS ioctls. Requires libuvc to build:
https://github.com/ktossell/libuvc
Therefore this test is not built/run by default. */
#include "util.h"
#include "libuvc/libuvc.h"
static int frame_count = 0;
void cb(uvc_frame_t* frame, __attribute__((unused)) void* ptr) {
uvc_frame_t* bgr;
uvc_error_t ret;
bgr = uvc_allocate_frame(frame->width * frame->height * 3);
test_assert(bgr != NULL);
ret = uvc_any2bgr(frame, bgr);
test_assert(ret == 0);
++frame_count;
uvc_free_frame(bgr);
}
int main(void) {
uvc_context_t* ctx;
uvc_device_t* dev;
uvc_device_handle_t* devh;
uvc_stream_ctrl_t ctrl;
uvc_error_t res;
res = uvc_init(&ctx, NULL);
test_assert(res >= 0);
atomic_puts("UVC initialized");
/* Locates the first attached UVC device, stores in dev */
