roscpp_serialization: roscpp_serialization: serialization.h Source File

00001 /*
00002  * Copyright (C) 2009, Willow Garage, Inc.
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the names of Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #ifndef ROSCPP_SERIALIZATION_H
00029 #define ROSCPP_SERIALIZATION_H
00030 
00031 #include "roscpp_serialization_macros.h"
00032 
00033 #include <ros/types.h>
00034 #include <ros/time.h>
00035 
00036 #include "serialized_message.h"
00037 #include "ros/message_traits.h"
00038 #include "ros/builtin_message_traits.h"
00039 #include "ros/exception.h"
00040 
00041 #include <vector>
00042 
00043 #include <boost/array.hpp>
00044 #include <boost/call_traits.hpp>
00045 #include <boost/utility/enable_if.hpp>
00046 #include <boost/mpl/and.hpp>
00047 #include <boost/mpl/or.hpp>
00048 #include <boost/mpl/not.hpp>
00049 
00050 #include <cstring>
00051 
00052 #define ROS_NEW_SERIALIZATION_API 1
00053 
00071 #define ROS_DECLARE_ALLINONE_SERIALIZER \
00072   template<typename Stream, typename T> \
00073   inline static void write(Stream& stream, const T& t) \
00074   { \
00075     allInOne<Stream, const T&>(stream, t); \
00076   } \
00077   \
00078   template<typename Stream, typename T> \
00079   inline static void read(Stream& stream, T& t) \
00080   { \
00081     allInOne<Stream, T&>(stream, t); \
00082   } \
00083   \
00084   template<typename T> \
00085   inline static uint32_t serializedLength(const T& t) \
00086   { \
00087     LStream stream; \
00088     allInOne<LStream, const T&>(stream, t); \
00089     return stream.getLength(); \
00090   }
00091 
00092 namespace ros
00093 {
00094 namespace serialization
00095 {
00096 namespace mt = message_traits;
00097 namespace mpl = boost::mpl;
00098 
00099 class ROSCPP_SERIALIZATION_DECL StreamOverrunException : public ros::Exception
00100 {
00101 public:
00102   StreamOverrunException(const std::string& what)
00103   : Exception(what)
00104   {}
00105 };
00106 
00107 ROSCPP_SERIALIZATION_DECL void throwStreamOverrun();
00108 
00116 template<typename T>
00117 struct Serializer
00118 {
00122   template<typename Stream>
00123   inline static void write(Stream& stream, typename boost::call_traits<T>::param_type t)
00124   {
00125     t.serialize(stream.getData(), 0);
00126   }
00127 
00131   template<typename Stream>
00132   inline static void read(Stream& stream, typename boost::call_traits<T>::reference t)
00133   {
00134     t.deserialize(stream.getData());
00135   }
00136 
00140   inline static uint32_t serializedLength(typename boost::call_traits<T>::param_type t)
00141   {
00142     return t.serializationLength();
00143   }
00144 };
00145 
00149 template<typename T, typename Stream>
00150 inline void serialize(Stream& stream, const T& t)
00151 {
00152   Serializer<T>::write(stream, t);
00153 }
00154 
00158 template<typename T, typename Stream>
00159 inline void deserialize(Stream& stream, T& t)
00160 {
00161   Serializer<T>::read(stream, t);
00162 }
00163 
00167 template<typename T>
00168 inline uint32_t serializationLength(const T& t)
00169 {
00170   return Serializer<T>::serializedLength(t);
00171 }
00172 
00173 #define ROS_CREATE_SIMPLE_SERIALIZER(Type) \
00174   template<> struct Serializer<Type> \
00175   { \
00176     template<typename Stream> inline static void write(Stream& stream, const Type v) \
00177     { \
00178       *reinterpret_cast<Type*>(stream.advance(sizeof(v))) = v; \
00179     } \
00180     \
00181     template<typename Stream> inline static void read(Stream& stream, Type& v) \
00182     { \
00183       v = *reinterpret_cast<Type*>(stream.advance(sizeof(v))); \
00184     } \
00185     \
00186     inline static uint32_t serializedLength(const Type&) \
00187     { \
00188       return sizeof(Type); \
00189     } \
00190   };
00191 
00192 ROS_CREATE_SIMPLE_SERIALIZER(uint8_t);
00193 ROS_CREATE_SIMPLE_SERIALIZER(int8_t);
00194 ROS_CREATE_SIMPLE_SERIALIZER(uint16_t);
00195 ROS_CREATE_SIMPLE_SERIALIZER(int16_t);
00196 ROS_CREATE_SIMPLE_SERIALIZER(uint32_t);
00197 ROS_CREATE_SIMPLE_SERIALIZER(int32_t);
00198 ROS_CREATE_SIMPLE_SERIALIZER(uint64_t);
00199 ROS_CREATE_SIMPLE_SERIALIZER(int64_t);
00200 ROS_CREATE_SIMPLE_SERIALIZER(float);
00201 ROS_CREATE_SIMPLE_SERIALIZER(double);
00202 
00206 template<> struct Serializer<bool>
00207 {
00208   template<typename Stream> inline static void write(Stream& stream, const bool v)
00209   {
00210     uint8_t b = (uint8_t)v;
00211     *reinterpret_cast<uint8_t*>(stream.advance(1)) = b;
00212   }
00213 
00214   template<typename Stream> inline static void read(Stream& stream, bool& v)
00215   {
00216     uint8_t b = *reinterpret_cast<uint8_t*>(stream.advance(1));
00217     v = (bool)b;
00218   }
00219 
00220   inline static uint32_t serializedLength(bool)
00221   {
00222     return 1;
00223   }
00224 };
00225 
00229 template<class ContainerAllocator>
00230 struct Serializer<std::basic_string<char, std::char_traits<char>, ContainerAllocator> >
00231 {
00232   typedef std::basic_string<char, std::char_traits<char>, ContainerAllocator> StringType;
00233 
00234   template<typename Stream>
00235   inline static void write(Stream& stream, const StringType& str)
00236   {
00237     size_t len = str.size();
00238     stream.next((uint32_t)len);
00239 
00240     if (len > 0)
00241     {
00242       memcpy(stream.advance((uint32_t)len), str.data(), len);
00243     }
00244   }
00245 
00246   template<typename Stream>
00247   inline static void read(Stream& stream, StringType& str)
00248   {
00249     uint32_t len;
00250     stream.next(len);
00251     if (len > 0)
00252     {
00253       str = StringType((char*)stream.advance(len), len);
00254     }
00255     else
00256     {
00257       str.clear();
00258     }
00259   }
00260 
00261   inline static uint32_t serializedLength(const StringType& str)
00262   {
00263     return 4 + (uint32_t)str.size();
00264   }
00265 };
00266 
00270 template<>
00271 struct Serializer<ros::Time>
00272 {
00273   template<typename Stream>
00274   inline static void write(Stream& stream, const ros::Time& v)
00275   {
00276     stream.next(v.sec);
00277     stream.next(v.nsec);
00278   }
00279 
00280   template<typename Stream>
00281   inline static void read(Stream& stream, ros::Time& v)
00282   {
00283     stream.next(v.sec);
00284     stream.next(v.nsec);
00285   }
00286 
00287   inline static uint32_t serializedLength(const ros::Time&)
00288   {
00289     return 8;
00290   }
00291 };
00292 
00296 template<>
00297 struct Serializer<ros::Duration>
00298 {
00299   template<typename Stream>
00300   inline static void write(Stream& stream, const ros::Duration& v)
00301   {
00302     stream.next(v.sec);
00303     stream.next(v.nsec);
00304   }
00305 
00306   template<typename Stream>
00307   inline static void read(Stream& stream, ros::Duration& v)
00308   {
00309     stream.next(v.sec);
00310     stream.next(v.nsec);
00311   }
00312 
00313   inline static uint32_t serializedLength(const ros::Duration&)
00314   {
00315     return 8;
00316   }
00317 };
00318 
00322 template<typename T, class ContainerAllocator, class Enabled = void>
00323 struct VectorSerializer
00324 {};
00325 
00329 template<typename T, class ContainerAllocator>
00330 struct VectorSerializer<T, ContainerAllocator, typename boost::disable_if<mt::IsFixedSize<T> >::type >
00331 {
00332   typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00333   typedef typename VecType::iterator IteratorType;
00334   typedef typename VecType::const_iterator ConstIteratorType;
00335 
00336   template<typename Stream>
00337   inline static void write(Stream& stream, const VecType& v)
00338   {
00339     stream.next((uint32_t)v.size());
00340     ConstIteratorType it = v.begin();
00341     ConstIteratorType end = v.end();
00342     for (; it != end; ++it)
00343     {
00344       stream.next(*it);
00345     }
00346   }
00347 
00348   template<typename Stream>
00349   inline static void read(Stream& stream, VecType& v)
00350   {
00351     uint32_t len;
00352     stream.next(len);
00353     v.resize(len);
00354     IteratorType it = v.begin();
00355     IteratorType end = v.end();
00356     for (; it != end; ++it)
00357     {
00358       stream.next(*it);
00359     }
00360   }
00361 
00362   inline static uint32_t serializedLength(const VecType& v)
00363   {
00364     uint32_t size = 4;
00365     ConstIteratorType it = v.begin();
00366     ConstIteratorType end = v.end();
00367     for (; it != end; ++it)
00368     {
00369       size += serializationLength(*it);
00370     }
00371 
00372     return size;
00373   }
00374 };
00375 
00379 template<typename T, class ContainerAllocator>
00380 struct VectorSerializer<T, ContainerAllocator, typename boost::enable_if<mt::IsSimple<T> >::type >
00381 {
00382   typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00383   typedef typename VecType::iterator IteratorType;
00384   typedef typename VecType::const_iterator ConstIteratorType;
00385 
00386   template<typename Stream>
00387   inline static void write(Stream& stream, const VecType& v)
00388   {
00389     uint32_t len = (uint32_t)v.size();
00390     stream.next(len);
00391     if (!v.empty())
00392     {
00393       const uint32_t data_len = len * (uint32_t)sizeof(T);
00394       memcpy(stream.advance(data_len), &v.front(), data_len);
00395     }
00396   }
00397 
00398   template<typename Stream>
00399   inline static void read(Stream& stream, VecType& v)
00400   {
00401     uint32_t len;
00402     stream.next(len);
00403     v.resize(len);
00404 
00405     if (len > 0)
00406     {
00407       const uint32_t data_len = (uint32_t)sizeof(T) * len;
00408       memcpy(&v.front(), stream.advance(data_len), data_len);
00409     }
00410   }
00411 
00412   inline static uint32_t serializedLength(const VecType& v)
00413   {
00414     return 4 + v.size() * (uint32_t)sizeof(T);
00415   }
00416 };
00417 
00421 template<typename T, class ContainerAllocator>
00422 struct VectorSerializer<T, ContainerAllocator, typename boost::enable_if<mpl::and_<mt::IsFixedSize<T>, mpl::not_<mt::IsSimple<T> > > >::type >
00423 {
00424   typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00425   typedef typename VecType::iterator IteratorType;
00426   typedef typename VecType::const_iterator ConstIteratorType;
00427 
00428   template<typename Stream>
00429   inline static void write(Stream& stream, const VecType& v)
00430   {
00431     stream.next((uint32_t)v.size());
00432     ConstIteratorType it = v.begin();
00433     ConstIteratorType end = v.end();
00434     for (; it != end; ++it)
00435     {
00436       stream.next(*it);
00437     }
00438   }
00439 
00440   template<typename Stream>
00441   inline static void read(Stream& stream, VecType& v)
00442   {
00443     uint32_t len;
00444     stream.next(len);
00445     v.resize(len);
00446     IteratorType it = v.begin();
00447     IteratorType end = v.end();
00448     for (; it != end; ++it)
00449     {
00450       stream.next(*it);
00451     }
00452   }
00453 
00454   inline static uint32_t serializedLength(const VecType& v)
00455   {
00456     uint32_t size = 4;
00457     if (!v.empty())
00458     {
00459       uint32_t len_each = serializationLength(v.front());
00460       size += len_each * (uint32_t)v.size();
00461     }
00462 
00463     return size;
00464   }
00465 };
00466 
00470 template<typename T, class ContainerAllocator, typename Stream>
00471 inline void serialize(Stream& stream, const std::vector<T, ContainerAllocator>& t)
00472 {
00473   VectorSerializer<T, ContainerAllocator>::write(stream, t);
00474 }
00475 
00479 template<typename T, class ContainerAllocator, typename Stream>
00480 inline void deserialize(Stream& stream, std::vector<T, ContainerAllocator>& t)
00481 {
00482   VectorSerializer<T, ContainerAllocator>::read(stream, t);
00483 }
00484 
00488 template<typename T, class ContainerAllocator>
00489 inline uint32_t serializationLength(const std::vector<T, ContainerAllocator>& t)
00490 {
00491   return VectorSerializer<T, ContainerAllocator>::serializedLength(t);
00492 }
00493 
00497 template<typename T, size_t N, class Enabled = void>
00498 struct ArraySerializer
00499 {};
00500 
00504 template<typename T, size_t N>
00505 struct ArraySerializer<T, N, typename boost::disable_if<mt::IsFixedSize<T> >::type>
00506 {
00507   typedef boost::array<T, N > ArrayType;
00508   typedef typename ArrayType::iterator IteratorType;
00509   typedef typename ArrayType::const_iterator ConstIteratorType;
00510 
00511   template<typename Stream>
00512   inline static void write(Stream& stream, const ArrayType& v)
00513   {
00514     ConstIteratorType it = v.begin();
00515     ConstIteratorType end = v.end();
00516     for (; it != end; ++it)
00517     {
00518       stream.next(*it);
00519     }
00520   }
00521 
00522   template<typename Stream>
00523   inline static void read(Stream& stream, ArrayType& v)
00524   {
00525     IteratorType it = v.begin();
00526     IteratorType end = v.end();
00527     for (; it != end; ++it)
00528     {
00529       stream.next(*it);
00530     }
00531   }
00532 
00533   inline static uint32_t serializedLength(const ArrayType& v)
00534   {
00535     uint32_t size = 0;
00536     ConstIteratorType it = v.begin();
00537     ConstIteratorType end = v.end();
00538     for (; it != end; ++it)
00539     {
00540       size += serializationLength(*it);
00541     }
00542 
00543     return size;
00544   }
00545 };
00546 
00550 template<typename T, size_t N>
00551 struct ArraySerializer<T, N, typename boost::enable_if<mt::IsSimple<T> >::type>
00552 {
00553   typedef boost::array<T, N > ArrayType;
00554   typedef typename ArrayType::iterator IteratorType;
00555   typedef typename ArrayType::const_iterator ConstIteratorType;
00556 
00557   template<typename Stream>
00558   inline static void write(Stream& stream, const ArrayType& v)
00559   {
00560     const uint32_t data_len = N * sizeof(T);
00561     memcpy(stream.advance(data_len), &v.front(), data_len);
00562   }
00563 
00564   template<typename Stream>
00565   inline static void read(Stream& stream, ArrayType& v)
00566   {
00567     const uint32_t data_len = N * sizeof(T);
00568     memcpy(&v.front(), stream.advance(data_len), data_len);
00569   }
00570 
00571   inline static uint32_t serializedLength(const ArrayType& v)
00572   {
00573     return N * sizeof(T);
00574   }
00575 };
00576 
00580 template<typename T, size_t N>
00581 struct ArraySerializer<T, N, typename boost::enable_if<mpl::and_<mt::IsFixedSize<T>, mpl::not_<mt::IsSimple<T> > > >::type>
00582 {
00583   typedef boost::array<T, N > ArrayType;
00584   typedef typename ArrayType::iterator IteratorType;
00585   typedef typename ArrayType::const_iterator ConstIteratorType;
00586 
00587   template<typename Stream>
00588   inline static void write(Stream& stream, const ArrayType& v)
00589   {
00590     ConstIteratorType it = v.begin();
00591     ConstIteratorType end = v.end();
00592     for (; it != end; ++it)
00593     {
00594       stream.next(*it);
00595     }
00596   }
00597 
00598   template<typename Stream>
00599   inline static void read(Stream& stream, ArrayType& v)
00600   {
00601     IteratorType it = v.begin();
00602     IteratorType end = v.end();
00603     for (; it != end; ++it)
00604     {
00605       stream.next(*it);
00606     }
00607   }
00608 
00609   inline static uint32_t serializedLength(const ArrayType& v)
00610   {
00611     return serializationLength(v.front()) * N;
00612   }
00613 };
00614 
00618 template<typename T, size_t N, typename Stream>
00619 inline void serialize(Stream& stream, const boost::array<T, N>& t)
00620 {
00621   ArraySerializer<T, N>::write(stream, t);
00622 }
00623 
00627 template<typename T, size_t N, typename Stream>
00628 inline void deserialize(Stream& stream, boost::array<T, N>& t)
00629 {
00630   ArraySerializer<T, N>::read(stream, t);
00631 }
00632 
00636 template<typename T, size_t N>
00637 inline uint32_t serializationLength(const boost::array<T, N>& t)
00638 {
00639   return ArraySerializer<T, N>::serializedLength(t);
00640 }
00641 
00645 namespace stream_types
00646 {
00647 enum StreamType
00648 {
00649   Input,
00650   Output,
00651   Length
00652 };
00653 }
00654 typedef stream_types::StreamType StreamType;
00655 
00659 struct ROSCPP_SERIALIZATION_DECL Stream
00660 {
00661   /*
00662    * \brief Returns a pointer to the current position of the stream
00663    */
00664   inline uint8_t* getData() { return data_; }
00670   ROS_FORCE_INLINE uint8_t* advance(uint32_t len)
00671   {
00672     uint8_t* old_data = data_;
00673     data_ += len;
00674     if (data_ > end_)
00675     {
00676       // Throwing directly here causes a significant speed hit due to the extra code generated
00677       // for the throw statement
00678       throwStreamOverrun();
00679     }
00680     return old_data;
00681   }
00682 
00686   inline uint32_t getLength() { return (uint32_t)(end_ - data_); }
00687 
00688 protected:
00689   Stream(uint8_t* _data, uint32_t _count)
00690   : data_(_data)
00691   , end_(_data + _count)
00692   {}
00693 
00694 private:
00695   uint8_t* data_;
00696   uint8_t* end_;
00697 };
00698 
00702 struct ROSCPP_SERIALIZATION_DECL IStream : public Stream
00703 {
00704   static const StreamType stream_type = stream_types::Input;
00705 
00706   IStream(uint8_t* data, uint32_t count)
00707   : Stream(data, count)
00708   {}
00709 
00713   template<typename T>
00714   ROS_FORCE_INLINE void next(T& t)
00715   {
00716     deserialize(*this, t);
00717   }
00718 
00719   template<typename T>
00720   ROS_FORCE_INLINE IStream& operator>>(T& t)
00721   {
00722     deserialize(*this, t);
00723     return *this;
00724   }
00725 };
00726 
00730 struct ROSCPP_SERIALIZATION_DECL OStream : public Stream
00731 {
00732   static const StreamType stream_type = stream_types::Output;
00733 
00734   OStream(uint8_t* data, uint32_t count)
00735   : Stream(data, count)
00736   {}
00737 
00741   template<typename T>
00742   ROS_FORCE_INLINE void next(const T& t)
00743   {
00744     serialize(*this, t);
00745   }
00746 
00747   template<typename T>
00748   ROS_FORCE_INLINE OStream& operator<<(const T& t)
00749   {
00750     serialize(*this, t);
00751     return *this;
00752   }
00753 };
00754 
00755 
00762 struct ROSCPP_SERIALIZATION_DECL LStream
00763 {
00764   static const StreamType stream_type = stream_types::Length;
00765 
00766   LStream()
00767   : count_(0)
00768   {}
00769 
00773   template<typename T>
00774   ROS_FORCE_INLINE void next(const T& t)
00775   {
00776     count_ += serializationLength(t);
00777   }
00778 
00782   ROS_FORCE_INLINE uint32_t advance(uint32_t len)
00783   {
00784     uint32_t old = count_;
00785     count_ += len;
00786     return old;
00787   }
00788 
00792   inline uint32_t getLength() { return count_; }
00793 
00794 private:
00795   uint32_t count_;
00796 };
00797 
00801 template<typename M>
00802 inline SerializedMessage serializeMessage(const M& message)
00803 {
00804   SerializedMessage m;
00805   uint32_t len = serializationLength(message);
00806   m.num_bytes = len + 4;
00807   m.buf.reset(new uint8_t[m.num_bytes]);
00808 
00809   OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00810   serialize(s, (uint32_t)m.num_bytes - 4);
00811   m.message_start = s.getData();
00812   serialize(s, message);
00813 
00814   return m;
00815 }
00816 
00820 template<typename M>
00821 inline SerializedMessage serializeServiceResponse(bool ok, const M& message)
00822 {
00823   SerializedMessage m;
00824 
00825   if (ok)
00826   {
00827     uint32_t len = serializationLength(message);
00828     m.num_bytes = len + 5;
00829     m.buf.reset(new uint8_t[m.num_bytes]);
00830 
00831     OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00832     serialize(s, (uint8_t)ok);
00833     serialize(s, (uint32_t)m.num_bytes - 5);
00834     serialize(s, message);
00835   }
00836   else
00837   {
00838     m.num_bytes = 5;
00839     m.buf.reset(new uint8_t[5]);
00840     OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00841     serialize(s, (uint8_t)ok);
00842     serialize(s, (uint32_t)0);
00843   }
00844 
00845   return m;
00846 }
00847 
00851 template<typename M>
00852 inline void deserializeMessage(const SerializedMessage& m, M& message)
00853 {
00854   IStream s(m.message_start, m.num_bytes - (m.message_start - m.buf.get()));
00855   deserialize(s, message);
00856 }
00857 
00858 } // namespace serialization
00859 } // namespace ros
00860 
00861 #endif // ROSCPP_SERIALIZATION_H