00001 #include "ulog_parser.h"
00002 #include "ulog_messages.h"
00003
00004 #include <fstream>
00005 #include <string.h>
00006 #include <iosfwd>
00007 #include <sstream>
00008 #include <iomanip>
00009 using ios = std::ios;
00010
00011
00012
00013 ULogParser::ULogParser(const std::string &filename):
00014 _file_start_time(0)
00015 {
00016 std::ifstream replay_file (filename, std::ifstream::in);
00017
00018 if (!replay_file.is_open())
00019 {
00020 throw std::runtime_error ("ULog: Failed to open replay file" );
00021 }
00022
00023 bool ret = readFileHeader(replay_file);
00024
00025 if( !ret )
00026 {
00027 throw std::runtime_error("ULog: wrong header");
00028 }
00029
00030 if( ! readFileDefinitions(replay_file) )
00031 {
00032 throw std::runtime_error("ULog: error loading definitions");
00033 }
00034
00035 replay_file.seekg(_data_section_start);
00036
00037
00038 while (replay_file)
00039 {
00040 ulog_message_header_s message_header;
00041 replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
00042
00043 _read_buffer.reserve(message_header.msg_size + 1);
00044 char *message = (char *)_read_buffer.data();
00045 replay_file.read(message, message_header.msg_size);
00046 message[message_header.msg_size] = '\0';
00047
00048 switch (message_header.msg_type)
00049 {
00050 case (int)ULogMessageType::ADD_LOGGED_MSG:
00051 {
00052 Subscription sub;
00053
00054 sub.multi_id = *reinterpret_cast<uint8_t*>(message);
00055 sub.msg_id = *reinterpret_cast<uint16_t*>( message+1 );
00056 message += 3;
00057 sub.message_name.assign( message, message_header.msg_size - 3 );
00058
00059 const auto it = _formats.find(sub.message_name);
00060 if ( it != _formats.end())
00061 {
00062 sub.format = &it->second;
00063 }
00064 _subscriptions.insert( {sub.msg_id, sub} );
00065
00066 if( sub.multi_id > 0 )
00067 {
00068 _message_name_with_multi_id.insert( sub.message_name );
00069 }
00070
00071
00072
00073 }break;
00074 case (int)ULogMessageType::REMOVE_LOGGED_MSG: printf("REMOVE_LOGGED_MSG\n" );
00075 {
00076 uint16_t msg_id = *reinterpret_cast<uint16_t*>( message );
00077 _subscriptions.erase( msg_id );
00078
00079 } break;
00080 case (int)ULogMessageType::DATA:
00081 {
00082 uint16_t msg_id = *reinterpret_cast<uint16_t*>( message );
00083 message += 2;
00084 auto sub_it = _subscriptions.find( msg_id );
00085 if( sub_it == _subscriptions.end() )
00086 {
00087 continue;
00088 }
00089 const Subscription& sub = sub_it->second;
00090
00091 parseDataMessage(sub, message);
00092
00093 } break;
00094
00095 case (int)ULogMessageType::LOGGING:
00096 {
00097 MessageLog msg;
00098 msg.level = static_cast<char>( message[0] );
00099 message += sizeof( char );
00100 msg.timestamp = *reinterpret_cast<uint64_t*>(message);
00101 message += sizeof( uint64_t );
00102 msg.msg.assign( message, message_header.msg_size - 9 );
00103
00104 _message_logs.push_back( std::move( msg ) );
00105
00106 } break;
00107 case (int)ULogMessageType::SYNC:
00108 break;
00109 case (int)ULogMessageType::DROPOUT:
00110 break;
00111 case (int)ULogMessageType::INFO:
00112 break;
00113 case (int)ULogMessageType::INFO_MULTIPLE:
00114 break;
00115 case (int)ULogMessageType::PARAMETER:
00116 printf("PARAMETER changed at run-time. Ignored\n" );
00117 std::cout << std::flush;
00118 break;
00119 }
00120 }
00121 }
00122
00123 void ULogParser::parseDataMessage(const ULogParser::Subscription &sub, char *message)
00124 {
00125 size_t other_fields_count = 0;
00126 std::string ts_name = sub.message_name;
00127
00128 for(const auto& field: sub.format->fields)
00129 {
00130 if( field.type == OTHER)
00131 {
00132 other_fields_count++;
00133 }
00134 }
00135
00136 if( _message_name_with_multi_id.count(ts_name) > 0 )
00137 {
00138 char buff[10];
00139 sprintf(buff,".%02d", sub.multi_id );
00140 ts_name += std::string(buff);
00141 }
00142
00143
00144 auto ts_it = _timeseries.find( ts_name );
00145 if( ts_it == _timeseries.end() )
00146 {
00147 ts_it = _timeseries.insert( { ts_name, createTimeseries(sub.format) } ).first;
00148 }
00149 Timeseries& timeseries = ts_it->second;
00150
00151 uint64_t time_val = *reinterpret_cast<uint64_t*>(message);
00152 timeseries.timestamps.push_back( time_val );
00153 message += sizeof(uint64_t);
00154
00155 size_t index = 0;
00156 parseSimpleDataMessage(timeseries, sub.format, message, &index);
00157
00158 }
00159
00160 char* ULogParser::parseSimpleDataMessage(Timeseries& timeseries, const Format *format,
00161 char *message, size_t* index)
00162 {
00163 for (const auto& field: format->fields)
00164 {
00165
00166 if (StringView(field.field_name).starts_with("_padding")) {
00167 message += field.array_size;
00168 continue;
00169 }
00170
00171 for (int array_pos = 0; array_pos < field.array_size; array_pos++)
00172 {
00173 double value = 0;
00174 switch( field.type )
00175 {
00176 case UINT8:{
00177 value = static_cast<double>( *reinterpret_cast<uint8_t*>(message));
00178 message += 1;
00179 }break;
00180 case INT8:{
00181 value = static_cast<double>( *reinterpret_cast<int8_t*>(message));
00182 message += 1;
00183 }break;
00184 case UINT16:{
00185 value = static_cast<double>( *reinterpret_cast<uint16_t*>(message));
00186 message += 2;
00187 }break;
00188 case INT16:{
00189 value = static_cast<double>( *reinterpret_cast<int16_t*>(message));
00190 message += 2;
00191 }break;
00192 case UINT32:{
00193 value = static_cast<double>( *reinterpret_cast<uint32_t*>(message));
00194 message += 4;
00195 }break;
00196 case INT32:{
00197 value = static_cast<double>( *reinterpret_cast<int32_t*>(message));
00198 message += 4;
00199 }break;
00200 case UINT64:{
00201 value = static_cast<double>( *reinterpret_cast<uint64_t*>(message));
00202 message += 8;
00203 }break;
00204 case INT64:{
00205 value = static_cast<double>( *reinterpret_cast<int64_t*>(message));
00206 message += 8;
00207 }break;
00208 case FLOAT:{
00209 value = static_cast<double>( *reinterpret_cast<float*>(message));
00210 message += 4;
00211 }break;
00212 case DOUBLE:{
00213 value = ( *reinterpret_cast<double*>(message));
00214 message += 8;
00215 }break;
00216 case CHAR:{
00217 value = static_cast<double>( *reinterpret_cast<char*>(message));
00218 message += 1;
00219 }break;
00220 case BOOL:{
00221 value = static_cast<double>( *reinterpret_cast<bool*>(message));
00222 message += 1;
00223 }break;
00224 case OTHER:{
00225
00226 auto child_format = _formats.at( field.other_type_ID );
00227 message += sizeof(uint64_t);
00228 message = parseSimpleDataMessage(timeseries, &child_format, message, index );
00229 }break;
00230
00231 }
00232
00233 if( field.type != OTHER)
00234 {
00235 timeseries.data[(*index)++].second.push_back( value );
00236 }
00237 }
00238 }
00239 return message;
00240 }
00241
00242
00243 const std::map<std::string, ULogParser::Timeseries> &ULogParser::getTimeseriesMap() const
00244 {
00245 return _timeseries;
00246 }
00247
00248 const std::vector<ULogParser::Parameter>& ULogParser::getParameters() const
00249 {
00250 return _parameters;
00251 }
00252
00253 const std::map<std::string, std::string> &ULogParser::getInfo() const
00254 {
00255 return _info;
00256 }
00257
00258 const std::vector<ULogParser::MessageLog> &ULogParser::getLogs() const
00259 {
00260 return _message_logs;
00261 }
00262
00263
00264 bool ULogParser::readSubscription(std::ifstream &file, uint16_t msg_size)
00265 {
00266 _read_buffer.reserve(msg_size + 1);
00267 char *message = (char *)_read_buffer.data();
00268
00269 file.read(message, msg_size);
00270 message[msg_size] = 0;
00271
00272 if (!file) {
00273 return false;
00274 }
00275
00276 return true;
00277 }
00278
00279 size_t ULogParser::fieldsCount(const ULogParser::Format &format) const
00280 {
00281 size_t count = 0;
00282 for (const auto& field: format.fields)
00283 {
00284 if( field.type == OTHER)
00285 {
00286
00287 count += fieldsCount( _formats.at( field.other_type_ID) );
00288 }
00289 else{
00290 count += size_t(field.array_size);
00291 }
00292 }
00293 return count;
00294 }
00295
00296 std::vector<StringView> ULogParser::splitString(const StringView &strToSplit, char delimeter)
00297 {
00298 std::vector<StringView> splitted_strings;
00299 splitted_strings.reserve(4);
00300
00301 size_t pos = 0;
00302 while( pos < strToSplit.size())
00303 {
00304 size_t new_pos = strToSplit.find_first_of(delimeter, pos);
00305 if( new_pos == std::string::npos)
00306 {
00307 new_pos = strToSplit.size();
00308 }
00309 StringView sv = { &strToSplit.data()[pos], new_pos - pos };
00310 splitted_strings.push_back( sv );
00311 pos = new_pos + 1;
00312 }
00313 return splitted_strings;
00314 }
00315
00316
00317
00318
00319 bool ULogParser::readFileHeader(std::ifstream &file)
00320 {
00321 file.seekg(0);
00322 ulog_file_header_s msg_header;
00323 file.read((char *)&msg_header, sizeof(msg_header));
00324
00325 if (!file) {
00326 return false;
00327 }
00328
00329 _file_start_time = msg_header.timestamp;
00330
00331
00332 char magic[8];
00333 magic[0] = 'U';
00334 magic[1] = 'L';
00335 magic[2] = 'o';
00336 magic[3] = 'g';
00337 magic[4] = 0x01;
00338 magic[5] = 0x12;
00339 magic[6] = 0x35;
00340 return memcmp(magic, msg_header.magic, 7) == 0;
00341 }
00342
00343 bool ULogParser::readFileDefinitions(std::ifstream &file)
00344 {
00345 ulog_message_header_s message_header;
00346 file.seekg(sizeof(ulog_file_header_s));
00347
00348 while (true)
00349 {
00350 file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
00351
00352 if (!file) {
00353 return false;
00354 }
00355
00356 switch (message_header.msg_type)
00357 {
00358 case (int)ULogMessageType::FLAG_BITS:
00359 if (!readFlagBits(file, message_header.msg_size)) {
00360 return false;
00361 }
00362 break;
00363
00364 case (int)ULogMessageType::FORMAT:
00365 if (!readFormat(file, message_header.msg_size)) {
00366 return false;
00367 }
00368
00369 break;
00370
00371 case (int)ULogMessageType::PARAMETER:
00372 if (!readParameter(file, message_header.msg_size)) {
00373 return false;
00374 }
00375
00376 break;
00377
00378 case (int)ULogMessageType::ADD_LOGGED_MSG:
00379 {
00380 _data_section_start = file.tellg() - (std::streamoff)ULOG_MSG_HEADER_LEN;
00381 return true;
00382 }
00383
00384 case (int)ULogMessageType::INFO:
00385 {
00386 if (!readInfo(file, message_header.msg_size)) {
00387 return false;
00388 }
00389 }break;
00390 case (int)ULogMessageType::INFO_MULTIPLE:
00391 file.seekg(message_header.msg_size, ios::cur);
00392 break;
00393
00394 default:
00395 printf("unknown log definition type %i, size %i (offset %i)",
00396 (int)message_header.msg_type, (int)message_header.msg_size, (int)file.tellg());
00397 file.seekg(message_header.msg_size, ios::cur);
00398 break;
00399 }
00400 }
00401 return true;
00402 }
00403
00404
00405
00406 bool ULogParser::readFlagBits(std::ifstream &file, uint16_t msg_size)
00407 {
00408 if (msg_size != 40) {
00409 printf("unsupported message length for FLAG_BITS message (%i)", msg_size);
00410 return false;
00411 }
00412
00413 _read_buffer.reserve(msg_size);
00414 uint8_t *message = (uint8_t *)_read_buffer.data();
00415 file.read((char *)message, msg_size);
00416
00417
00418 uint8_t *incompat_flags = message + 8;
00419
00420
00421 bool contains_appended_data = incompat_flags[0] & ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK;
00422 bool has_unknown_incompat_bits = false;
00423
00424 if (incompat_flags[0] & ~0x1) {
00425 has_unknown_incompat_bits = true;
00426 }
00427
00428 for (int i = 1; i < 8; ++i) {
00429 if (incompat_flags[i]) {
00430 has_unknown_incompat_bits = true;
00431 }
00432 }
00433
00434 if (has_unknown_incompat_bits) {
00435 printf("Log contains unknown incompat bits set. Refusing to parse" );
00436 return false;
00437 }
00438
00439 if (contains_appended_data)
00440 {
00441 uint64_t appended_offsets[3];
00442 memcpy(appended_offsets, message + 16, sizeof(appended_offsets));
00443
00444 if (appended_offsets[0] > 0) {
00445
00446
00447 _read_until_file_position = appended_offsets[0];
00448 }
00449 }
00450 return true;
00451 }
00452
00453 bool ULogParser::readFormat(std::ifstream &file, uint16_t msg_size)
00454 {
00455 static int count = 0;
00456
00457 _read_buffer.reserve(msg_size + 1);
00458 char *buffer = (char *)_read_buffer.data();
00459 file.read(buffer, msg_size);
00460 buffer[msg_size] = 0;
00461
00462 if (!file) {
00463 return false;
00464 }
00465
00466 std::string str_format(buffer);
00467 size_t pos = str_format.find(':');
00468
00469 if (pos == std::string::npos) {
00470 return false;
00471 }
00472
00473 std::string name = str_format.substr(0, pos);
00474 std::string fields = str_format.substr(pos + 1);
00475
00476 Format format;
00477 auto fields_split = splitString( fields, ';' );
00478 format.fields.reserve( fields_split.size() );
00479 for (auto field_section: fields_split)
00480 {
00481 auto field_pair = splitString( field_section, ' ');
00482 auto field_type = field_pair.at(0);
00483 auto field_name = field_pair.at(1);
00484
00485 Field field;
00486 if( field_type.starts_with("int8_t") )
00487 {
00488 field.type = INT8;
00489 field_type.remove_prefix(6);
00490 }
00491 else if( field_type.starts_with("int16_t") )
00492 {
00493 field.type = INT16;
00494 field_type.remove_prefix(7);
00495 }
00496 else if( field_type.starts_with("int32_t") )
00497 {
00498 field.type = INT32;
00499 field_type.remove_prefix(7);
00500 }
00501 else if( field_type.starts_with("int64_t") )
00502 {
00503 field.type = INT64;
00504 field_type.remove_prefix(7);
00505 }
00506 else if( field_type.starts_with("uint8_t") )
00507 {
00508 field.type = UINT8;
00509 field_type.remove_prefix(7);
00510 }
00511 else if( field_type.starts_with("uint16_t") )
00512 {
00513 field.type = UINT16;
00514 field_type.remove_prefix(8);
00515 }
00516 else if( field_type.starts_with("uint32_t") )
00517 {
00518 field.type = UINT32;
00519 field_type.remove_prefix(8);
00520 }
00521 else if( field_type.starts_with("uint64_t") )
00522 {
00523 field.type = UINT64;
00524 field_type.remove_prefix(8);
00525 }
00526 else if( field_type.starts_with("double") )
00527 {
00528 field.type = DOUBLE;
00529 field_type.remove_prefix(6);
00530 }
00531 else if( field_type.starts_with("float") )
00532 {
00533 field.type = FLOAT;
00534 field_type.remove_prefix(5);
00535 }
00536 else if( field_type.starts_with("bool") )
00537 {
00538 field.type = BOOL;
00539 field_type.remove_prefix(4);
00540 }
00541 else if( field_type.starts_with("char") )
00542 {
00543 field.type = CHAR;
00544 field_type.remove_prefix(4);
00545 }
00546 else{
00547 field.type = OTHER;
00548
00549 if (field_type.ends_with("]")) {
00550 StringView helper = field_type;
00551 while (!helper.ends_with("[")) {
00552 helper.remove_suffix(1);
00553 }
00554
00555 helper.remove_suffix(1);
00556 field.other_type_ID = helper.to_string();
00557
00558 while(!field_type.starts_with("[")) {
00559 field_type.remove_prefix(1);
00560 }
00561
00562 } else {
00563 field.other_type_ID = field_type.to_string();
00564 }
00565 }
00566
00567 field.array_size = 1;
00568
00569 if( field_type.size() > 0 && field_type[0] == '[' )
00570 {
00571 field_type.remove_prefix(1);
00572 field.array_size = field_type[0] - '0';
00573 field_type.remove_prefix(1);
00574
00575 while (field_type[0] != ']')
00576 {
00577 field.array_size = 10*field.array_size + field_type[0] - '0';
00578 field_type.remove_prefix(1);
00579 }
00580 }
00581
00582 if( field.type == UINT64 && field_name == StringView("timestamp") )
00583 {
00584
00585 }
00586 else {
00587 field.field_name = field_name.to_string();
00588 format.fields.push_back( field );
00589 }
00590 }
00591
00592 format.name = name;
00593 _formats[name] = std::move(format);
00594
00595 return true;
00596 }
00597
00598 template< typename T >
00599 std::string int_to_hex( T i )
00600 {
00601 std::stringstream stream;
00602 stream << "0x"
00603 << std::setfill ('0') << std::setw(sizeof(T)*2)
00604 << std::hex << i;
00605 return stream.str();
00606 }
00607
00608 bool ULogParser::readInfo(std::ifstream &file, uint16_t msg_size)
00609 {
00610 _read_buffer.reserve(msg_size);
00611 uint8_t *message = (uint8_t *)_read_buffer.data();
00612 file.read((char *)message, msg_size);
00613
00614 if (!file) {
00615 return false;
00616 }
00617 uint8_t key_len = message[0];
00618 message++;
00619 std::string raw_key((char *)message, key_len);
00620 message += key_len;
00621
00622 auto key_parts = splitString( raw_key, ' ' );
00623
00624 std::string key = key_parts[1].to_string();
00625
00626 std::string value;
00627 if( key_parts[0].starts_with("char["))
00628 {
00629 value = std::string( (char *)message, msg_size - key_len - 1 );
00630 }
00631 else if( key_parts[0] == StringView("bool"))
00632 {
00633 bool val = *reinterpret_cast<const bool*>(key_parts[0].data());
00634 value = std::to_string( val );
00635 }
00636 else if( key_parts[0] == StringView("uint8_t"))
00637 {
00638 uint8_t val = *reinterpret_cast<const uint8_t*>(key_parts[0].data());
00639 value = std::to_string( val );
00640 }
00641 else if( key_parts[0] == StringView("int8_t"))
00642 {
00643 int8_t val = *reinterpret_cast<const int8_t*>(key_parts[0].data());
00644 value = std::to_string( val );
00645 }
00646 else if( key_parts[0] == StringView("uint16_t"))
00647 {
00648 uint16_t val = *reinterpret_cast<const uint16_t*>(key_parts[0].data());
00649 value = std::to_string( val );
00650 }
00651 else if( key_parts[0] == StringView("int16_t"))
00652 {
00653 int16_t val = *reinterpret_cast<const int16_t*>(key_parts[0].data());
00654 value = std::to_string( val );
00655 }
00656 else if( key_parts[0] == StringView("uint32_t"))
00657 {
00658 uint32_t val = *reinterpret_cast<const uint32_t*>(key_parts[0].data());
00659 if( key_parts[1].starts_with("ver_") && key_parts[1].ends_with( "_release") )
00660 {
00661 value = int_to_hex(val);
00662 }
00663 else{
00664 value = std::to_string( val );
00665 }
00666 }
00667 else if( key_parts[0] == StringView("int32_t"))
00668 {
00669 int32_t val = *reinterpret_cast<const int32_t*>(key_parts[0].data());
00670 value = std::to_string( val );
00671 }
00672 else if( key_parts[0] == StringView("float"))
00673 {
00674 float val = *reinterpret_cast<const float*>(key_parts[0].data());
00675 value = std::to_string( val );
00676 }
00677 else if( key_parts[0] == StringView("double"))
00678 {
00679 double val = *reinterpret_cast<const double*>(key_parts[0].data());
00680 value = std::to_string( val );
00681 }
00682 else if( key_parts[0] == StringView("uint64_t"))
00683 {
00684 uint64_t val = *reinterpret_cast<const uint64_t*>(key_parts[0].data());
00685 value = std::to_string( val );
00686 }
00687 else if( key_parts[0] == StringView("int64_t"))
00688 {
00689 int64_t val = *reinterpret_cast<const int64_t*>(key_parts[0].data());
00690 value = std::to_string( val );
00691 }
00692
00693
00694 _info.insert( { key, value} );
00695 return true;
00696 }
00697
00698 bool ULogParser::readParameter(std::ifstream &file, uint16_t msg_size)
00699 {
00700 _read_buffer.reserve(msg_size);
00701 uint8_t *message = (uint8_t *)_read_buffer.data();
00702 file.read((char *)message, msg_size);
00703
00704 if (!file) {
00705 return false;
00706 }
00707
00708 uint8_t key_len = message[0];
00709 std::string key((char *)message + 1, key_len);
00710
00711 size_t pos = key.find(' ');
00712
00713 if (pos == std::string::npos) {
00714 return false;
00715 }
00716
00717 std::string type = key.substr(0, pos);
00718
00719 Parameter param;
00720 param.name = key.substr(pos + 1);
00721
00722 if( type == "int32_t" )
00723 {
00724 param.value.val_int = *reinterpret_cast<int32_t*>(message + 1 + key_len);
00725 param.val_type = INT32;
00726 }
00727 else if( type == "float" )
00728 {
00729 param.value.val_real = *reinterpret_cast<float*>(message + 1 + key_len);
00730 param.val_type = FLOAT;
00731 }
00732 else {
00733 throw std::runtime_error("unknown parameter type");
00734 }
00735 _parameters.push_back( param );
00736 return true;
00737 }
00738
00739
00740
00741 ULogParser::Timeseries ULogParser::createTimeseries(const ULogParser::Format* format)
00742 {
00743 std::function<void(const Format& format, const std::string& prefix)> appendVector;
00744
00745 Timeseries timeseries;
00746
00747 appendVector = [&appendVector,this, ×eries](const Format& format, const std::string& prefix)
00748 {
00749 for( const auto& field: format.fields)
00750 {
00751
00752 if (StringView(field.field_name).starts_with("_padding")) {
00753 continue;
00754 }
00755
00756 std::string new_prefix = prefix + "/" + field.field_name;
00757 for(int i=0; i < field.array_size; i++)
00758 {
00759 std::string array_suffix = "";
00760 if( field.array_size > 1)
00761 {
00762 char buff[10];
00763 sprintf(buff, ".%02d", i);
00764 array_suffix = buff;
00765 }
00766 if( field.type != OTHER )
00767 {
00768 timeseries.data.push_back( {new_prefix + array_suffix, std::vector<double>()} );
00769 }
00770 else{
00771 appendVector( this->_formats.at( field.other_type_ID ), new_prefix + array_suffix);
00772 }
00773 }
00774 }
00775 };
00776
00777 appendVector(*format, {});
00778 return timeseries;
00779 }