diff --git a/src/converters/camera.cpp b/src/converters/camera.cpp index 337358d3..b432e9d3 100644 --- a/src/converters/camera.cpp +++ b/src/converters/camera.cpp @@ -28,11 +28,6 @@ */ #include -/* -* CV includes -*/ -#include - /* * BOOST includes */ @@ -126,7 +121,7 @@ CameraConverter::CameraConverter( const std::string& name, const float& frequenc camera_source_(camera_source), resolution_(resolution), // change in case of depth camera - colorspace_( (camera_source_!=AL::kDepthCamera)?AL::kRGBColorSpace:AL::kRawDepthColorSpace ), + colorspace_( (camera_source_!=AL::kDepthCamera)?AL::kYUV422ColorSpace:AL::kRawDepthColorSpace ), msg_colorspace_( (camera_source_!=AL::kDepthCamera)?"rgb8":"16UC1" ), cv_mat_type_( (camera_source_!=AL::kDepthCamera)?CV_8UC3:CV_16U ), camera_info_( camera_info_definitions::getCameraInfo(camera_source, resolution) ) @@ -211,8 +206,15 @@ void CameraConverter::callAll( const std::vector } // Create a cv::Mat of the right dimensions - cv::Mat cv_img(image.height, image.width, cv_mat_type_, image.buffer); - msg_ = cv_bridge::CvImage(std_msgs::Header(), msg_colorspace_, cv_img).toImageMsg(); + if (colorspace_ == AL::kYUV422ColorSpace) + { + cv::Mat cv_YUV(image.height, image.width, CV_8UC2, image.buffer); + cvtColor(cv_YUV, cv_img_, CV_YUV2RGB_YUYV); + } + else + cv_img_= cv::Mat(image.height, image.width, cv_mat_type_,image.buffer); + + msg_ = cv_bridge::CvImage(std_msgs::Header(), msg_colorspace_, cv_img_).toImageMsg(); msg_->header.frame_id = msg_frameid_; msg_->header.stamp = ros::Time::now(); diff --git a/src/converters/camera.hpp b/src/converters/camera.hpp index 88d13c58..74880fd4 100644 --- a/src/converters/camera.hpp +++ b/src/converters/camera.hpp @@ -29,6 +29,11 @@ */ #include +/* +* CV includes +*/ +#include + namespace naoqi { namespace converter @@ -64,6 +69,7 @@ class CameraConverter : public BaseConverter // goes along with colorspace_ std::string msg_colorspace_; int cv_mat_type_; + cv::Mat cv_img_; // msg frame id std::string msg_frameid_; sensor_msgs::CameraInfo camera_info_;