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 #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 #include "ros/datatypes.h"
00041
00042 #include <vector>
00043 #include <map>
00044
00045 #include <boost/array.hpp>
00046 #include <boost/call_traits.hpp>
00047 #include <boost/utility/enable_if.hpp>
00048 #include <boost/mpl/and.hpp>
00049 #include <boost/mpl/or.hpp>
00050 #include <boost/mpl/not.hpp>
00051
00052 #include <cstring>
00053
00054 #define ROS_NEW_SERIALIZATION_API 1
00055
00073 #define ROS_DECLARE_ALLINONE_SERIALIZER \
00074 template<typename Stream, typename T> \
00075 inline static void write(Stream& stream, const T& t) \
00076 { \
00077 allInOne<Stream, const T&>(stream, t); \
00078 } \
00079 \
00080 template<typename Stream, typename T> \
00081 inline static void read(Stream& stream, T& t) \
00082 { \
00083 allInOne<Stream, T&>(stream, t); \
00084 } \
00085 \
00086 template<typename T> \
00087 inline static uint32_t serializedLength(const T& t) \
00088 { \
00089 LStream stream; \
00090 allInOne<LStream, const T&>(stream, t); \
00091 return stream.getLength(); \
00092 }
00093
00094 namespace ros
00095 {
00096 namespace serialization
00097 {
00098 namespace mt = message_traits;
00099 namespace mpl = boost::mpl;
00100
00101 class ROSCPP_SERIALIZATION_DECL StreamOverrunException : public ros::Exception
00102 {
00103 public:
00104 StreamOverrunException(const std::string& what)
00105 : Exception(what)
00106 {}
00107 };
00108
00109 ROSCPP_SERIALIZATION_DECL void throwStreamOverrun();
00110
00118 template<typename T>
00119 struct Serializer
00120 {
00124 template<typename Stream>
00125 inline static void write(Stream& stream, typename boost::call_traits<T>::param_type t)
00126 {
00127 t.serialize(stream.getData(), 0);
00128 }
00129
00133 template<typename Stream>
00134 inline static void read(Stream& stream, typename boost::call_traits<T>::reference t)
00135 {
00136 t.deserialize(stream.getData());
00137 }
00138
00142 inline static uint32_t serializedLength(typename boost::call_traits<T>::param_type t)
00143 {
00144 return t.serializationLength();
00145 }
00146 };
00147
00151 template<typename T, typename Stream>
00152 inline void serialize(Stream& stream, const T& t)
00153 {
00154 Serializer<T>::write(stream, t);
00155 }
00156
00160 template<typename T, typename Stream>
00161 inline void deserialize(Stream& stream, T& t)
00162 {
00163 Serializer<T>::read(stream, t);
00164 }
00165
00169 template<typename T>
00170 inline uint32_t serializationLength(const T& t)
00171 {
00172 return Serializer<T>::serializedLength(t);
00173 }
00174
00175 #define ROS_CREATE_SIMPLE_SERIALIZER(Type) \
00176 template<> struct Serializer<Type> \
00177 { \
00178 template<typename Stream> inline static void write(Stream& stream, const Type v) \
00179 { \
00180 *reinterpret_cast<Type*>(stream.advance(sizeof(v))) = v; \
00181 } \
00182 \
00183 template<typename Stream> inline static void read(Stream& stream, Type& v) \
00184 { \
00185 v = *reinterpret_cast<Type*>(stream.advance(sizeof(v))); \
00186 } \
00187 \
00188 inline static uint32_t serializedLength(const Type&) \
00189 { \
00190 return sizeof(Type); \
00191 } \
00192 };
00193
00194 #define ROS_CREATE_SIMPLE_SERIALIZER_ARM(Type) \
00195 template<> struct Serializer<Type> \
00196 { \
00197 template<typename Stream> inline static void write(Stream& stream, const Type v) \
00198 { \
00199 memcpy(stream.advance(sizeof(v)), &v, sizeof(v) ); \
00200 } \
00201 \
00202 template<typename Stream> inline static void read(Stream& stream, Type& v) \
00203 { \
00204 memcpy(&v, stream.advance(sizeof(v)), sizeof(v) ); \
00205 } \
00206 \
00207 inline static uint32_t serializedLength(const Type t) \
00208 { \
00209 return sizeof(Type); \
00210 } \
00211 };
00212
00213 #if defined(__arm__) || defined(__arm)
00214 ROS_CREATE_SIMPLE_SERIALIZER_ARM(uint8_t);
00215 ROS_CREATE_SIMPLE_SERIALIZER_ARM(int8_t);
00216 ROS_CREATE_SIMPLE_SERIALIZER_ARM(uint16_t);
00217 ROS_CREATE_SIMPLE_SERIALIZER_ARM(int16_t);
00218 ROS_CREATE_SIMPLE_SERIALIZER_ARM(uint32_t);
00219 ROS_CREATE_SIMPLE_SERIALIZER_ARM(int32_t);
00220 ROS_CREATE_SIMPLE_SERIALIZER_ARM(uint64_t);
00221 ROS_CREATE_SIMPLE_SERIALIZER_ARM(int64_t);
00222 ROS_CREATE_SIMPLE_SERIALIZER_ARM(float);
00223 ROS_CREATE_SIMPLE_SERIALIZER_ARM(double);
00224 #else
00225 ROS_CREATE_SIMPLE_SERIALIZER(uint8_t);
00226 ROS_CREATE_SIMPLE_SERIALIZER(int8_t);
00227 ROS_CREATE_SIMPLE_SERIALIZER(uint16_t);
00228 ROS_CREATE_SIMPLE_SERIALIZER(int16_t);
00229 ROS_CREATE_SIMPLE_SERIALIZER(uint32_t);
00230 ROS_CREATE_SIMPLE_SERIALIZER(int32_t);
00231 ROS_CREATE_SIMPLE_SERIALIZER(uint64_t);
00232 ROS_CREATE_SIMPLE_SERIALIZER(int64_t);
00233 ROS_CREATE_SIMPLE_SERIALIZER(float);
00234 ROS_CREATE_SIMPLE_SERIALIZER(double);
00235 #endif
00236
00240 template<> struct Serializer<bool>
00241 {
00242 template<typename Stream> inline static void write(Stream& stream, const bool v)
00243 {
00244 uint8_t b = (uint8_t)v;
00245 #if defined(__arm__) || defined(__arm)
00246 memcpy(stream.advance(1), &b, 1 );
00247 #else
00248 *reinterpret_cast<uint8_t*>(stream.advance(1)) = b;
00249 #endif
00250 }
00251
00252 template<typename Stream> inline static void read(Stream& stream, bool& v)
00253 {
00254 uint8_t b;
00255 #if defined(__arm__) || defined(__arm)
00256 memcpy(&b, stream.advance(1), 1 );
00257 #else
00258 b = *reinterpret_cast<uint8_t*>(stream.advance(1));
00259 #endif
00260 v = (bool)b;
00261 }
00262
00263 inline static uint32_t serializedLength(bool)
00264 {
00265 return 1;
00266 }
00267 };
00268
00272 template<class ContainerAllocator>
00273 struct Serializer<std::basic_string<char, std::char_traits<char>, ContainerAllocator> >
00274 {
00275 typedef std::basic_string<char, std::char_traits<char>, ContainerAllocator> StringType;
00276
00277 template<typename Stream>
00278 inline static void write(Stream& stream, const StringType& str)
00279 {
00280 size_t len = str.size();
00281 stream.next((uint32_t)len);
00282
00283 if (len > 0)
00284 {
00285 memcpy(stream.advance((uint32_t)len), str.data(), len);
00286 }
00287 }
00288
00289 template<typename Stream>
00290 inline static void read(Stream& stream, StringType& str)
00291 {
00292 uint32_t len;
00293 stream.next(len);
00294 if (len > 0)
00295 {
00296 str = StringType((char*)stream.advance(len), len);
00297 }
00298 else
00299 {
00300 str.clear();
00301 }
00302 }
00303
00304 inline static uint32_t serializedLength(const StringType& str)
00305 {
00306 return 4 + (uint32_t)str.size();
00307 }
00308 };
00309
00313 template<>
00314 struct Serializer<ros::Time>
00315 {
00316 template<typename Stream>
00317 inline static void write(Stream& stream, const ros::Time& v)
00318 {
00319 stream.next(v.sec);
00320 stream.next(v.nsec);
00321 }
00322
00323 template<typename Stream>
00324 inline static void read(Stream& stream, ros::Time& v)
00325 {
00326 stream.next(v.sec);
00327 stream.next(v.nsec);
00328 }
00329
00330 inline static uint32_t serializedLength(const ros::Time&)
00331 {
00332 return 8;
00333 }
00334 };
00335
00339 template<>
00340 struct Serializer<ros::Duration>
00341 {
00342 template<typename Stream>
00343 inline static void write(Stream& stream, const ros::Duration& v)
00344 {
00345 stream.next(v.sec);
00346 stream.next(v.nsec);
00347 }
00348
00349 template<typename Stream>
00350 inline static void read(Stream& stream, ros::Duration& v)
00351 {
00352 stream.next(v.sec);
00353 stream.next(v.nsec);
00354 }
00355
00356 inline static uint32_t serializedLength(const ros::Duration&)
00357 {
00358 return 8;
00359 }
00360 };
00361
00365 template<typename T, class ContainerAllocator, class Enabled = void>
00366 struct VectorSerializer
00367 {};
00368
00372 template<typename T, class ContainerAllocator>
00373 struct VectorSerializer<T, ContainerAllocator, typename boost::disable_if<mt::IsFixedSize<T> >::type >
00374 {
00375 typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00376 typedef typename VecType::iterator IteratorType;
00377 typedef typename VecType::const_iterator ConstIteratorType;
00378
00379 template<typename Stream>
00380 inline static void write(Stream& stream, const VecType& v)
00381 {
00382 stream.next((uint32_t)v.size());
00383 ConstIteratorType it = v.begin();
00384 ConstIteratorType end = v.end();
00385 for (; it != end; ++it)
00386 {
00387 stream.next(*it);
00388 }
00389 }
00390
00391 template<typename Stream>
00392 inline static void read(Stream& stream, VecType& v)
00393 {
00394 uint32_t len;
00395 stream.next(len);
00396 v.resize(len);
00397 IteratorType it = v.begin();
00398 IteratorType end = v.end();
00399 for (; it != end; ++it)
00400 {
00401 stream.next(*it);
00402 }
00403 }
00404
00405 inline static uint32_t serializedLength(const VecType& v)
00406 {
00407 uint32_t size = 4;
00408 ConstIteratorType it = v.begin();
00409 ConstIteratorType end = v.end();
00410 for (; it != end; ++it)
00411 {
00412 size += serializationLength(*it);
00413 }
00414
00415 return size;
00416 }
00417 };
00418
00422 template<typename T, class ContainerAllocator>
00423 struct VectorSerializer<T, ContainerAllocator, typename boost::enable_if<mt::IsSimple<T> >::type >
00424 {
00425 typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00426 typedef typename VecType::iterator IteratorType;
00427 typedef typename VecType::const_iterator ConstIteratorType;
00428
00429 template<typename Stream>
00430 inline static void write(Stream& stream, const VecType& v)
00431 {
00432 uint32_t len = (uint32_t)v.size();
00433 stream.next(len);
00434 if (!v.empty())
00435 {
00436 const uint32_t data_len = len * (uint32_t)sizeof(T);
00437 memcpy(stream.advance(data_len), &v.front(), data_len);
00438 }
00439 }
00440
00441 template<typename Stream>
00442 inline static void read(Stream& stream, VecType& v)
00443 {
00444 uint32_t len;
00445 stream.next(len);
00446 v.resize(len);
00447
00448 if (len > 0)
00449 {
00450 const uint32_t data_len = (uint32_t)sizeof(T) * len;
00451 memcpy(&v.front(), stream.advance(data_len), data_len);
00452 }
00453 }
00454
00455 inline static uint32_t serializedLength(const VecType& v)
00456 {
00457 return 4 + v.size() * (uint32_t)sizeof(T);
00458 }
00459 };
00460
00464 template<typename T, class ContainerAllocator>
00465 struct VectorSerializer<T, ContainerAllocator, typename boost::enable_if<mpl::and_<mt::IsFixedSize<T>, mpl::not_<mt::IsSimple<T> > > >::type >
00466 {
00467 typedef std::vector<T, typename ContainerAllocator::template rebind<T>::other> VecType;
00468 typedef typename VecType::iterator IteratorType;
00469 typedef typename VecType::const_iterator ConstIteratorType;
00470
00471 template<typename Stream>
00472 inline static void write(Stream& stream, const VecType& v)
00473 {
00474 stream.next((uint32_t)v.size());
00475 ConstIteratorType it = v.begin();
00476 ConstIteratorType end = v.end();
00477 for (; it != end; ++it)
00478 {
00479 stream.next(*it);
00480 }
00481 }
00482
00483 template<typename Stream>
00484 inline static void read(Stream& stream, VecType& v)
00485 {
00486 uint32_t len;
00487 stream.next(len);
00488 v.resize(len);
00489 IteratorType it = v.begin();
00490 IteratorType end = v.end();
00491 for (; it != end; ++it)
00492 {
00493 stream.next(*it);
00494 }
00495 }
00496
00497 inline static uint32_t serializedLength(const VecType& v)
00498 {
00499 uint32_t size = 4;
00500 if (!v.empty())
00501 {
00502 uint32_t len_each = serializationLength(v.front());
00503 size += len_each * (uint32_t)v.size();
00504 }
00505
00506 return size;
00507 }
00508 };
00509
00513 template<typename T, class ContainerAllocator, typename Stream>
00514 inline void serialize(Stream& stream, const std::vector<T, ContainerAllocator>& t)
00515 {
00516 VectorSerializer<T, ContainerAllocator>::write(stream, t);
00517 }
00518
00522 template<typename T, class ContainerAllocator, typename Stream>
00523 inline void deserialize(Stream& stream, std::vector<T, ContainerAllocator>& t)
00524 {
00525 VectorSerializer<T, ContainerAllocator>::read(stream, t);
00526 }
00527
00531 template<typename T, class ContainerAllocator>
00532 inline uint32_t serializationLength(const std::vector<T, ContainerAllocator>& t)
00533 {
00534 return VectorSerializer<T, ContainerAllocator>::serializedLength(t);
00535 }
00536
00540 template<typename T, size_t N, class Enabled = void>
00541 struct ArraySerializer
00542 {};
00543
00547 template<typename T, size_t N>
00548 struct ArraySerializer<T, N, typename boost::disable_if<mt::IsFixedSize<T> >::type>
00549 {
00550 typedef boost::array<T, N > ArrayType;
00551 typedef typename ArrayType::iterator IteratorType;
00552 typedef typename ArrayType::const_iterator ConstIteratorType;
00553
00554 template<typename Stream>
00555 inline static void write(Stream& stream, const ArrayType& v)
00556 {
00557 ConstIteratorType it = v.begin();
00558 ConstIteratorType end = v.end();
00559 for (; it != end; ++it)
00560 {
00561 stream.next(*it);
00562 }
00563 }
00564
00565 template<typename Stream>
00566 inline static void read(Stream& stream, ArrayType& v)
00567 {
00568 IteratorType it = v.begin();
00569 IteratorType end = v.end();
00570 for (; it != end; ++it)
00571 {
00572 stream.next(*it);
00573 }
00574 }
00575
00576 inline static uint32_t serializedLength(const ArrayType& v)
00577 {
00578 uint32_t size = 0;
00579 ConstIteratorType it = v.begin();
00580 ConstIteratorType end = v.end();
00581 for (; it != end; ++it)
00582 {
00583 size += serializationLength(*it);
00584 }
00585
00586 return size;
00587 }
00588 };
00589
00593 template<typename T, size_t N>
00594 struct ArraySerializer<T, N, typename boost::enable_if<mt::IsSimple<T> >::type>
00595 {
00596 typedef boost::array<T, N > ArrayType;
00597 typedef typename ArrayType::iterator IteratorType;
00598 typedef typename ArrayType::const_iterator ConstIteratorType;
00599
00600 template<typename Stream>
00601 inline static void write(Stream& stream, const ArrayType& v)
00602 {
00603 const uint32_t data_len = N * sizeof(T);
00604 memcpy(stream.advance(data_len), &v.front(), data_len);
00605 }
00606
00607 template<typename Stream>
00608 inline static void read(Stream& stream, ArrayType& v)
00609 {
00610 const uint32_t data_len = N * sizeof(T);
00611 memcpy(&v.front(), stream.advance(data_len), data_len);
00612 }
00613
00614 inline static uint32_t serializedLength(const ArrayType&)
00615 {
00616 return N * sizeof(T);
00617 }
00618 };
00619
00623 template<typename T, size_t N>
00624 struct ArraySerializer<T, N, typename boost::enable_if<mpl::and_<mt::IsFixedSize<T>, mpl::not_<mt::IsSimple<T> > > >::type>
00625 {
00626 typedef boost::array<T, N > ArrayType;
00627 typedef typename ArrayType::iterator IteratorType;
00628 typedef typename ArrayType::const_iterator ConstIteratorType;
00629
00630 template<typename Stream>
00631 inline static void write(Stream& stream, const ArrayType& v)
00632 {
00633 ConstIteratorType it = v.begin();
00634 ConstIteratorType end = v.end();
00635 for (; it != end; ++it)
00636 {
00637 stream.next(*it);
00638 }
00639 }
00640
00641 template<typename Stream>
00642 inline static void read(Stream& stream, ArrayType& v)
00643 {
00644 IteratorType it = v.begin();
00645 IteratorType end = v.end();
00646 for (; it != end; ++it)
00647 {
00648 stream.next(*it);
00649 }
00650 }
00651
00652 inline static uint32_t serializedLength(const ArrayType& v)
00653 {
00654 return serializationLength(v.front()) * N;
00655 }
00656 };
00657
00661 template<typename T, size_t N, typename Stream>
00662 inline void serialize(Stream& stream, const boost::array<T, N>& t)
00663 {
00664 ArraySerializer<T, N>::write(stream, t);
00665 }
00666
00670 template<typename T, size_t N, typename Stream>
00671 inline void deserialize(Stream& stream, boost::array<T, N>& t)
00672 {
00673 ArraySerializer<T, N>::read(stream, t);
00674 }
00675
00679 template<typename T, size_t N>
00680 inline uint32_t serializationLength(const boost::array<T, N>& t)
00681 {
00682 return ArraySerializer<T, N>::serializedLength(t);
00683 }
00684
00688 namespace stream_types
00689 {
00690 enum StreamType
00691 {
00692 Input,
00693 Output,
00694 Length
00695 };
00696 }
00697 typedef stream_types::StreamType StreamType;
00698
00702 struct ROSCPP_SERIALIZATION_DECL Stream
00703 {
00704
00705
00706
00707 inline uint8_t* getData() { return data_; }
00713 ROS_FORCE_INLINE uint8_t* advance(uint32_t len)
00714 {
00715 uint8_t* old_data = data_;
00716 data_ += len;
00717 if (data_ > end_)
00718 {
00719
00720
00721 throwStreamOverrun();
00722 }
00723 return old_data;
00724 }
00725
00729 inline uint32_t getLength() { return (uint32_t)(end_ - data_); }
00730
00731 protected:
00732 Stream(uint8_t* _data, uint32_t _count)
00733 : data_(_data)
00734 , end_(_data + _count)
00735 {}
00736
00737 private:
00738 uint8_t* data_;
00739 uint8_t* end_;
00740 };
00741
00745 struct ROSCPP_SERIALIZATION_DECL IStream : public Stream
00746 {
00747 static const StreamType stream_type = stream_types::Input;
00748
00749 IStream(uint8_t* data, uint32_t count)
00750 : Stream(data, count)
00751 {}
00752
00756 template<typename T>
00757 ROS_FORCE_INLINE void next(T& t)
00758 {
00759 deserialize(*this, t);
00760 }
00761
00762 template<typename T>
00763 ROS_FORCE_INLINE IStream& operator>>(T& t)
00764 {
00765 deserialize(*this, t);
00766 return *this;
00767 }
00768 };
00769
00773 struct ROSCPP_SERIALIZATION_DECL OStream : public Stream
00774 {
00775 static const StreamType stream_type = stream_types::Output;
00776
00777 OStream(uint8_t* data, uint32_t count)
00778 : Stream(data, count)
00779 {}
00780
00784 template<typename T>
00785 ROS_FORCE_INLINE void next(const T& t)
00786 {
00787 serialize(*this, t);
00788 }
00789
00790 template<typename T>
00791 ROS_FORCE_INLINE OStream& operator<<(const T& t)
00792 {
00793 serialize(*this, t);
00794 return *this;
00795 }
00796 };
00797
00798
00805 struct ROSCPP_SERIALIZATION_DECL LStream
00806 {
00807 static const StreamType stream_type = stream_types::Length;
00808
00809 LStream()
00810 : count_(0)
00811 {}
00812
00816 template<typename T>
00817 ROS_FORCE_INLINE void next(const T& t)
00818 {
00819 count_ += serializationLength(t);
00820 }
00821
00825 ROS_FORCE_INLINE uint32_t advance(uint32_t len)
00826 {
00827 uint32_t old = count_;
00828 count_ += len;
00829 return old;
00830 }
00831
00835 inline uint32_t getLength() { return count_; }
00836
00837 private:
00838 uint32_t count_;
00839 };
00840
00844 template<typename M>
00845 inline SerializedMessage serializeMessage(const M& message)
00846 {
00847 SerializedMessage m;
00848 uint32_t len = serializationLength(message);
00849 m.num_bytes = len + 4;
00850 m.buf.reset(new uint8_t[m.num_bytes]);
00851
00852 OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00853 serialize(s, (uint32_t)m.num_bytes - 4);
00854 m.message_start = s.getData();
00855 serialize(s, message);
00856
00857 return m;
00858 }
00859
00863 template<typename M>
00864 inline SerializedMessage serializeServiceResponse(bool ok, const M& message)
00865 {
00866 SerializedMessage m;
00867
00868 if (ok)
00869 {
00870 uint32_t len = serializationLength(message);
00871 m.num_bytes = len + 5;
00872 m.buf.reset(new uint8_t[m.num_bytes]);
00873
00874 OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00875 serialize(s, (uint8_t)ok);
00876 serialize(s, (uint32_t)m.num_bytes - 5);
00877 serialize(s, message);
00878 }
00879 else
00880 {
00881 uint32_t len = serializationLength(message);
00882 m.num_bytes = len + 1;
00883 m.buf.reset(new uint8_t[m.num_bytes]);
00884
00885 OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00886 serialize(s, (uint8_t)ok);
00887 serialize(s, message);
00888 }
00889
00890 return m;
00891 }
00892
00896 template<typename M>
00897 inline void deserializeMessage(const SerializedMessage& m, M& message)
00898 {
00899 IStream s(m.message_start, m.num_bytes - (m.message_start - m.buf.get()));
00900 deserialize(s, message);
00901 }
00902
00903
00904
00905
00906 template<typename M>
00907 struct PreDeserializeParams
00908 {
00909 boost::shared_ptr<M> message;
00910 boost::shared_ptr<std::map<std::string, std::string> > connection_header;
00911 };
00912
00917 template<typename M>
00918 struct PreDeserialize
00919 {
00920 static void notify(const PreDeserializeParams<M>&) { }
00921 };
00922
00923 }
00924
00925 }
00926
00927 #endif // ROSCPP_SERIALIZATION_H