00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef CV_BRIDGE_CV_BRIDGE_H
00037 #define CV_BRIDGE_CV_BRIDGE_H
00038
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/CompressedImage.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <ros/static_assert.h>
00043 #include <opencv2/core/core.hpp>
00044 #include <opencv2/imgproc/imgproc.hpp>
00045 #include <opencv2/imgproc/types_c.h>
00046 #include <stdexcept>
00047
00048 namespace cv_bridge {
00049
00050 class Exception : public std::runtime_error
00051 {
00052 public:
00053 Exception(const std::string& description) : std::runtime_error(description) {}
00054 };
00055
00056 class CvImage;
00057
00058 typedef boost::shared_ptr<CvImage> CvImagePtr;
00059 typedef boost::shared_ptr<CvImage const> CvImageConstPtr;
00060
00061
00062 typedef enum {
00063 BMP, DIB,
00064 JPG, JPEG, JPE,
00065 JP2,
00066 PNG,
00067 PBM, PGM, PPM,
00068 SR, RAS,
00069 TIFF, TIF,
00070 } Format;
00071
00076 class CvImage
00077 {
00078 public:
00079 std_msgs::Header header;
00080 std::string encoding;
00081 cv::Mat image;
00082
00086 CvImage() {}
00087
00091 CvImage(const std_msgs::Header& header, const std::string& encoding,
00092 const cv::Mat& image = cv::Mat())
00093 : header(header), encoding(encoding), image(image)
00094 {
00095 }
00096
00102 sensor_msgs::ImagePtr toImageMsg() const;
00103
00111 sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format = JPG) const;
00112
00119 void toImageMsg(sensor_msgs::Image& ros_image) const;
00120
00128 void toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, const Format dst_format = JPG) const;
00129
00130
00131 typedef boost::shared_ptr<CvImage> Ptr;
00132 typedef boost::shared_ptr<CvImage const> ConstPtr;
00133
00134 protected:
00135 boost::shared_ptr<void const> tracked_object_;
00136
00138 friend
00139 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
00140 const boost::shared_ptr<void const>& tracked_object,
00141 const std::string& encoding);
00143 };
00144
00145
00162 CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source,
00163 const std::string& encoding = std::string());
00164
00165 CvImagePtr toCvCopy(const sensor_msgs::CompressedImageConstPtr& source,
00166 const std::string& encoding = std::string());
00167
00187 CvImagePtr toCvCopy(const sensor_msgs::Image& source,
00188 const std::string& encoding = std::string());
00189
00190 CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source,
00191 const std::string& encoding = std::string());
00192
00213 CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr& source,
00214 const std::string& encoding = std::string());
00215
00240 CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
00241 const boost::shared_ptr<void const>& tracked_object,
00242 const std::string& encoding = std::string());
00243
00247 CvImagePtr cvtColor(const CvImageConstPtr& source,
00248 const std::string& encoding);
00249
00250 struct CvtColorForDisplayOptions {
00251 CvtColorForDisplayOptions() :
00252 do_dynamic_scaling(false),
00253 min_image_value(0.0),
00254 max_image_value(0.0),
00255 colormap(-1),
00256 bg_label(-1) {}
00257 bool do_dynamic_scaling;
00258 double min_image_value;
00259 double max_image_value;
00260 int colormap;
00261 int bg_label;
00262 };
00263
00264
00296 CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
00297 const std::string& encoding = std::string(),
00298 const CvtColorForDisplayOptions options = CvtColorForDisplayOptions());
00299
00305 int getCvType(const std::string& encoding);
00306
00307 }
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00319 namespace ros {
00320
00321 namespace message_traits {
00322
00323 template<> struct MD5Sum<cv_bridge::CvImage>
00324 {
00325 static const char* value() { return MD5Sum<sensor_msgs::Image>::value(); }
00326 static const char* value(const cv_bridge::CvImage&) { return value(); }
00327
00328 static const uint64_t static_value1 = MD5Sum<sensor_msgs::Image>::static_value1;
00329 static const uint64_t static_value2 = MD5Sum<sensor_msgs::Image>::static_value2;
00330
00331
00332 ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value1 == 0x060021388200f6f0ULL);
00333 ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value2 == 0xf447d0fcd9c64743ULL);
00334 };
00335
00336 template<> struct DataType<cv_bridge::CvImage>
00337 {
00338 static const char* value() { return DataType<sensor_msgs::Image>::value(); }
00339 static const char* value(const cv_bridge::CvImage&) { return value(); }
00340 };
00341
00342 template<> struct Definition<cv_bridge::CvImage>
00343 {
00344 static const char* value() { return Definition<sensor_msgs::Image>::value(); }
00345 static const char* value(const cv_bridge::CvImage&) { return value(); }
00346 };
00347
00348 template<> struct HasHeader<cv_bridge::CvImage> : TrueType {};
00349
00350 }
00351
00352 namespace serialization {
00353
00354 template<> struct Serializer<cv_bridge::CvImage>
00355 {
00357
00358 template<typename Stream>
00359 inline static void write(Stream& stream, const cv_bridge::CvImage& m)
00360 {
00361 stream.next(m.header);
00362 stream.next((uint32_t)m.image.rows);
00363 stream.next((uint32_t)m.image.cols);
00364 stream.next(m.encoding);
00365 uint8_t is_bigendian = 0;
00366 stream.next(is_bigendian);
00367 stream.next((uint32_t)m.image.step);
00368 size_t data_size = m.image.step*m.image.rows;
00369 stream.next((uint32_t)data_size);
00370 if (data_size > 0)
00371 memcpy(stream.advance(data_size), m.image.data, data_size);
00372 }
00373
00374 template<typename Stream>
00375 inline static void read(Stream& stream, cv_bridge::CvImage& m)
00376 {
00377 stream.next(m.header);
00378 uint32_t height, width;
00379 stream.next(height);
00380 stream.next(width);
00381 stream.next(m.encoding);
00382 uint8_t is_bigendian;
00383 stream.next(is_bigendian);
00384 uint32_t step, data_size;
00385 stream.next(step);
00386 stream.next(data_size);
00387 int type = cv_bridge::getCvType(m.encoding);
00388
00389 cv::Mat tmp((int)height, (int)width, type, stream.advance(data_size), (size_t)step);
00390 tmp.copyTo(m.image);
00391 }
00392
00393 inline static uint32_t serializedLength(const cv_bridge::CvImage& m)
00394 {
00395 size_t data_size = m.image.step*m.image.rows;
00396 return serializationLength(m.header) + serializationLength(m.encoding) + 17 + data_size;
00397 }
00398 };
00399
00400 }
00401
00402 namespace message_operations {
00403
00404 template<> struct Printer<cv_bridge::CvImage>
00405 {
00406 template<typename Stream>
00407 static void stream(Stream& s, const std::string& indent, const cv_bridge::CvImage& m)
00408 {
00410 }
00411 };
00412
00413 }
00414
00415 }
00416
00417 namespace cv_bridge {
00418
00419 inline std::ostream& operator<<(std::ostream& s, const CvImage& m)
00420 {
00421 ros::message_operations::Printer<CvImage>::stream(s, "", m);
00422 return s;
00423 }
00424
00425 }
00426
00428
00429 #endif