Руслан Урядинский
libuav original
Dependents: UAVCAN UAVCAN_Subscriber
Embed:
(wiki syntax)
Show/hide line numbers
array.cpp
00001 /* 00002 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> 00003 */ 00004 00005 #if __GNUC__ 00006 # pragma GCC diagnostic ignored "-Wfloat-equal" 00007 # pragma GCC diagnostic ignored "-Wdouble-promotion" 00008 #endif 00009 00010 #include <gtest/gtest.h> 00011 #include <uavcan/marshal/types.hpp> 00012 #include <uavcan/transport/transfer_buffer.hpp> 00013 00014 using uavcan::Array; 00015 using uavcan::ArrayModeDynamic; 00016 using uavcan::ArrayModeStatic; 00017 using uavcan::IntegerSpec; 00018 using uavcan::FloatSpec; 00019 using uavcan::SignednessSigned; 00020 using uavcan::SignednessUnsigned; 00021 using uavcan::CastModeSaturate; 00022 using uavcan::CastModeTruncate; 00023 00024 struct CustomType 00025 { 00026 typedef uavcan::IntegerSpec<8, uavcan::SignednessSigned, uavcan::CastModeTruncate> A; 00027 typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> B; 00028 // Dynamic array of max len 5 --> 3 bits for len, 5 bits for data --> 1 byte max len 00029 typedef uavcan::Array<uavcan::IntegerSpec<1, uavcan::SignednessUnsigned, uavcan::CastModeSaturate>, 00030 uavcan::ArrayModeDynamic, 5> C; 00031 00032 enum { MinBitLen = A::MinBitLen + B::MinBitLen + C::MinBitLen }; 00033 enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen + C::MaxBitLen }; 00034 00035 typename uavcan::StorageType<A>::Type a; 00036 typename uavcan::StorageType<B>::Type b; 00037 typename uavcan::StorageType<C>::Type c; 00038 00039 CustomType() 00040 : a() 00041 , b() 00042 , c() 00043 { } 00044 00045 bool operator==(const CustomType& rhs) const 00046 { 00047 return a == rhs.a && 00048 uavcan::areFloatsExactlyEqual(b, rhs.b) && 00049 c == rhs.c; 00050 } 00051 00052 static int encode(const CustomType& obj, uavcan::ScalarCodec& codec, 00053 uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) 00054 { 00055 int res = 0; 00056 res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); 00057 if (res <= 0) 00058 { 00059 return res; 00060 } 00061 res = B::encode(obj.b, codec, uavcan::TailArrayOptDisabled); 00062 if (res <= 0) 00063 { 00064 return res; 00065 } 00066 res = C::encode(obj.c, codec, tao_mode); 00067 if (res <= 0) 00068 { 00069 return res; 00070 } 00071 return 1; 00072 } 00073 00074 static int decode(CustomType& obj, uavcan::ScalarCodec& codec, 00075 uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) 00076 { 00077 int res = 0; 00078 res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); 00079 if (res <= 0) 00080 { 00081 return res; 00082 } 00083 res = B::decode(obj.b, codec, uavcan::TailArrayOptDisabled); 00084 if (res <= 0) 00085 { 00086 return res; 00087 } 00088 res = C::decode(obj.c, codec, tao_mode); 00089 if (res <= 0) 00090 { 00091 return res; 00092 } 00093 return 1; 00094 } 00095 }; 00096 00097 00098 TEST(Array, Basic) 00099 { 00100 typedef Array<IntegerSpec<8, SignednessSigned, CastModeTruncate>, ArrayModeStatic, 4> A1; 00101 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeStatic, 2> A2; 00102 typedef Array<CustomType, ArrayModeStatic, 2> A3; 00103 00104 A1 a1; 00105 A2 a2; 00106 A3 a3; 00107 00108 ASSERT_EQ(1, A3::ValueType::C::RawValueType::BitLen); 00109 00110 ASSERT_EQ(8 * 4, A1::MaxBitLen); 00111 ASSERT_EQ(16 * 2, A2::MaxBitLen); 00112 ASSERT_EQ((8 + 16 + 5 + 3) * 2, A3::MaxBitLen); 00113 00114 /* 00115 * Zero initialization check 00116 */ 00117 ASSERT_FALSE(a1.empty()); 00118 for (A1::const_iterator it = a1.begin(); it != a1.end(); ++it) 00119 { 00120 ASSERT_EQ(0, *it); 00121 } 00122 00123 ASSERT_FALSE(a2.empty()); 00124 for (A2::const_iterator it = a2.begin(); it != a2.end(); ++it) 00125 { 00126 ASSERT_EQ(0, *it); 00127 } 00128 00129 for (A3::const_iterator it = a3.begin(); it != a3.end(); ++it) 00130 { 00131 ASSERT_EQ(0, it->a); 00132 ASSERT_EQ(0, it->b); 00133 ASSERT_EQ(0, it->c.size()); 00134 ASSERT_TRUE(it->c.empty()); 00135 } 00136 00137 /* 00138 * Modification with known values; array lengths are hard coded. 00139 */ 00140 for (uint8_t i = 0; i < 4; i++) 00141 { 00142 a1.at(i) = int8_t(i); 00143 } 00144 for (uint8_t i = 0; i < 2; i++) 00145 { 00146 a2.at(i) = i; 00147 } 00148 for (uint8_t i = 0; i < 2; i++) 00149 { 00150 a3[i].a = int8_t(i); 00151 a3[i].b = i; 00152 for (uint8_t i2 = 0; i2 < 5; i2++) 00153 { 00154 a3[i].c.push_back(i2 & 1); 00155 } 00156 ASSERT_EQ(5, a3[i].c.size()); 00157 ASSERT_FALSE(a3[i].c.empty()); 00158 } 00159 00160 /* 00161 * Representation check 00162 * Note that TAO in A3 is not possible because A3::C has less than one byte per item 00163 */ 00164 uavcan::StaticTransferBuffer<16> buf; 00165 uavcan::BitStream bs_wr(buf); 00166 uavcan::ScalarCodec sc_wr(bs_wr); 00167 00168 ASSERT_EQ(1, A1::encode(a1, sc_wr, uavcan::TailArrayOptDisabled)); 00169 ASSERT_EQ(1, A2::encode(a2, sc_wr, uavcan::TailArrayOptDisabled)); 00170 ASSERT_EQ(1, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); 00171 00172 ASSERT_EQ(0, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); // Out of buffer space 00173 00174 static const std::string Reference = 00175 "00000000 00000001 00000010 00000011 " // A1 (0, 1, 2, 3) 00176 "00000000 00000000 00000000 00111100 " // A2 (0, 1) 00177 "00000000 00000000 00000000 10101010 " // A3[0] (0, 0, bool[5]) 00178 "00000001 00000000 00111100 10101010"; // A3[1] (1, 1, bool[5]) 00179 00180 ASSERT_EQ(Reference, bs_wr.toString()); 00181 00182 /* 00183 * Read back 00184 */ 00185 uavcan::BitStream bs_rd(buf); 00186 uavcan::ScalarCodec sc_rd(bs_rd); 00187 00188 A1 a1_; 00189 A2 a2_; 00190 A3 a3_; 00191 00192 ASSERT_EQ(1, A1::decode(a1_, sc_rd, uavcan::TailArrayOptDisabled)); 00193 ASSERT_EQ(1, A2::decode(a2_, sc_rd, uavcan::TailArrayOptDisabled)); 00194 ASSERT_EQ(1, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); 00195 00196 ASSERT_EQ(a1_, a1); 00197 ASSERT_EQ(a2_, a2); 00198 ASSERT_EQ(a3_, a3); 00199 00200 for (uint8_t i = 0; i < 4; i++) 00201 { 00202 ASSERT_EQ(a1[i], a1_[i]); 00203 } 00204 for (uint8_t i = 0; i < 2; i++) 00205 { 00206 ASSERT_EQ(a2[i], a2_[i]); 00207 } 00208 for (uint8_t i = 0; i < 2; i++) 00209 { 00210 ASSERT_EQ(a3[i].a, a3_[i].a); 00211 ASSERT_EQ(a3[i].b, a3_[i].b); 00212 ASSERT_EQ(a3[i].c, a3_[i].c); 00213 } 00214 00215 ASSERT_EQ(0, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); // Out of buffer space 00216 00217 /* 00218 * STL compatibility 00219 */ 00220 std::vector<int> v1; 00221 v1.push_back(0); 00222 v1.push_back(1); 00223 v1.push_back(2); 00224 v1.push_back(3); 00225 00226 ASSERT_TRUE(a1 == v1); 00227 ASSERT_FALSE(a1 != v1); 00228 ASSERT_TRUE(v1 == a1); 00229 ASSERT_FALSE(v1 != a1); 00230 ASSERT_FALSE(a1 < v1); 00231 00232 v1[0] = 9000; 00233 ASSERT_FALSE(a1 == v1); 00234 ASSERT_TRUE(a1 != v1); 00235 ASSERT_TRUE(a1 < v1); 00236 00237 ASSERT_EQ(0, a1.front()); 00238 ASSERT_EQ(3, a1.back()); 00239 00240 // Boolean vector 00241 std::vector<bool> v2; 00242 v2.push_back(false); 00243 v2.push_back(true); 00244 v2.push_back(false); 00245 v2.push_back(true); 00246 v2.push_back(false); 00247 00248 ASSERT_TRUE(a3[0].c == v2); 00249 ASSERT_FALSE(a3[0].c == v1); 00250 ASSERT_FALSE(a3[0].c != v2); 00251 ASSERT_TRUE(a3[0].c != v1); 00252 00253 v2[0] = true; 00254 ASSERT_TRUE(a3[0].c != v2); 00255 ASSERT_FALSE(a3[0].c == v2); 00256 } 00257 00258 00259 TEST(Array, Dynamic) 00260 { 00261 typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> A; 00262 typedef Array<IntegerSpec<8, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 255> B; 00263 00264 A a; 00265 B b; 00266 B b2; 00267 00268 ASSERT_EQ(3 + 5, A::MaxBitLen); 00269 ASSERT_EQ(8 + 255 * 8, B::MaxBitLen); 00270 00271 ASSERT_TRUE(a.empty()); 00272 ASSERT_TRUE(b.empty()); 00273 ASSERT_TRUE(b2.empty()); 00274 00275 { 00276 uavcan::StaticTransferBuffer<16> buf; 00277 uavcan::BitStream bs_wr(buf); 00278 uavcan::ScalarCodec sc_wr(bs_wr); 00279 00280 ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); 00281 ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); 00282 ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); 00283 00284 ASSERT_EQ("000" "00000 000" "00000", bs_wr.toString()); // Last array was optimized away completely 00285 00286 uavcan::BitStream bs_rd(buf); 00287 uavcan::ScalarCodec sc_rd(bs_rd); 00288 00289 ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); 00290 ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); 00291 ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); 00292 00293 ASSERT_TRUE(a.empty()); 00294 ASSERT_TRUE(b.empty()); 00295 ASSERT_TRUE(b2.empty()); 00296 } 00297 00298 a.push_back(true); 00299 a.push_back(false); 00300 a.push_back(true); 00301 a.push_back(false); 00302 a.push_back(true); 00303 00304 b.push_back(42); 00305 b.push_back(-42); 00306 00307 b2.push_back(123); 00308 b2.push_back(72); 00309 00310 { 00311 uavcan::StaticTransferBuffer<16> buf; 00312 uavcan::BitStream bs_wr(buf); 00313 uavcan::ScalarCodec sc_wr(bs_wr); 00314 00315 ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); 00316 ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); 00317 ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); // No length field 00318 00319 // A B len B[0] B[1] B2[0] B2[1] 00320 ASSERT_EQ("10110101 00000010 00101010 11010110 01111011 01001000", bs_wr.toString()); 00321 00322 uavcan::BitStream bs_rd(buf); 00323 uavcan::ScalarCodec sc_rd(bs_rd); 00324 00325 a.clear(); 00326 b.clear(); 00327 b2.clear(); 00328 ASSERT_TRUE(a.empty()); 00329 ASSERT_TRUE(b.empty()); 00330 ASSERT_TRUE(b2.empty()); 00331 00332 ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); 00333 ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); 00334 ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); 00335 00336 ASSERT_EQ(5, a.size()); 00337 ASSERT_EQ(2, b.size()); 00338 ASSERT_EQ(2, b2.size()); 00339 00340 ASSERT_TRUE(a[0]); 00341 ASSERT_FALSE(a[1]); 00342 ASSERT_TRUE(a[2]); 00343 ASSERT_FALSE(a[3]); 00344 ASSERT_TRUE(a[4]); 00345 00346 ASSERT_EQ(42, b[0]); 00347 ASSERT_EQ(-42, b[1]); 00348 00349 ASSERT_EQ(123, b2[0]); 00350 ASSERT_EQ(72, b2[1]); 00351 } 00352 00353 ASSERT_FALSE(a == b); 00354 ASSERT_FALSE(b == a); 00355 ASSERT_TRUE(a != b); 00356 ASSERT_TRUE(b != a); 00357 00358 a.resize(0); 00359 b.resize(0); 00360 ASSERT_TRUE(a.empty()); 00361 ASSERT_TRUE(b.empty()); 00362 00363 a.resize(5, true); 00364 b.resize(255, 72); 00365 ASSERT_EQ(5, a.size()); 00366 ASSERT_EQ(255, b.size()); 00367 00368 for (uint8_t i = 0; i < 5; i++) 00369 { 00370 ASSERT_TRUE(a[i]); 00371 } 00372 for (uint8_t i = 0; i < 255; i++) 00373 { 00374 ASSERT_EQ(72, b[i]); 00375 } 00376 } 00377 00378 00379 template <typename B> 00380 struct CustomType2 00381 { 00382 typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> A; 00383 00384 enum { MinBitLen = A::MinBitLen + B::MinBitLen }; 00385 enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen }; 00386 00387 typename uavcan::StorageType<A>::Type a; 00388 typename uavcan::StorageType<B>::Type b; 00389 00390 CustomType2() 00391 : a() 00392 , b() 00393 { } 00394 00395 bool operator==(const CustomType2& rhs) const 00396 { 00397 return uavcan::areFloatsExactlyEqual(a, rhs.a) && 00398 b == rhs.b; 00399 } 00400 00401 static int encode(const CustomType2& obj, uavcan::ScalarCodec& codec, 00402 uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) 00403 { 00404 int res = 0; 00405 res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); 00406 if (res <= 0) 00407 { 00408 return res; 00409 } 00410 res = B::encode(obj.b, codec, tao_mode); 00411 if (res <= 0) 00412 { 00413 return res; 00414 } 00415 return 1; 00416 } 00417 00418 static int decode(CustomType2& obj, uavcan::ScalarCodec& codec, 00419 uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) 00420 { 00421 int res = 0; 00422 res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); 00423 if (res <= 0) 00424 { 00425 return res; 00426 } 00427 res = B::decode(obj.b, codec, tao_mode); 00428 if (res <= 0) 00429 { 00430 return res; 00431 } 00432 return 1; 00433 } 00434 }; 00435 00436 00437 template <typename T> 00438 static std::string runEncodeDecode(const typename uavcan::StorageType<T>::Type& value, 00439 const uavcan::TailArrayOptimizationMode tao_mode) 00440 { 00441 uavcan::StaticTransferBuffer<(T::MaxBitLen + 7) / 8> buf; 00442 uavcan::BitStream bs_wr(buf); 00443 uavcan::ScalarCodec sc_wr(bs_wr); 00444 EXPECT_EQ(1, T::encode(value, sc_wr, tao_mode)); 00445 00446 typename uavcan::StorageType<T>::Type value2 = typename uavcan::StorageType<T>::Type(); 00447 // Decode multiple times to make sure that the decoded type is being correctly de-initialized 00448 for (int i = 0; i < 3; i++) 00449 { 00450 uavcan::BitStream bs_rd(buf); 00451 uavcan::ScalarCodec sc_rd(bs_rd); 00452 EXPECT_EQ(1, T::decode(value2, sc_rd, tao_mode)); 00453 EXPECT_EQ(value, value2); 00454 } 00455 return bs_wr.toString(); 00456 } 00457 00458 00459 TEST(Array, TailArrayOptimization) 00460 { 00461 typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> OneBitArray; 00462 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 255> EightBitArray; 00463 typedef CustomType2<Array<OneBitArray, ArrayModeDynamic, 255> > A; 00464 typedef CustomType2<Array<EightBitArray, ArrayModeDynamic, 255> > B; 00465 typedef CustomType2<EightBitArray> C; 00466 00467 A a; 00468 B b; 00469 C c; 00470 00471 /* 00472 * Empty 00473 */ 00474 // a LSB a MSB b len 00475 ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled)); 00476 ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled)); 00477 00478 // a LSB a MSB b len 00479 ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<B>(b, uavcan::TailArrayOptEnabled)); 00480 ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<B>(b, uavcan::TailArrayOptDisabled)); 00481 00482 // a LSB a MSB 00483 ASSERT_EQ("00000000 00000000", runEncodeDecode<C>(c, uavcan::TailArrayOptEnabled)); 00484 ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode<C>(c, uavcan::TailArrayOptDisabled)); 00485 00486 /* 00487 * A 00488 */ 00489 a.b.resize(2); 00490 a.b[0].push_back(true); 00491 a.b[0].push_back(false); 00492 // a.b[1] remains empty 00493 // a LSB a MSB b len b: len(2), 1, 0, len(0) 00494 ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled)); 00495 ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled)); 00496 00497 /* 00498 * B 00499 */ 00500 b.b.resize(3); 00501 b.b[0].push_back(42); 00502 b.b[0].push_back(72); 00503 // b.b[1] remains empty 00504 b.b[2].push_back(123); 00505 b.b[2].push_back(99); 00506 // a LSB a MSB b len b[0]len 42 72 b[1]len 123 99 (b[2] len optimized out) 00507 ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 01111011 01100011", 00508 runEncodeDecode<B>(b, uavcan::TailArrayOptEnabled)); 00509 // Same as above, but b[2] len is present v here v 00510 ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 00000010 01111011 01100011", 00511 runEncodeDecode<B>(b, uavcan::TailArrayOptDisabled)); 00512 00513 /* 00514 * C 00515 */ 00516 c.a = 1; 00517 c.b.push_back(1); 00518 c.b.push_back(2); 00519 c.b.push_back(3); 00520 // a LSB a MSB 1 2 3 00521 ASSERT_EQ("00000000 00111100 00000001 00000010 00000011", 00522 runEncodeDecode<C>(c, uavcan::TailArrayOptEnabled)); 00523 // a LSB a MSB b len 1 2 3 00524 ASSERT_EQ("00000000 00111100 00000011 00000001 00000010 00000011", 00525 runEncodeDecode<C>(c, uavcan::TailArrayOptDisabled)); 00526 } 00527 00528 00529 TEST(Array, TailArrayOptimizationErrors) 00530 { 00531 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> A; 00532 00533 A a; 00534 ASSERT_TRUE(a.empty()); 00535 ASSERT_EQ("", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled)); 00536 ASSERT_EQ("00000000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled)); 00537 00538 // Correct decode/encode 00539 a.push_back(1); 00540 a.push_back(126); 00541 a.push_back(5); 00542 ASSERT_FALSE(a.empty()); 00543 ASSERT_EQ("00000001 01111110 00000101", runEncodeDecode<A>(a, uavcan::TailArrayOptEnabled)); 00544 ASSERT_EQ("01100000 00101111 11000000 10100000", runEncodeDecode<A>(a, uavcan::TailArrayOptDisabled)); 00545 00546 // Invalid decode - length field is out of range 00547 uavcan::StaticTransferBuffer<7> buf; 00548 uavcan::BitStream bs_wr(buf); 00549 uavcan::ScalarCodec sc_wr(bs_wr); 00550 00551 ASSERT_EQ(1, sc_wr.encode<3>(uint8_t(6))); // Length - more than 5 items, error 00552 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(42))); 00553 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(72))); 00554 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(126))); 00555 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(1))); 00556 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(2))); 00557 ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(3))); // Out of range - only 5 items allowed 00558 00559 // 197 73 15 192 32 ... 00560 ASSERT_EQ("11000101 01001001 00001111 11000000 00100000 01000000 01100000", bs_wr.toString()); 00561 00562 { 00563 uavcan::BitStream bs_rd(buf); 00564 uavcan::ScalarCodec sc_rd(bs_rd); 00565 A a2; 00566 a2.push_back(56); // Garbage 00567 ASSERT_EQ(1, a2.size()); 00568 // Will fail - declared length is more than 5 items 00569 ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptDisabled)); 00570 // Must be cleared 00571 ASSERT_TRUE(a2.empty()); 00572 } 00573 { 00574 uavcan::BitStream bs_rd(buf); 00575 uavcan::ScalarCodec sc_rd(bs_rd); 00576 A a2; 00577 a2.push_back(56); // Garbage 00578 ASSERT_EQ(1, a2.size()); 00579 // Will fail - no length field, but the stream is too long 00580 ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptEnabled)); 00581 // Will contain some garbage 00582 ASSERT_EQ(5, a2.size()); 00583 // Interpreted stream - see the values above 00584 ASSERT_EQ(197, a2[0]); 00585 ASSERT_EQ(73, a2[1]); 00586 ASSERT_EQ(15, a2[2]); 00587 ASSERT_EQ(192, a2[3]); 00588 ASSERT_EQ(32, a2[4]); 00589 } 00590 } 00591 00592 00593 TEST(Array, DynamicEncodeDecodeErrors) 00594 { 00595 typedef CustomType2<Array<Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, 00596 ArrayModeDynamic, 255>, 00597 ArrayModeDynamic, 255> > A; 00598 A a; 00599 a.b.resize(2); 00600 a.b[0].push_back(55); 00601 a.b[0].push_back(66); 00602 { 00603 uavcan::StaticTransferBuffer<4> buf; 00604 uavcan::BitStream bs_wr(buf); 00605 uavcan::ScalarCodec sc_wr(bs_wr); 00606 ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptEnabled)); // Not enough buffer space 00607 00608 uavcan::BitStream bs_rd(buf); 00609 uavcan::ScalarCodec sc_rd(bs_rd); 00610 ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); 00611 } 00612 { 00613 uavcan::StaticTransferBuffer<4> buf; 00614 uavcan::BitStream bs_wr(buf); 00615 uavcan::ScalarCodec sc_wr(bs_wr); 00616 ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); // Not enough buffer space 00617 00618 uavcan::BitStream bs_rd(buf); 00619 uavcan::ScalarCodec sc_rd(bs_rd); 00620 ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); 00621 } 00622 } 00623 00624 00625 TEST(Array, StaticEncodeDecodeErrors) 00626 { 00627 typedef CustomType2<Array<Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, 00628 ArrayModeStatic, 2>, 00629 ArrayModeStatic, 2> > A; 00630 A a; 00631 a.a = 1.0; 00632 a.b[0][0] = 0x11; 00633 a.b[0][1] = 0x22; 00634 a.b[1][0] = 0x33; 00635 a.b[1][1] = 0x44; 00636 { // Just enough buffer space - 6 bytes 00637 uavcan::StaticTransferBuffer<6> buf; 00638 uavcan::BitStream bs_wr(buf); 00639 uavcan::ScalarCodec sc_wr(bs_wr); 00640 ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); 00641 00642 ASSERT_EQ("00000000 00111100 00010001 00100010 00110011 01000100", bs_wr.toString()); 00643 00644 uavcan::BitStream bs_rd(buf); 00645 uavcan::ScalarCodec sc_rd(bs_rd); 00646 ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); 00647 } 00648 { // Not enough space 00649 uavcan::StaticTransferBuffer<5> buf; 00650 uavcan::BitStream bs_wr(buf); 00651 uavcan::ScalarCodec sc_wr(bs_wr); 00652 ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); 00653 00654 ASSERT_EQ("00000000 00111100 00010001 00100010 00110011", bs_wr.toString()); 00655 00656 uavcan::BitStream bs_rd(buf); 00657 uavcan::ScalarCodec sc_rd(bs_rd); 00658 ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); 00659 } 00660 } 00661 00662 00663 TEST(Array, Copyability) 00664 { 00665 typedef Array<IntegerSpec<1, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 5> OneBitArray; 00666 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 255> EightBitArray; 00667 typedef Array<OneBitArray, ArrayModeDynamic, 255> A; 00668 typedef Array<EightBitArray, ArrayModeDynamic, 255> B; 00669 typedef EightBitArray C; 00670 00671 A a; 00672 B b; 00673 C c; 00674 00675 A a2 = a; 00676 B b2 = b; 00677 C c2 = c; 00678 00679 ASSERT_TRUE(a == a2); 00680 ASSERT_TRUE(b == b2); 00681 ASSERT_TRUE(c == c2); 00682 00683 a.push_back(OneBitArray()); 00684 b.push_back(EightBitArray()); 00685 c.push_back(42); 00686 00687 ASSERT_TRUE(a != a2); 00688 ASSERT_TRUE(b != b2); 00689 ASSERT_TRUE(c != c2); 00690 00691 a2 = a; 00692 b2 = b; 00693 c2 = c; 00694 00695 ASSERT_TRUE(a2 == a); 00696 ASSERT_TRUE(b2 == b); 00697 ASSERT_TRUE(c2 == c); 00698 } 00699 00700 00701 TEST(Array, Appending) 00702 { 00703 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 2> A; 00704 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 257> B; 00705 A a; 00706 B b; 00707 00708 a.push_back(1); 00709 a.push_back(2); 00710 a += b; 00711 ASSERT_EQ(2, a.size()); 00712 ASSERT_EQ(1, a[0]); 00713 ASSERT_EQ(2, a[1]); 00714 00715 b += a; 00716 ASSERT_TRUE(b == a); 00717 b += a; 00718 ASSERT_EQ(4, b.size()); 00719 ASSERT_EQ(1, b[0]); 00720 ASSERT_EQ(2, b[1]); 00721 ASSERT_EQ(1, b[2]); 00722 ASSERT_EQ(2, b[3]); 00723 } 00724 00725 00726 TEST(Array, Strings) 00727 { 00728 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A8; 00729 typedef Array<IntegerSpec<7, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A7; 00730 00731 A8 a8; 00732 A8 a8_2; 00733 A7 a7; 00734 00735 ASSERT_TRUE(a8 == a7); 00736 // cppcheck-suppress duplicateExpression 00737 ASSERT_TRUE(a8 == a8); 00738 // cppcheck-suppress duplicateExpression 00739 ASSERT_TRUE(a7 == a7); 00740 ASSERT_TRUE(a8 == ""); 00741 ASSERT_TRUE(a7 == ""); 00742 00743 a8 = "Hello world!"; 00744 a7 = "123"; 00745 ASSERT_TRUE(a8 == "Hello world!"); 00746 ASSERT_TRUE(a7 == "123"); 00747 00748 a8 = "Our sun is dying."; 00749 a7 = "456"; 00750 ASSERT_TRUE("Our sun is dying." == a8); 00751 ASSERT_TRUE("456" == a7); 00752 00753 a8 += " 123456"; 00754 a8 += "-789"; 00755 ASSERT_TRUE("Our sun is dying. 123456-789" == a8); 00756 00757 ASSERT_TRUE(a8_2 == ""); 00758 ASSERT_TRUE(a8_2.empty()); 00759 ASSERT_TRUE(a8_2 != a8); 00760 a8_2 = a8; 00761 ASSERT_TRUE(a8_2 == "Our sun is dying. 123456-789"); 00762 ASSERT_TRUE(a8_2 == a8); 00763 00764 /* 00765 * c_str() 00766 */ 00767 ASSERT_STREQ("", A8().c_str()); 00768 ASSERT_STREQ("", A7().c_str()); 00769 ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str()); 00770 ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str()); 00771 ASSERT_STREQ("456", a7.c_str()); 00772 00773 /* 00774 * String constructor 00775 */ 00776 A8 a8_3("123"); 00777 A7 a7_3 = "456"; 00778 ASSERT_EQ(3, a8_3.size()); 00779 ASSERT_EQ(3, a7_3.size()); 00780 ASSERT_STREQ("123", a8_3.c_str()); 00781 ASSERT_STREQ("456", a7_3.c_str()); 00782 } 00783 00784 00785 TEST(Array, AppendFormatted) 00786 { 00787 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 45> A8; 00788 00789 A8 a; 00790 00791 ASSERT_TRUE("" == a); 00792 00793 a.appendFormatted("%4.1f", 12.3); // 4 00794 a += " "; // 1 00795 a.appendFormatted("%li", -123456789L); // 10 00796 a.appendFormatted("%s", " TOTAL PERSPECTIVE VORTEX "); // 26 00797 a.appendFormatted("0x%X", 0xDEADBEEF); // 10 --> 4 00798 00799 ASSERT_STREQ("12.3 -123456789 TOTAL PERSPECTIVE VORTEX 0xDE", a.c_str()); 00800 } 00801 00802 00803 TEST(Array, FlatStreaming) 00804 { 00805 typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, 32> A8D; 00806 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 16> AF16D; 00807 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeStatic, 3> AF16S; 00808 00809 A8D a1; 00810 a1 = "12\n3\x44\xa5\xde\xad\x79"; 00811 uavcan::YamlStreamer<A8D>::stream(std::cout, a1, 0); 00812 std::cout << std::endl; 00813 00814 A8D a2; 00815 a2 = "Hello"; 00816 uavcan::YamlStreamer<A8D>::stream(std::cout, a2, 0); 00817 std::cout << std::endl; 00818 00819 AF16D af16d1; 00820 af16d1.push_back(1.23F); 00821 af16d1.push_back(4.56F); 00822 uavcan::YamlStreamer<AF16D>::stream(std::cout, af16d1, 0); 00823 std::cout << std::endl; 00824 00825 AF16D af16d2; 00826 uavcan::YamlStreamer<AF16D>::stream(std::cout, af16d2, 0); 00827 std::cout << std::endl; 00828 00829 AF16S af16s; 00830 uavcan::YamlStreamer<AF16S>::stream(std::cout, af16s, 0); 00831 std::cout << std::endl; 00832 } 00833 00834 00835 TEST(Array, MultidimensionalStreaming) 00836 { 00837 typedef Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 16> Float16Array; 00838 typedef Array<Float16Array, ArrayModeDynamic, 8> TwoDimensional; 00839 typedef Array<TwoDimensional, ArrayModeDynamic, 4> ThreeDimensional; 00840 00841 ThreeDimensional threedee; 00842 threedee.resize(3); 00843 for (uint8_t x = 0; x < threedee.size(); x++) 00844 { 00845 threedee[x].resize(3); 00846 for (uint8_t y = 0; y < threedee[x].size(); y++) 00847 { 00848 threedee[x][y].resize(3); 00849 for (uint8_t z = 0; z < threedee[x][y].size(); z++) 00850 { 00851 threedee[x][y][z] = 1.0F / (float(x + y + z) + 1.0F); 00852 } 00853 } 00854 } 00855 00856 uavcan::YamlStreamer<ThreeDimensional>::stream(std::cout, threedee, 0); 00857 std::cout << std::endl; 00858 } 00859 00860 00861 TEST(Array, SquareMatrixPacking) 00862 { 00863 Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s; 00864 Array<FloatSpec<32, CastModeSaturate>, ArrayModeDynamic, 4> m2x2f; 00865 Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d; 00866 00867 // NAN will be reduced to empty array 00868 { 00869 const double nans3x3[] = 00870 { 00871 NAN, NAN, NAN, 00872 NAN, NAN, NAN, 00873 NAN, NAN, NAN 00874 }; 00875 m3x3s.packSquareMatrix(nans3x3); 00876 ASSERT_EQ(0, m3x3s.size()); 00877 00878 // Empty array will be decoded as zero matrix 00879 double nans3x3_out[9]; 00880 m3x3s.unpackSquareMatrix(nans3x3_out); 00881 for (uint8_t i = 0; i < 9; i++) 00882 { 00883 ASSERT_DOUBLE_EQ(0, nans3x3_out[i]); 00884 } 00885 } 00886 { 00887 std::vector<double> empty; 00888 m3x3s.packSquareMatrix(empty); 00889 ASSERT_EQ(0, m3x3s.size()); 00890 00891 empty.resize(9); 00892 m3x3s.unpackSquareMatrix(empty); 00893 for (uint8_t i = 0; i < 9; i++) 00894 { 00895 ASSERT_DOUBLE_EQ(0, empty.at(i)); 00896 } 00897 } 00898 00899 // Scalar matrix will be reduced to a single value 00900 { 00901 std::vector<float> scalar2x2(4); 00902 scalar2x2[0] = scalar2x2[3] = 3.14F; 00903 m2x2f.packSquareMatrix(scalar2x2); 00904 ASSERT_EQ(1, m2x2f.size()); 00905 ASSERT_FLOAT_EQ(3.14F, m2x2f[0]); 00906 00907 m2x2f.unpackSquareMatrix(scalar2x2); 00908 const float reference[] = 00909 { 00910 3.14F, 0.0F, 00911 0.0F, 3.14F 00912 }; 00913 ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference)); 00914 } 00915 { 00916 const float scalar6x6[] = 00917 { 00918 -18, 0, 0, 0, 0, 0, 00919 0, -18, 0, 0, 0, 0, 00920 0, 0, -18, 0, 0, 0, 00921 0, 0, 0, -18, 0, 0, 00922 0, 0, 0, 0, -18, 0, 00923 0, 0, 0, 0, 0, -18 00924 }; 00925 m6x6d.packSquareMatrix(scalar6x6); 00926 ASSERT_EQ(1, m6x6d.size()); 00927 ASSERT_DOUBLE_EQ(-18, m6x6d[0]); 00928 00929 std::vector<long double> output(36); 00930 m6x6d.unpackSquareMatrix(output); 00931 ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6)); 00932 } 00933 00934 // Diagonal matrix will be reduced to an array of length Width 00935 { 00936 const float diagonal6x6[] = 00937 { 00938 1, 0, 0, 0, 0, 0, 00939 0, -2, 0, 0, 0, 0, 00940 0, 0, 3, 0, 0, 0, 00941 0, 0, 0, -4, 0, 0, 00942 0, 0, 0, 0, 5, 0, 00943 0, 0, 0, 0, 0, -6 00944 }; 00945 m6x6d.packSquareMatrix(diagonal6x6); 00946 ASSERT_EQ(6, m6x6d.size()); 00947 ASSERT_DOUBLE_EQ(1, m6x6d[0]); 00948 ASSERT_DOUBLE_EQ(-2, m6x6d[1]); 00949 ASSERT_DOUBLE_EQ(3, m6x6d[2]); 00950 ASSERT_DOUBLE_EQ(-4, m6x6d[3]); 00951 ASSERT_DOUBLE_EQ(5, m6x6d[4]); 00952 ASSERT_DOUBLE_EQ(-6, m6x6d[5]); 00953 00954 std::vector<long double> output(36); 00955 m6x6d.unpackSquareMatrix(output); 00956 ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6)); 00957 } 00958 00959 // A matrix filled with random values will not be compressed 00960 { 00961 std::vector<float> full3x3(9); 00962 for (uint8_t i = 0; i < 9; i++) 00963 { 00964 full3x3[i] = float(i); 00965 } 00966 m3x3s.packSquareMatrix(full3x3); 00967 ASSERT_EQ(9, m3x3s.size()); 00968 for (uint8_t i = 0; i < 9; i++) 00969 { 00970 ASSERT_FLOAT_EQ(float(i), m3x3s[i]); 00971 } 00972 00973 long output[9]; 00974 m3x3s.unpackSquareMatrix(output); 00975 ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output)); 00976 } 00977 00978 // This will be represented as diagonal - NANs are exceptional 00979 { 00980 const double scalarnan3x3[] = 00981 { 00982 NAN, 0, 0, 00983 0, NAN, 0, 00984 0, 0, NAN 00985 }; 00986 m3x3s.packSquareMatrix(scalarnan3x3); 00987 ASSERT_EQ(3, m3x3s.size()); 00988 ASSERT_FALSE(m3x3s[0] <= m3x3s[0]); // NAN 00989 ASSERT_FALSE(m3x3s[1] <= m3x3s[1]); // NAN 00990 ASSERT_FALSE(m3x3s[2] <= m3x3s[2]); // NAN 00991 00992 float output[9]; 00993 m3x3s.unpackSquareMatrix(output); 00994 ASSERT_FALSE(output[0] <= output[0]); // NAN 00995 ASSERT_EQ(0, output[1]); 00996 ASSERT_EQ(0, output[2]); 00997 ASSERT_EQ(0, output[3]); 00998 ASSERT_FALSE(output[4] <= output[4]); // NAN 00999 ASSERT_EQ(0, output[5]); 01000 ASSERT_EQ(0, output[6]); 01001 ASSERT_EQ(0, output[7]); 01002 ASSERT_FALSE(output[8] <= output[8]); // NAN 01003 } 01004 01005 // This is a full matrix too (notice the NAN) 01006 { 01007 const float full2x2[] = 01008 { 01009 1, NAN, 01010 0, -2 01011 }; 01012 m2x2f.packSquareMatrix(full2x2); 01013 ASSERT_EQ(4, m2x2f.size()); 01014 ASSERT_FLOAT_EQ(1, m2x2f[0]); 01015 ASSERT_FALSE(m2x2f[1] <= m2x2f[1]); // NAN 01016 ASSERT_FLOAT_EQ(0, m2x2f[2]); 01017 ASSERT_FLOAT_EQ(-2, m2x2f[3]); 01018 01019 float output[4]; 01020 m2x2f.unpackSquareMatrix(output); 01021 ASSERT_EQ(1, output[0]); 01022 ASSERT_FALSE(output[1] <= output[1]); // NAN 01023 ASSERT_EQ(0, output[2]); 01024 ASSERT_EQ(-2, output[3]); 01025 } 01026 01027 // Zero matrix will be represented as scalar matrix 01028 { 01029 const float zero2x2[] = 01030 { 01031 0, 0, 01032 0, 0 01033 }; 01034 m2x2f.packSquareMatrix(zero2x2); 01035 ASSERT_EQ(1, m2x2f.size()); 01036 ASSERT_FLOAT_EQ(0, m2x2f[0]); 01037 } 01038 01039 // Symmetric matrix will contain only upper-right triangle 01040 { 01041 const float sym2x2[] = 01042 { 01043 1, 2, 01044 2, 1 01045 }; 01046 m2x2f.packSquareMatrix(sym2x2); 01047 ASSERT_EQ(3, m2x2f.size()); 01048 01049 float sym2x2_out[4]; 01050 m2x2f.unpackSquareMatrix(sym2x2_out); 01051 ASSERT_FLOAT_EQ(1, sym2x2_out[0]); 01052 ASSERT_FLOAT_EQ(2, sym2x2_out[1]); 01053 ASSERT_FLOAT_EQ(2, sym2x2_out[2]); 01054 ASSERT_FLOAT_EQ(1, sym2x2_out[3]); 01055 } 01056 { 01057 const float sym3x3[] = 01058 { 01059 1, 2, 3, 01060 2, 4, 5, 01061 3, 5, 6 01062 }; 01063 m3x3s.packSquareMatrix(sym3x3); 01064 ASSERT_EQ(6, m3x3s.size()); 01065 ASSERT_EQ(1, m3x3s[0]); 01066 ASSERT_EQ(2, m3x3s[1]); 01067 ASSERT_EQ(3, m3x3s[2]); 01068 ASSERT_EQ(4, m3x3s[3]); 01069 ASSERT_EQ(5, m3x3s[4]); 01070 ASSERT_EQ(6, m3x3s[5]); 01071 01072 float sym3x3_out[9]; 01073 m3x3s.unpackSquareMatrix(sym3x3_out); 01074 01075 for (int i = 0; i < 9; i++) 01076 { 01077 ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]); 01078 } 01079 } 01080 { 01081 const double sym6x6[] = 01082 { 01083 1, 2, 3, 4, 5, 6, 01084 2, 7, 8, 9, 10, 11, 01085 3, 8, 12, 13, 14, 15, 01086 4, 9, 13, 16, 17, 18, 01087 5, 10, 14, 17, 19, 20, 01088 6, 11, 15, 18, 20, 21 01089 }; 01090 m6x6d.packSquareMatrix(sym6x6); 01091 ASSERT_EQ(21, m6x6d.size()); 01092 for (uavcan::uint8_t i = 0; i < 21; i++) 01093 { 01094 ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]); 01095 } 01096 01097 double sym6x6_out[36]; 01098 m6x6d.unpackSquareMatrix(sym6x6_out); 01099 01100 for (int i = 0; i < 36; i++) 01101 { 01102 ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]); 01103 } 01104 } 01105 } 01106 01107 01108 TEST(Array, FuzzySquareMatrixPacking) 01109 { 01110 Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 36> m6x6d; 01111 01112 // Diagonal matrix will be reduced to an array of length Width 01113 { 01114 float diagonal6x6[] = 01115 { 01116 1, 0, 0, 0, 0, 0, 01117 0, -2, 0, 0, 0, 0, 01118 0, 0, 3, 0, 0, 0, 01119 0, 0, 0, -4, 0, 0, 01120 0, 0, 0, 0, 5, 0, 01121 0, 0, 0, 0, 0, -6 01122 }; 01123 01124 // Some almost-zeroes 01125 diagonal6x6[1] = std::numeric_limits<float>::epsilon(); 01126 diagonal6x6[4] = -std::numeric_limits<float>::epsilon(); 01127 diagonal6x6[34] = -std::numeric_limits<float>::epsilon(); 01128 01129 m6x6d.packSquareMatrix(diagonal6x6); 01130 ASSERT_EQ(6, m6x6d.size()); 01131 ASSERT_DOUBLE_EQ(1, m6x6d[0]); 01132 ASSERT_DOUBLE_EQ(-2, m6x6d[1]); 01133 ASSERT_DOUBLE_EQ(3, m6x6d[2]); 01134 ASSERT_DOUBLE_EQ(-4, m6x6d[3]); 01135 ASSERT_DOUBLE_EQ(5, m6x6d[4]); 01136 ASSERT_DOUBLE_EQ(-6, m6x6d[5]); 01137 01138 std::vector<long double> output(36); 01139 m6x6d.unpackSquareMatrix(output); 01140 01141 // This comparison will fail because epsilons 01142 ASSERT_FALSE(std::equal(output.begin(), output.end(), diagonal6x6)); 01143 01144 // This comparison will be ok 01145 ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6, &uavcan::areClose<float, float>)); 01146 } 01147 } 01148 01149 01150 TEST(Array, SquareMatrixPackingIntegers) 01151 { 01152 Array<IntegerSpec<30, SignednessSigned, CastModeSaturate>, ArrayModeDynamic, 9> m3x3int; 01153 { 01154 const long scalar[] = 01155 { 01156 42, 0, 0, 01157 0, 42, 0, 01158 0, 0, 42 01159 }; 01160 m3x3int.packSquareMatrix(scalar); 01161 ASSERT_EQ(1, m3x3int.size()); 01162 ASSERT_EQ(42, m3x3int[0]); 01163 01164 std::vector<int> output(9); 01165 m3x3int.unpackSquareMatrix(output); 01166 ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar)); 01167 } 01168 { 01169 std::vector<short> diagonal(9); 01170 diagonal[0] = 6; 01171 diagonal[4] = -57; 01172 diagonal[8] = 1139; 01173 m3x3int.packSquareMatrix(diagonal); 01174 ASSERT_EQ(3, m3x3int.size()); 01175 ASSERT_EQ(6, m3x3int[0]); 01176 ASSERT_EQ(-57, m3x3int[1]); 01177 ASSERT_EQ(1139, m3x3int[2]); 01178 } 01179 { 01180 std::vector<long double> full(9); 01181 for (uint8_t i = 0; i < 9; i++) 01182 { 01183 full[i] = i; 01184 } 01185 m3x3int.packSquareMatrix(full); 01186 ASSERT_EQ(9, m3x3int.size()); 01187 for (uint8_t i = 0; i < 9; i++) 01188 { 01189 ASSERT_EQ(i, m3x3int[i]); 01190 } 01191 } 01192 } 01193 01194 #if UAVCAN_EXCEPTIONS 01195 TEST(Array, SquareMatrixPackingErrors) 01196 { 01197 Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s; 01198 01199 std::vector<float> ill_formed_row_major(8); 01200 ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range); 01201 01202 ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range); 01203 } 01204 #endif 01205 01206 TEST(Array, SquareMatrixPackingInPlace) 01207 { 01208 Array<FloatSpec<16, CastModeSaturate>, ArrayModeDynamic, 9> m3x3s; 01209 01210 // Will do nothing - matrix is empty 01211 m3x3s.packSquareMatrix(); 01212 ASSERT_TRUE(m3x3s.empty()); 01213 01214 // Will fill with zeros - matrix is empty 01215 m3x3s.unpackSquareMatrix(); 01216 ASSERT_EQ(9, m3x3s.size()); 01217 for (uint8_t i = 0; i < 9; i++) 01218 { 01219 ASSERT_EQ(0, m3x3s[i]); 01220 } 01221 01222 // Fill an unpackaple matrix 01223 m3x3s.clear(); 01224 m3x3s.push_back(11); 01225 m3x3s.push_back(12); 01226 m3x3s.push_back(13); 01227 01228 #if UAVCAN_EXCEPTIONS 01229 // Shall throw - matrix is ill-formed 01230 ASSERT_THROW(m3x3s.packSquareMatrix(), std::out_of_range); 01231 #endif 01232 01233 m3x3s.push_back(21); 01234 m3x3s.push_back(22); 01235 m3x3s.push_back(23); 01236 m3x3s.push_back(31); 01237 m3x3s.push_back(32); 01238 m3x3s.push_back(33); 01239 01240 // Will pack/unpack successfully 01241 ASSERT_EQ(9, m3x3s.size()); 01242 m3x3s.packSquareMatrix(); 01243 ASSERT_EQ(9, m3x3s.size()); 01244 m3x3s.unpackSquareMatrix(); 01245 01246 // Make sure it was unpacked properly 01247 ASSERT_EQ(11, m3x3s[0]); 01248 ASSERT_EQ(12, m3x3s[1]); 01249 ASSERT_EQ(13, m3x3s[2]); 01250 ASSERT_EQ(21, m3x3s[3]); 01251 ASSERT_EQ(22, m3x3s[4]); 01252 ASSERT_EQ(23, m3x3s[5]); 01253 ASSERT_EQ(31, m3x3s[6]); 01254 ASSERT_EQ(32, m3x3s[7]); 01255 ASSERT_EQ(33, m3x3s[8]); 01256 01257 // Try again with a scalar matrix 01258 m3x3s.clear(); 01259 for (unsigned i = 0; i < 9; i++) 01260 { 01261 const bool diagonal = (i == 0) || (i == 4) || (i == 8); 01262 m3x3s.push_back(diagonal ? 123 : 0); 01263 } 01264 01265 ASSERT_EQ(9, m3x3s.size()); 01266 m3x3s.packSquareMatrix(); 01267 ASSERT_EQ(1, m3x3s.size()); 01268 m3x3s.unpackSquareMatrix(); 01269 ASSERT_EQ(9, m3x3s.size()); 01270 01271 for (uint8_t i = 0; i < 9; i++) 01272 { 01273 const bool diagonal = (i == 0) || (i == 4) || (i == 8); 01274 ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]); 01275 } 01276 01277 // Try again with symmetric matrix 01278 /* 01279 * Full matrix: 01280 * 1 2 3 01281 * 2 4 5 01282 * 3 5 6 01283 * Compressed triangle: 01284 * 1 2 3 01285 * 4 5 01286 * 6 01287 */ 01288 m3x3s.clear(); 01289 m3x3s.push_back(1); 01290 m3x3s.push_back(2); 01291 m3x3s.push_back(3); 01292 m3x3s.push_back(4); 01293 m3x3s.push_back(5); 01294 m3x3s.push_back(6); 01295 // Unpacking 01296 ASSERT_EQ(6, m3x3s.size()); 01297 m3x3s.unpackSquareMatrix(); 01298 ASSERT_EQ(9, m3x3s.size()); 01299 // Validating 01300 ASSERT_EQ(1, m3x3s[0]); 01301 ASSERT_EQ(2, m3x3s[1]); 01302 ASSERT_EQ(3, m3x3s[2]); 01303 ASSERT_EQ(2, m3x3s[3]); 01304 ASSERT_EQ(4, m3x3s[4]); 01305 ASSERT_EQ(5, m3x3s[5]); 01306 ASSERT_EQ(3, m3x3s[6]); 01307 ASSERT_EQ(5, m3x3s[7]); 01308 ASSERT_EQ(6, m3x3s[8]); 01309 // Packing back 01310 m3x3s.packSquareMatrix(); 01311 ASSERT_EQ(6, m3x3s.size()); 01312 // Validating 01313 ASSERT_EQ(1, m3x3s[0]); 01314 ASSERT_EQ(2, m3x3s[1]); 01315 ASSERT_EQ(3, m3x3s[2]); 01316 ASSERT_EQ(4, m3x3s[3]); 01317 ASSERT_EQ(5, m3x3s[4]); 01318 ASSERT_EQ(6, m3x3s[5]); 01319 } 01320 01321 TEST(Array, FuzzyComparison) 01322 { 01323 typedef Array<Array<Array<FloatSpec<32, CastModeSaturate>, ArrayModeStatic, 2>, 01324 ArrayModeStatic, 2>, 01325 ArrayModeStatic, 2> ArrayStatic32; 01326 01327 typedef Array<Array<Array<FloatSpec<64, CastModeSaturate>, ArrayModeDynamic, 2>, 01328 ArrayModeDynamic, 2>, 01329 ArrayModeDynamic, 2> ArrayDynamic64; 01330 01331 ArrayStatic32 array_s32; 01332 01333 ArrayDynamic64 array_d64; 01334 01335 array_d64.resize(2); 01336 array_d64[0].resize(2); 01337 array_d64[1].resize(2); 01338 array_d64[0][0].resize(2); 01339 array_d64[0][1].resize(2); 01340 array_d64[1][0].resize(2); 01341 array_d64[1][1].resize(2); 01342 01343 std::cout << "One:"; 01344 uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0); 01345 std::cout << std::endl << "------"; 01346 uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0); 01347 std::cout << std::endl; 01348 01349 // Both are equal right now 01350 ASSERT_TRUE(array_d64 == array_s32); 01351 ASSERT_TRUE(array_d64.isClose(array_s32)); 01352 ASSERT_TRUE(array_s32.isClose(array_d64)); 01353 01354 // Slightly modifying - still close enough 01355 array_s32[0][0][0] = 123.456F + uavcan::NumericTraits<float>::epsilon() * 123.0F; 01356 array_s32[0][0][1] = uavcan::NumericTraits<float>::infinity(); 01357 array_s32[0][1][0] = uavcan::NumericTraits<float>::epsilon(); 01358 array_s32[0][1][1] = -uavcan::NumericTraits<float>::epsilon(); 01359 01360 array_d64[0][0][0] = 123.456; 01361 array_d64[0][0][1] = uavcan::NumericTraits<double>::infinity(); 01362 array_d64[0][1][0] = -uavcan::NumericTraits<double>::epsilon(); // Note that the sign is inverted 01363 array_d64[0][1][1] = uavcan::NumericTraits<double>::epsilon(); 01364 01365 std::cout << "Two:"; 01366 uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0); 01367 std::cout << std::endl << "------"; 01368 uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0); 01369 std::cout << std::endl; 01370 01371 // They are close bot not exactly equal 01372 ASSERT_FALSE(array_d64 == array_s32); 01373 ASSERT_TRUE(array_d64.isClose(array_s32)); 01374 ASSERT_TRUE(array_s32.isClose(array_d64)); 01375 01376 // Not close 01377 array_d64[0][0][0] = 123.457; 01378 01379 ASSERT_FALSE(array_d64 == array_s32); 01380 ASSERT_FALSE(array_d64.isClose(array_s32)); 01381 ASSERT_FALSE(array_s32.isClose(array_d64)); 01382 01383 // Values are close, but lengths differ 01384 array_d64[0][0][0] = 123.456; 01385 01386 ASSERT_FALSE(array_d64 == array_s32); 01387 ASSERT_TRUE(array_d64.isClose(array_s32)); 01388 ASSERT_TRUE(array_s32.isClose(array_d64)); 01389 01390 array_d64[0][0].resize(1); 01391 01392 ASSERT_FALSE(array_d64 == array_s32); 01393 ASSERT_FALSE(array_d64.isClose(array_s32)); 01394 ASSERT_FALSE(array_s32.isClose(array_d64)); 01395 01396 std::cout << "Three:"; 01397 uavcan::YamlStreamer<ArrayStatic32>::stream(std::cout, array_s32, 0); 01398 std::cout << std::endl << "------"; 01399 uavcan::YamlStreamer<ArrayDynamic64>::stream(std::cout, array_d64, 0); 01400 std::cout << std::endl; 01401 } 01402 01403 TEST(Array, CaseConversion) 01404 { 01405 Array<IntegerSpec<8, SignednessUnsigned, CastModeTruncate>, ArrayModeDynamic, 30> str; 01406 01407 str.convertToLowerCaseASCII(); 01408 str.convertToUpperCaseASCII(); 01409 01410 ASSERT_STREQ("", str.c_str()); 01411 01412 str = "Hello World!"; 01413 01414 ASSERT_STREQ("Hello World!", str.c_str()); 01415 str.convertToLowerCaseASCII(); 01416 ASSERT_STREQ("hello world!", str.c_str()); 01417 str.convertToUpperCaseASCII(); 01418 ASSERT_STREQ("HELLO WORLD!", str.c_str()); 01419 }
Generated on Tue Jul 12 2022 17:17:30 by
1.7.2