/** @brief Convert a frame to BGR
* @ingroup frame
*
* @param in non-BGR frame
* @param out BGR frame
*/
uvc_error_t uvc_any2bgr(uvc_frame_t *in, uvc_frame_t *out) {
switch (in->color_format) {
case UVC_COLOR_FORMAT_YUYV:
return uvc_yuyv2bgr(in, out);
case UVC_COLOR_FORMAT_UYVY:
return uvc_uyvy2bgr(in, out);
case UVC_COLOR_FORMAT_BGR:
return uvc_duplicate_frame(in, out);
default:
return UVC_ERROR_NOT_SUPPORTED;
}
}
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;
}
}