serialization.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* Author: Josh Faust */
00031 
00032 /*
00033  * Test serialization templates
00034  */
00035 
00036 #include <gtest/gtest.h>
00037 #include <ros/static_assert.h>
00038 #include <std_msgs/Header.h>
00039 #include "helpers.h"
00040 
00041 using namespace ros;
00042 using namespace ros::serialization;
00043 using namespace test_roscpp;
00044 
00045 ROS_STATIC_ASSERT(sizeof(ros::Time) == 8);
00046 ROS_STATIC_ASSERT(sizeof(ros::Duration) == 8);
00047 
00049 // Tests for compilation/validity of serialization/deserialization of primitive types
00051 
00052 #define PRIMITIVE_SERIALIZATION_TEST(Type, SerInit, DeserInit) \
00053   TEST(Serialization, Type) \
00054   { \
00055     Type ser_val SerInit; \
00056     Type deser_val DeserInit; \
00057     Array b = serializeAndDeserialize(ser_val, deser_val); \
00058     EXPECT_EQ(*(Type*)b.get(), ser_val); \
00059     EXPECT_EQ(ser_val, deser_val); \
00060   }
00061 
00062 PRIMITIVE_SERIALIZATION_TEST(uint8_t, (5), (0));
00063 PRIMITIVE_SERIALIZATION_TEST(int8_t, (5), (0));
00064 PRIMITIVE_SERIALIZATION_TEST(uint16_t, (5), (0));
00065 PRIMITIVE_SERIALIZATION_TEST(int16_t, (5), (0));
00066 PRIMITIVE_SERIALIZATION_TEST(uint32_t, (5), (0));
00067 PRIMITIVE_SERIALIZATION_TEST(int32_t, (5), (0));
00068 PRIMITIVE_SERIALIZATION_TEST(uint64_t, (5), (0));
00069 PRIMITIVE_SERIALIZATION_TEST(int64_t, (5), (0));
00070 PRIMITIVE_SERIALIZATION_TEST(float, (5.0f), (0.0f));
00071 PRIMITIVE_SERIALIZATION_TEST(double, (5.0), (0.0));
00072 PRIMITIVE_SERIALIZATION_TEST(Time, (500, 10000), (0, 0));
00073 PRIMITIVE_SERIALIZATION_TEST(Duration, (500, 10000), (0, 0));
00074 
00075 #define PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(Type, Start, Increment) \
00076   TEST(Serialization, variableLengthArray_##Type) \
00077   { \
00078     std::vector<Type> ser_val, deser_val; \
00079     Type val = Start; \
00080     for (uint32_t i = 0; i < 8; ++i) \
00081     { \
00082       ser_val.push_back(val); \
00083       val = val + Increment; \
00084     } \
00085     \
00086     Array b = serializeAndDeserialize(ser_val, deser_val); \
00087     EXPECT_TRUE(ser_val == deser_val); \
00088     \
00089     EXPECT_EQ(*(uint32_t*)b.get(), (uint32_t)ser_val.size()); \
00090     for(size_t i = 0; i < ser_val.size(); ++i) \
00091     { \
00092       Type* ptr = ((Type*)(b.get() + 4)) + i; \
00093       EXPECT_EQ(*ptr, ser_val[i]); \
00094     } \
00095   }
00096 
00097 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(uint8_t, 65, 1);
00098 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(int8_t, 65, 1);
00099 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(uint16_t, 0, 100);
00100 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(int16_t, 0, 100);
00101 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(uint32_t, 0, 100);
00102 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(int32_t, 0, 100);
00103 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(uint64_t, 0, 100);
00104 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(int64_t, 0, 100);
00105 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(float, 0.0f, 100.0f);
00106 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(double, 0.0, 100.0);
00107 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(Time, Time(), Duration(100));
00108 PRIMITIVE_VARIABLE_LENGTH_ARRAY_TEST(Duration, Duration(), Duration(100));
00109 
00110 #define PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(Type, Start, Increment) \
00111   TEST(Serialization, fixedLengthArray_##Type) \
00112   { \
00113     boost::array<Type, 8> ser_val, deser_val; \
00114     Type val = Start; \
00115     for (uint32_t i = 0; i < 8; ++i) \
00116     { \
00117       ser_val[i] = val; \
00118       val = val + Increment; \
00119     } \
00120     \
00121     Array b = serializeAndDeserialize(ser_val, deser_val); \
00122     EXPECT_TRUE(ser_val == deser_val); \
00123     \
00124     for(size_t i = 0; i < ser_val.size(); ++i) \
00125     { \
00126       Type* ptr = ((Type*)b.get()) + i; \
00127       EXPECT_EQ(*ptr, ser_val[i]); \
00128     } \
00129   }
00130 
00131 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(uint8_t, 65, 1);
00132 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(int8_t, 65, 1);
00133 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(uint16_t, 0, 100);
00134 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(int16_t, 0, 100);
00135 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(uint32_t, 0, 100);
00136 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(int32_t, 0, 100);
00137 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(uint64_t, 0, 100);
00138 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(int64_t, 0, 100);
00139 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(float, 0.0f, 100.0f);
00140 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(double, 0.0, 100.0);
00141 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(Time, Time(), Duration(100));
00142 PRIMITIVE_FIXED_LENGTH_ARRAY_TEST(Duration, Duration(), Duration(100));
00143 
00144 TEST(Serialization, string)
00145 {
00146   std::string ser_val = "hello world";
00147   std::string deser_val;
00148   Array b = serializeAndDeserialize(ser_val, deser_val);
00149   EXPECT_STREQ(ser_val.c_str(), deser_val.c_str());
00150 
00151   EXPECT_EQ(*(uint32_t*)b.get(), (uint32_t)ser_val.size());
00152   EXPECT_EQ(memcmp(b.get() + 4, ser_val.data(), ser_val.size()), 0);
00153 }
00154 
00155 TEST(Serialization, variableLengthArray_string)
00156 {
00157   std::vector<std::string> ser_val, deser_val;
00158   ser_val.push_back("hello world");
00159   ser_val.push_back("hello world22");
00160   ser_val.push_back("hello world333");
00161   ser_val.push_back("hello world4444");
00162   ser_val.push_back("hello world55555");
00163   Array b = serializeAndDeserialize(ser_val, deser_val);
00164   EXPECT_TRUE(ser_val == deser_val);
00165 }
00166 
00167 TEST(Serialization, fixedLengthArray_string)
00168 {
00169   boost::array<std::string, 5> ser_val, deser_val;
00170   ser_val[0] = "hello world";
00171   ser_val[1] = "hello world22";
00172   ser_val[2] = "hello world333";
00173   ser_val[3] = "hello world4444";
00174   ser_val[4] = "hello world55555";
00175   Array b = serializeAndDeserialize(ser_val, deser_val);
00176   EXPECT_TRUE(ser_val == deser_val);
00177 }
00178 
00180 // Test custom types and traits
00182 
00183 // Class used to make sure fixed-size simple structs use a memcpy when serializing an array of them
00184 // serialization only serializes a, memcpy will get both a and b.
00185 struct FixedSizeSimple
00186 {
00187   FixedSizeSimple()
00188   : a(0)
00189   , b(0)
00190   {}
00191 
00192   int32_t a;
00193   int32_t b;
00194 };
00195 
00196 namespace ros
00197 {
00198 namespace message_traits
00199 {
00200 template<> struct IsFixedSize<FixedSizeSimple> : public TrueType {};
00201 template<> struct IsSimple<FixedSizeSimple> : public TrueType {};
00202 } // namespace message_traits
00203 
00204 namespace serialization
00205 {
00206 template<>
00207 struct Serializer<FixedSizeSimple>
00208 {
00209   template<typename Stream>
00210   inline static void write(Stream& stream, const FixedSizeSimple& v)
00211   {
00212     serialize(stream, v.a);
00213   }
00214 
00215   template<typename Stream>
00216   inline static void read(Stream& stream, FixedSizeSimple& v)
00217   {
00218     deserialize(stream, v.a);
00219   }
00220 
00221   inline static uint32_t serializedLength(const FixedSizeSimple&)
00222   {
00223     return 4;
00224   }
00225 };
00226 } // namespace serialization
00227 } // namespace ros
00228 
00229 TEST(Serialization, fixedSizeSimple_vector)
00230 {
00231   {
00232     FixedSizeSimple in, out;
00233     in.a = 1;
00234     in.b = 1;
00235 
00236     serializeAndDeserialize(in, out);
00237     ASSERT_EQ(out.a, 1);
00238     ASSERT_EQ(out.b, 0);
00239   }
00240 
00241   {
00242     std::vector<FixedSizeSimple> in, out;
00243     in.resize(1);
00244     in[0].a = 1;
00245     in[0].b = 1;
00246 
00247     serializeAndDeserialize(in, out);
00248     ASSERT_EQ(out[0].a, 1);
00249     ASSERT_EQ(out[0].b, 1);
00250   }
00251 }
00252 
00253 TEST(Serialization, fixedSizeSimple_array)
00254 {
00255   boost::array<FixedSizeSimple, 2> in, out;
00256   in[0].a = 1;
00257   in[0].b = 1;
00258 
00259   serializeAndDeserialize(in, out);
00260   ASSERT_EQ(out[0].a, 1);
00261   ASSERT_EQ(out[0].b, 1);
00262 }
00263 
00264 // Class used to make sure fixed-size non-simple structs only query the length of the first element
00265 // in an array.
00266 struct FixedSizeNonSimple
00267 {
00268   FixedSizeNonSimple()
00269   : length_to_report(4)
00270   {}
00271 
00272   int32_t length_to_report;
00273 };
00274 
00275 namespace ros
00276 {
00277 namespace message_traits
00278 {
00279 template<> struct IsFixedSize<FixedSizeNonSimple> : public TrueType {};
00280 } // namespace message_traits
00281 
00282 namespace serialization
00283 {
00284 template<>
00285 struct Serializer<FixedSizeNonSimple>
00286 {
00287   inline static uint32_t serializedLength(const FixedSizeNonSimple& v)
00288   {
00289     return v.length_to_report;
00290   }
00291 };
00292 } // namespace serialization
00293 } // namespace ros
00294 
00295 TEST(Serialization, fixedSizeNonSimple_vector)
00296 {
00297   std::vector<FixedSizeNonSimple> in;
00298   in.resize(2);
00299   in[1].length_to_report = 100;
00300 
00301   int32_t len = ros::serialization::serializationLength(in);
00302   ASSERT_EQ(len, 12);  // 12 = 4 bytes for each item + 4-byte array length
00303 }
00304 
00305 TEST(Serialization, fixedSizeNonSimple_array)
00306 {
00307   boost::array<FixedSizeNonSimple, 2> in;
00308   in[1].length_to_report = 100;
00309 
00310   int32_t len = ros::serialization::serializationLength(in);
00311   ASSERT_EQ(len, 8);  // 8 = 4 bytes for each item
00312 }
00313 
00314 // Class used to make sure variable-size structs query the length of the every element
00315 // in an array.
00316 struct VariableSize
00317 {
00318   VariableSize()
00319   : length_to_report(4)
00320   {}
00321 
00322   int32_t length_to_report;
00323 };
00324 
00325 namespace ros
00326 {
00327 namespace serialization
00328 {
00329 template<>
00330 struct Serializer<VariableSize>
00331 {
00332   inline static uint32_t serializedLength(const VariableSize& v)
00333   {
00334     return v.length_to_report;
00335   }
00336 };
00337 } // namespace serialization
00338 } // namespace ros
00339 
00340 TEST(Serialization, variableSize_vector)
00341 {
00342   std::vector<VariableSize> in;
00343   in.resize(2);
00344   in[1].length_to_report = 100;
00345 
00346   int32_t len = ros::serialization::serializationLength(in);
00347   ASSERT_EQ(len, 108);  // 108 = 4 bytes for the first item + 100 bytes for the second + 4-byte array length
00348 }
00349 
00350 TEST(Serialization, variableSize_array)
00351 {
00352   boost::array<VariableSize, 2> in;
00353   in[1].length_to_report = 100;
00354 
00355   int32_t len = ros::serialization::serializationLength(in);
00356   ASSERT_EQ(len, 104);  // 104 = 4 bytes for the first item + 100 bytes for the second
00357 }
00358 
00359 struct AllInOneSerializer
00360 {
00361   uint32_t a;
00362 };
00363 
00364 namespace ros
00365 {
00366 namespace serialization
00367 {
00368 template<>
00369 struct Serializer<AllInOneSerializer>
00370 {
00371   template<typename Stream, typename T>
00372   inline static void allInOne(Stream& stream, T t)
00373   {
00374     stream.next(t.a);
00375   }
00376 
00377   ROS_DECLARE_ALLINONE_SERIALIZER;
00378 };
00379 } // namespace serialization
00380 } // namespace ros
00381 
00382 TEST(Serialization, allInOne)
00383 {
00384   AllInOneSerializer in, out;
00385   in.a = 5;
00386   serializeAndDeserialize(in, out);
00387   ASSERT_EQ(out.a, in.a);
00388 }
00389 
00390 // Class with a header, used to ensure message_traits::header(m) returns the header
00391 struct WithHeader
00392 {
00393   WithHeader()
00394   {}
00395 
00396   std_msgs::Header header;
00397 };
00398 
00399 // Class without a header, used to ensure message_traits::header(m) return NULL
00400 struct WithoutHeader
00401 {
00402   WithoutHeader()
00403   {}
00404 };
00405 
00406 namespace ros
00407 {
00408 namespace message_traits
00409 {
00410 template<> struct HasHeader<WithHeader> : public TrueType {};
00411 } // namespace message_traits
00412 } // namespace ros
00413 
00414 TEST(MessageTraits, headers)
00415 {
00416   WithHeader wh;
00417   WithoutHeader woh;
00418   const WithHeader cwh;
00419   const WithoutHeader cwoh;
00420 
00421   wh.header.seq = 100;
00422   ASSERT_TRUE(ros::message_traits::header(wh) != 0);
00423   ASSERT_EQ(ros::message_traits::header(wh)->seq, 100UL);
00424 
00425   ASSERT_TRUE(ros::message_traits::header(woh) == 0);
00426 
00427   ASSERT_TRUE(ros::message_traits::header(cwh) != 0);
00428   ASSERT_TRUE(ros::message_traits::header(cwoh) == 0);
00429 
00430   ASSERT_TRUE(ros::message_traits::frameId(wh) != 0);
00431   ASSERT_TRUE(ros::message_traits::frameId(woh) == 0);
00432   ASSERT_TRUE(ros::message_traits::frameId(cwh) != 0);
00433   ASSERT_TRUE(ros::message_traits::frameId(cwoh) == 0);
00434 
00435   ASSERT_TRUE(ros::message_traits::timeStamp(wh) != 0);
00436   ASSERT_TRUE(ros::message_traits::timeStamp(woh) == 0);
00437   ASSERT_TRUE(ros::message_traits::timeStamp(cwh) != 0);
00438   ASSERT_TRUE(ros::message_traits::timeStamp(cwoh) == 0);
00439 }
00440 
00441 TEST(Serialization, bufferOverrun)
00442 {
00443   Array b(new uint8_t[4]);
00444   IStream stream(b.get(), 4);
00445   uint32_t i;
00446   deserialize(stream, i);
00447   try
00448   {
00449     deserialize(stream, i);
00450     FAIL();
00451   }
00452   catch(ros::Exception&)
00453   {
00454     SUCCEED();
00455   }
00456 }
00457 
00458 TEST(Serialization, streamOperators)
00459 {
00460   Array b(new uint8_t[4]);
00461   OStream ostream(b.get(), 4);
00462   uint32_t a = 5;
00463   ostream << a;
00464   a = 100;
00465   IStream istream(b.get(), 4);
00466   istream >> a;
00467   ASSERT_EQ(a, 5UL);
00468 }
00469 
00470 int main(int argc, char** argv)
00471 {
00472   testing::InitGoogleTest(&argc, argv);
00473   return RUN_ALL_TESTS();
00474 }
00475 
00476 
00477 


test_roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Thu Jun 6 2019 21:10:42