ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "byte_array.h" 00008 #include <cstring> 00009 00010 namespace argos { 00011 00012 /****************************************/ 00013 /****************************************/ 00014 00015 CByteArray::CByteArray(const UInt8* pun_buffer, 00016 size_t un_size) { 00017 AddBuffer(pun_buffer, un_size); 00018 } 00019 00020 /****************************************/ 00021 /****************************************/ 00022 00023 CByteArray::CByteArray(size_t un_size, 00024 UInt8 un_value) { 00025 m_vecBuffer.assign(un_size, un_value); 00026 } 00027 00028 /****************************************/ 00029 /****************************************/ 00030 00031 void CByteArray::Zero() { 00032 ::memset(&m_vecBuffer[0], 0, sizeof(UInt8) * Size()); 00033 } 00034 00035 /****************************************/ 00036 /****************************************/ 00037 00038 CByteArray& CByteArray::operator=(const CByteArray& c_byte_array) { 00039 if(this != &c_byte_array) { 00040 m_vecBuffer = c_byte_array.m_vecBuffer; 00041 } 00042 return *this; 00043 } 00044 00045 /****************************************/ 00046 /****************************************/ 00047 00048 CByteArray& CByteArray::AddBuffer(const UInt8* pun_buffer, 00049 size_t un_size) { 00050 for(size_t i = 0; i < un_size; ++i) { 00051 m_vecBuffer.push_back(pun_buffer[i]); 00052 } 00053 return *this; 00054 } 00055 00056 /****************************************/ 00057 /****************************************/ 00058 00059 CByteArray& CByteArray::FetchBuffer(UInt8* pun_buffer, 00060 size_t un_size) { 00061 if(Size() < un_size) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << un_size << " requested, " << Size() << " available)"); 00062 for(size_t i = 0; i < un_size; ++i) { 00063 *(pun_buffer+i) = m_vecBuffer[i]; 00064 } 00065 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + un_size); 00066 return *this; 00067 } 00068 00069 /****************************************/ 00070 /****************************************/ 00071 00072 CByteArray& CByteArray::operator<<(UInt8 un_value) { 00073 m_vecBuffer.push_back(un_value); 00074 return *this; 00075 } 00076 00077 /****************************************/ 00078 /****************************************/ 00079 00080 CByteArray& CByteArray::operator>>(UInt8& un_value) { 00081 if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)"); 00082 un_value = m_vecBuffer.front(); 00083 m_vecBuffer.erase(m_vecBuffer.begin()); 00084 return *this; 00085 } 00086 00087 /****************************************/ 00088 /****************************************/ 00089 00090 CByteArray& CByteArray::operator<<(SInt8 n_value) { 00091 m_vecBuffer.push_back(n_value); 00092 return *this; 00093 } 00094 00095 /****************************************/ 00096 /****************************************/ 00097 00098 CByteArray& CByteArray::operator>>(SInt8& n_value) { 00099 if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)"); 00100 n_value = m_vecBuffer.front(); 00101 m_vecBuffer.erase(m_vecBuffer.begin()); 00102 return *this; 00103 } 00104 00105 /****************************************/ 00106 /****************************************/ 00107 00108 CByteArray& CByteArray::operator<<(UInt16 un_value) { 00109 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00110 m_vecBuffer.push_back(punByte[0]); 00111 m_vecBuffer.push_back(punByte[1]); 00112 return *this; 00113 } 00114 00115 /****************************************/ 00116 /****************************************/ 00117 00118 CByteArray& CByteArray::operator>>(UInt16& un_value) { 00119 if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)"); 00120 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00121 punByte[0] = m_vecBuffer[0]; 00122 punByte[1] = m_vecBuffer[1]; 00123 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2); 00124 return *this; 00125 } 00126 00127 /****************************************/ 00128 /****************************************/ 00129 00130 CByteArray& CByteArray::operator<<(SInt16 n_value) { 00131 UInt8* punByte = reinterpret_cast<UInt8*>(&n_value); 00132 m_vecBuffer.push_back(punByte[0]); 00133 m_vecBuffer.push_back(punByte[1]); 00134 return *this; 00135 } 00136 00137 /****************************************/ 00138 /****************************************/ 00139 00140 CByteArray& CByteArray::operator>>(SInt16& n_value) { 00141 if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)"); 00142 UInt8* punByte = reinterpret_cast<UInt8*>(&n_value); 00143 punByte[0] = m_vecBuffer[0]; 00144 punByte[1] = m_vecBuffer[1]; 00145 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2); 00146 return *this; 00147 } 00148 00149 /****************************************/ 00150 /****************************************/ 00151 00152 CByteArray& CByteArray::operator<<(UInt32 un_value) { 00153 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00154 m_vecBuffer.push_back(punByte[0]); 00155 m_vecBuffer.push_back(punByte[1]); 00156 m_vecBuffer.push_back(punByte[2]); 00157 m_vecBuffer.push_back(punByte[3]); 00158 return *this; 00159 } 00160 00161 /****************************************/ 00162 /****************************************/ 00163 00164 CByteArray& CByteArray::operator>>(UInt32& un_value) { 00165 if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)"); 00166 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00167 punByte[0] = m_vecBuffer[0]; 00168 punByte[1] = m_vecBuffer[1]; 00169 punByte[2] = m_vecBuffer[2]; 00170 punByte[3] = m_vecBuffer[3]; 00171 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4); 00172 return *this; 00173 } 00174 00175 /****************************************/ 00176 /****************************************/ 00177 00178 CByteArray& CByteArray::operator<<(SInt32 un_value) { 00179 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00180 m_vecBuffer.push_back(punByte[0]); 00181 m_vecBuffer.push_back(punByte[1]); 00182 m_vecBuffer.push_back(punByte[2]); 00183 m_vecBuffer.push_back(punByte[3]); 00184 return *this; 00185 } 00186 00187 /****************************************/ 00188 /****************************************/ 00189 00190 CByteArray& CByteArray::operator>>(SInt32& un_value) { 00191 if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)"); 00192 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00193 punByte[0] = m_vecBuffer[0]; 00194 punByte[1] = m_vecBuffer[1]; 00195 punByte[2] = m_vecBuffer[2]; 00196 punByte[3] = m_vecBuffer[3]; 00197 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4); 00198 return *this; 00199 } 00200 00201 /****************************************/ 00202 /****************************************/ 00203 00204 CByteArray& CByteArray::operator<<(UInt64 un_value) { 00205 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00206 m_vecBuffer.push_back(punByte[0]); 00207 m_vecBuffer.push_back(punByte[1]); 00208 m_vecBuffer.push_back(punByte[2]); 00209 m_vecBuffer.push_back(punByte[3]); 00210 m_vecBuffer.push_back(punByte[4]); 00211 m_vecBuffer.push_back(punByte[5]); 00212 m_vecBuffer.push_back(punByte[6]); 00213 m_vecBuffer.push_back(punByte[7]); 00214 return *this; 00215 } 00216 00217 /****************************************/ 00218 /****************************************/ 00219 00220 CByteArray& CByteArray::operator>>(UInt64& un_value) { 00221 if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)"); 00222 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00223 punByte[0] = m_vecBuffer[0]; 00224 punByte[1] = m_vecBuffer[1]; 00225 punByte[2] = m_vecBuffer[2]; 00226 punByte[3] = m_vecBuffer[3]; 00227 punByte[4] = m_vecBuffer[4]; 00228 punByte[5] = m_vecBuffer[5]; 00229 punByte[6] = m_vecBuffer[6]; 00230 punByte[7] = m_vecBuffer[7]; 00231 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8); 00232 return *this; 00233 } 00234 00235 /****************************************/ 00236 /****************************************/ 00237 00238 CByteArray& CByteArray::operator<<(SInt64 un_value) { 00239 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00240 m_vecBuffer.push_back(punByte[0]); 00241 m_vecBuffer.push_back(punByte[1]); 00242 m_vecBuffer.push_back(punByte[2]); 00243 m_vecBuffer.push_back(punByte[3]); 00244 m_vecBuffer.push_back(punByte[4]); 00245 m_vecBuffer.push_back(punByte[5]); 00246 m_vecBuffer.push_back(punByte[6]); 00247 m_vecBuffer.push_back(punByte[7]); 00248 return *this; 00249 } 00250 00251 /****************************************/ 00252 /****************************************/ 00253 00254 CByteArray& CByteArray::operator>>(SInt64& un_value) { 00255 if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)"); 00256 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00257 punByte[0] = m_vecBuffer[0]; 00258 punByte[1] = m_vecBuffer[1]; 00259 punByte[2] = m_vecBuffer[2]; 00260 punByte[3] = m_vecBuffer[3]; 00261 punByte[4] = m_vecBuffer[4]; 00262 punByte[5] = m_vecBuffer[5]; 00263 punByte[6] = m_vecBuffer[6]; 00264 punByte[7] = m_vecBuffer[7]; 00265 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8); 00266 return *this; 00267 } 00268 00269 /****************************************/ 00270 /****************************************/ 00271 00272 CByteArray& CByteArray::operator<<(unsigned long int un_value) { 00273 UInt32 unSize = sizeof(un_value); 00274 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00275 for(UInt32 i = 0; i < unSize; ++i) { 00276 m_vecBuffer.push_back(punByte[i]); 00277 } 00278 return *this; 00279 } 00280 00281 /****************************************/ 00282 /****************************************/ 00283 00284 CByteArray& CByteArray::operator>>(unsigned long int& un_value) { 00285 UInt32 unSize = sizeof(un_value); 00286 if(Size() < unSize) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << unSize << " requested, " << Size() << " available)"); 00287 UInt8* punByte = reinterpret_cast<UInt8*>(&un_value); 00288 for(UInt32 i = 0; i < unSize; ++i) { 00289 *punByte = m_vecBuffer[i]; 00290 ++punByte; 00291 } 00292 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + unSize); 00293 return *this; 00294 } 00295 00296 /****************************************/ 00297 /****************************************/ 00298 00299 CByteArray& CByteArray::operator<<(signed long int n_value) { 00300 UInt32 unSize = sizeof(n_value); 00301 UInt8* punByte = reinterpret_cast<UInt8*>(&n_value); 00302 for(UInt32 i = 0; i < unSize; ++i) { 00303 m_vecBuffer.push_back(punByte[i]); 00304 } 00305 return *this; 00306 } 00307 00308 /****************************************/ 00309 /****************************************/ 00310 00311 CByteArray& CByteArray::operator>>(signed long int& n_value) { 00312 UInt32 unSize = sizeof(n_value); 00313 if(Size() < unSize) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << unSize << " requested, " << Size() << " available)"); 00314 UInt8* punByte = reinterpret_cast<UInt8*>(&n_value); 00315 for(UInt32 i = 0; i < unSize; ++i) { 00316 *punByte = m_vecBuffer[i]; 00317 ++punByte; 00318 } 00319 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + unSize); 00320 return *this; 00321 } 00322 00323 /****************************************/ 00324 /****************************************/ 00325 00326 CByteArray& CByteArray::operator<<(Real f_value) { 00327 UInt32 unSize = sizeof(f_value); 00328 UInt8* punByte = reinterpret_cast<UInt8*>(&f_value); 00329 for(UInt32 i = 0; i < unSize; ++i) { 00330 m_vecBuffer.push_back(punByte[i]); 00331 } 00332 return *this; 00333 } 00334 00335 /****************************************/ 00336 /****************************************/ 00337 00338 CByteArray& CByteArray::operator>>(Real& f_value) { 00339 UInt32 unSize = sizeof(f_value); 00340 if(Size() < unSize) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << unSize << " requested, " << Size() << " available)"); 00341 UInt8* punByte = reinterpret_cast<UInt8*>(&f_value); 00342 for(UInt32 i = 0; i < unSize; ++i) { 00343 *punByte = m_vecBuffer[i]; 00344 ++punByte; 00345 } 00346 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + unSize); 00347 return *this; 00348 } 00349 00350 /****************************************/ 00351 /****************************************/ 00352 00353 CByteArray& CByteArray::operator<<(const std::string& str_value) { 00354 /* Insert string contents */ 00355 for(size_t i = 0; i < str_value.size(); ++i) { 00356 *this << static_cast<UInt8>(str_value[i]); 00357 } 00358 /* Terminate string with a \0 */ 00359 *this << static_cast<UInt8>(0); 00360 return *this; 00361 } 00362 00363 /****************************************/ 00364 /****************************************/ 00365 00366 CByteArray& CByteArray::operator>>(std::string& str_value) { 00367 if(Empty()) THROW_ARGOSEXCEPTION("Attempting to extract values from empty byte array"); 00368 str_value.clear(); 00369 size_t i = 0; 00370 while(i < Size() && m_vecBuffer[i] != '\0') { 00371 str_value += m_vecBuffer[i]; 00372 ++i; 00373 } 00374 if(m_vecBuffer[i] == '\0') { 00375 ++i; 00376 } 00377 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + i); 00378 return *this; 00379 } 00380 00381 /****************************************/ 00382 /****************************************/ 00383 00384 std::ostream& operator<<(std::ostream& c_os, const CByteArray& c_byte_array) { 00385 c_os << "CByteArray ["; 00386 for(size_t i = 0; i < c_byte_array.Size(); ++i) { 00387 c_os << " " << c_byte_array.m_vecBuffer[i]; 00388 } 00389 c_os << " ]" << std::endl; 00390 return c_os; 00391 } 00392 00393 /****************************************/ 00394 /****************************************/ 00395 00396 }