diff --git a/examples/Utils/RobotLoggingUtil.cpp b/examples/Utils/RobotLoggingUtil.cpp new file mode 100644 index 000000000..0499a5aa9 --- /dev/null +++ b/examples/Utils/RobotLoggingUtil.cpp @@ -0,0 +1,253 @@ +#include "RobotLoggingUtil.h" +#include +#include "LinearMath/btAlignedObjectArray.h" + +#include "../Importers/ImportURDFDemo/urdfStringSplit.h" + + +static bool readLine(FILE* file, btAlignedObjectArray& line) +{ + int c = 0; + for (c=fgetc(file);(c != EOF && c != '\n');c=fgetc(file)) + { + line.push_back(c); + } + line.push_back(0); + return (c == EOF); +} + + +int readMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes, btAlignedObjectArray& logRecords, bool verbose) +{ + + int retVal = 0; + + FILE* f = fopen(fileName,"rb"); + if (f) + { + if (verbose) + { + printf("Opened file %s\n", fileName); + } + btAlignedObjectArray line0Buf; + bool eof = readLine(f,line0Buf); + btAlignedObjectArray line1Buf; + eof |= readLine(f,line1Buf); + std::string line0 = &line0Buf[0]; + structTypes = &line1Buf[0]; + + btAlignedObjectArray separators; + separators.push_back(","); + + urdfStringSplit(structNames,line0,separators); + if (verbose) + { + printf("Num Fields = %d\n",structNames.size()); + } + btAssert(structTypes.size() == structNames.size()); + if (structTypes.size() != structNames.size()) + { + retVal = eCorruptHeader; + } + int numStructsRead = 0; + + if (structTypes.size() == structNames.size()) + { + while (!eof) + { + unsigned char blaat[1024]; + size_t s = fread(blaat,2,1,f); + if (s!=1) + { + eof=true; + retVal = eInvalidAABBAlignCheck; + break; + } + if ((blaat[0] != 0xaa) || (blaat[1] != 0xbb)) + { + if (verbose) + { + printf("Expected 0xaa0xbb, terminating\n"); + } + } + + if (verbose) + { + printf("Reading structure %d\n",numStructsRead); + } + MinitaurLogRecord record; + + for (int i=0;i& structNames, std::string& structTypes) +{ + FILE* f = fopen(fileName,"wb"); + if (f) + { + for (int i=0;i + +struct MinitaurLogValue +{ + MinitaurLogValue() + :m_intVal(0xcdcdcdcd) + { + } + MinitaurLogValue(int iv) + :m_intVal(iv) + { + } + MinitaurLogValue(float fv) + :m_floatVal(fv) + { + } + MinitaurLogValue(char fv) + :m_charVal(fv) + { + } + + union + { + char m_charVal; + int m_intVal; + float m_floatVal; + }; +}; + +struct MinitaurLogRecord +{ + btAlignedObjectArray m_values; +}; + +enum MINITAUR_LOG_ERROR +{ + eMinitaurFileNotFound = -1, + eCorruptHeader = -2, + eUnknownType = -3, + eCorruptValue = -4, + eInvalidAABBAlignCheck = -5, +}; + +int readMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes, btAlignedObjectArray& logRecords, bool verbose); + +FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes); +void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData); +void closeMinitaurLogFile(FILE* f); + +#endif //ROBOT_LOGGING_UTIL_H