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
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
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
00677
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 uint32_t len = serializationLength(message);
00839 m.num_bytes = len + 1;
00840 m.buf.reset(new uint8_t[m.num_bytes]);
00841
00842 OStream s(m.buf.get(), (uint32_t)m.num_bytes);
00843 serialize(s, (uint8_t)ok);
00844 serialize(s, message);
00845 }
00846
00847 return m;
00848 }
00849
00853 template<typename M>
00854 inline void deserializeMessage(const SerializedMessage& m, M& message)
00855 {
00856 IStream s(m.message_start, m.num_bytes - (m.message_start - m.buf.get()));
00857 deserialize(s, message);
00858 }
00859
00860 }
00861 }
00862
00863 #endif // ROSCPP_SERIALIZATION_H