00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <image_transport/raw_publisher.h> 00036 #include <ros/static_assert.h> 00037 #include <sensor_msgs/Image.h> 00038 00045 class ImageTransportImage 00046 { 00047 public: 00048 sensor_msgs::Image image_; 00049 const uint8_t* data_; 00050 00054 ImageTransportImage() {} 00055 00059 ImageTransportImage(const sensor_msgs::Image& image, const uint8_t* data) 00060 : image_(image), data_(data) 00061 { 00062 } 00063 }; 00064 00066 namespace ros { 00067 00068 namespace message_traits { 00069 00070 template<> struct MD5Sum<ImageTransportImage> 00071 { 00072 static const char* value() { return MD5Sum<sensor_msgs::Image>::value(); } 00073 static const char* value(const ImageTransportImage&) { return value(); } 00074 00075 static const uint64_t static_value1 = MD5Sum<sensor_msgs::Image>::static_value1; 00076 static const uint64_t static_value2 = MD5Sum<sensor_msgs::Image>::static_value2; 00077 00078 // If the definition of sensor_msgs/Image changes, we'll get a compile error here. 00079 ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value1 == 0x060021388200f6f0ULL); 00080 ROS_STATIC_ASSERT(MD5Sum<sensor_msgs::Image>::static_value2 == 0xf447d0fcd9c64743ULL); 00081 }; 00082 00083 template<> struct DataType<ImageTransportImage> 00084 { 00085 static const char* value() { return DataType<sensor_msgs::Image>::value(); } 00086 static const char* value(const ImageTransportImage&) { return value(); } 00087 }; 00088 00089 template<> struct Definition<ImageTransportImage> 00090 { 00091 static const char* value() { return Definition<sensor_msgs::Image>::value(); } 00092 static const char* value(const ImageTransportImage&) { return value(); } 00093 }; 00094 00095 template<> struct HasHeader<ImageTransportImage> : TrueType {}; 00096 00097 } // namespace ros::message_traits 00098 00099 namespace serialization { 00100 00101 template<> struct Serializer<ImageTransportImage> 00102 { 00104 00105 template<typename Stream> 00106 inline static void write(Stream& stream, const ImageTransportImage& m) 00107 { 00108 stream.next(m.image_.header); 00109 stream.next((uint32_t)m.image_.height); // height 00110 stream.next((uint32_t)m.image_.width); // width 00111 stream.next(m.image_.encoding); 00112 uint8_t is_bigendian = 0; 00113 stream.next(is_bigendian); 00114 stream.next((uint32_t)m.image_.step); 00115 size_t data_size = m.image_.step*m.image_.height; 00116 stream.next((uint32_t)data_size); 00117 if (data_size > 0) 00118 memcpy(stream.advance(data_size), m.data_, data_size); 00119 } 00120 00121 inline static uint32_t serializedLength(const ImageTransportImage& m) 00122 { 00123 size_t data_size = m.image_.step*m.image_.height; 00124 return serializationLength(m.image_.header) + serializationLength(m.image_.encoding) + 17 + data_size; 00125 } 00126 }; 00127 00128 } // namespace ros::serialization 00129 00130 } // namespace ros 00131 00132 00133 namespace image_transport { 00134 00135 void RawPublisher::publish(const sensor_msgs::Image& message, const uint8_t* data) const 00136 { 00137 getPublisher().publish(ImageTransportImage(message, data)); 00138 } 00139 00140 } //namespace image_transport