mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
add utility for reading and writing of minitaur quadruped robot log files.
Example usage:
const char* fileName = "D:/LOG00053.TXT";
btAlignedObjectArray<MinitaurLogRecord> logRecords;
btAlignedObjectArray<std::string> structNames;
std::string structTypes;
bool verbose = false;
//reading
int val = readMinitaurLogFile(fileName, structNames, structTypes,
logRecords, verbose);
//writing
const char* fileNameOut = "D:/LOG00100.TXT";
FILE* f = createMinitaurLogFile(fileNameOut,structNames,structTypes);
for (int i=0;i<logRecords.size();i++)
{
appendMinitaurLogData(f, structTypes, logRecords[i]);
}
closeMinitaurLogFile(f);
This commit is contained in:
Erwin Coumans
parent
0c464e6848
commit
0e8bc418d7
253
examples/Utils/RobotLoggingUtil.cpp
Normal file
253
examples/Utils/RobotLoggingUtil.cpp
Normal file
@ -0,0 +1,253 @@
|
||||
#include "RobotLoggingUtil.h"
|
||||
#include <stdio.h>
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
|
||||
|
||||
|
||||
static bool readLine(FILE* file, btAlignedObjectArray<char>& 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<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose)
|
||||
{
|
||||
|
||||
int retVal = 0;
|
||||
|
||||
FILE* f = fopen(fileName,"rb");
|
||||
if (f)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("Opened file %s\n", fileName);
|
||||
}
|
||||
btAlignedObjectArray<char> line0Buf;
|
||||
bool eof = readLine(f,line0Buf);
|
||||
btAlignedObjectArray<char> line1Buf;
|
||||
eof |= readLine(f,line1Buf);
|
||||
std::string line0 = &line0Buf[0];
|
||||
structTypes = &line1Buf[0];
|
||||
|
||||
btAlignedObjectArray<std::string> 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.size();i++)
|
||||
{
|
||||
|
||||
|
||||
switch (structTypes[i])
|
||||
{
|
||||
case 'I':
|
||||
{
|
||||
size_t s = fread(blaat,sizeof(int),1,f);
|
||||
if (s != 1)
|
||||
{
|
||||
eof = true;
|
||||
retVal = eCorruptValue;
|
||||
break;
|
||||
|
||||
}
|
||||
int v = *(int*)blaat;
|
||||
if (s==1)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("%s = %d\n",structNames[i].c_str(),v);
|
||||
}
|
||||
record.m_values.push_back(v);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'f':
|
||||
{
|
||||
float v;
|
||||
size_t s = fread(&v,sizeof(float),1,f);
|
||||
if (s != 1)
|
||||
{
|
||||
eof = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (s==1)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("%s = %f\n",structNames[i].c_str(),v);
|
||||
}
|
||||
record.m_values.push_back(v);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'B':
|
||||
{
|
||||
char v;
|
||||
size_t s = fread(&v,sizeof(char),1,f);
|
||||
if (s != 1)
|
||||
{
|
||||
eof = true;
|
||||
break;
|
||||
}
|
||||
if (s==1)
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("%s = %d\n",structNames[i].c_str(),v);
|
||||
}
|
||||
record.m_values.push_back(v);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("Unknown type\n");
|
||||
}
|
||||
retVal = eUnknownType;
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
logRecords.push_back(record);
|
||||
numStructsRead++;
|
||||
}
|
||||
if (verbose)
|
||||
{
|
||||
printf("numStructsRead = %d\n",numStructsRead);
|
||||
}
|
||||
if (retVal==0)
|
||||
{
|
||||
retVal = numStructsRead;
|
||||
}
|
||||
}
|
||||
|
||||
//read header and
|
||||
} else
|
||||
{
|
||||
if (verbose)
|
||||
{
|
||||
printf("Could not open file %s", fileName);
|
||||
}
|
||||
retVal = eMinitaurFileNotFound;
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes)
|
||||
{
|
||||
FILE* f = fopen(fileName,"wb");
|
||||
if (f)
|
||||
{
|
||||
for (int i=0;i<structNames.size();i++)
|
||||
{
|
||||
int len = strlen(structNames[i].c_str());
|
||||
fwrite(structNames[i].c_str(),len,1,f);
|
||||
if (i<structNames.size()-1)
|
||||
{
|
||||
fwrite(",",1,1,f);
|
||||
}
|
||||
}
|
||||
int sz = sizeof("\n");
|
||||
fwrite("\n",sz-1,1,f);
|
||||
fwrite(structTypes.c_str(),strlen(structTypes.c_str()),1,f);
|
||||
fwrite("\n",sz-1,1,f);
|
||||
|
||||
}
|
||||
|
||||
|
||||
return f;
|
||||
}
|
||||
|
||||
void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData)
|
||||
{
|
||||
if (f)
|
||||
{
|
||||
unsigned char buf[2] = {0xaa,0xbb};
|
||||
fwrite(buf,2,1,f);
|
||||
if (structTypes.length() == logData.m_values.size())
|
||||
{
|
||||
for (int i=0;i<logData.m_values.size();i++)
|
||||
{
|
||||
switch(structTypes[i])
|
||||
{
|
||||
case 'I':
|
||||
{
|
||||
fwrite(&logData.m_values[i].m_intVal,sizeof(int),1,f);
|
||||
break;
|
||||
}
|
||||
case 'f':
|
||||
{
|
||||
fwrite(&logData.m_values[i].m_floatVal,sizeof(float),1,f);
|
||||
break;
|
||||
}
|
||||
case 'B':
|
||||
{
|
||||
fwrite(&logData.m_values[i].m_charVal,sizeof(char),1,f);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void closeMinitaurLogFile(FILE* f)
|
||||
{
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
}
|
||||
}
|
||||
54
examples/Utils/RobotLoggingUtil.h
Normal file
54
examples/Utils/RobotLoggingUtil.h
Normal file
@ -0,0 +1,54 @@
|
||||
#ifndef ROBOT_LOGGING_UTIL_H
|
||||
#define ROBOT_LOGGING_UTIL_H
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include <string>
|
||||
|
||||
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<MinitaurLogValue> m_values;
|
||||
};
|
||||
|
||||
enum MINITAUR_LOG_ERROR
|
||||
{
|
||||
eMinitaurFileNotFound = -1,
|
||||
eCorruptHeader = -2,
|
||||
eUnknownType = -3,
|
||||
eCorruptValue = -4,
|
||||
eInvalidAABBAlignCheck = -5,
|
||||
};
|
||||
|
||||
int readMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes, btAlignedObjectArray<MinitaurLogRecord>& logRecords, bool verbose);
|
||||
|
||||
FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray<std::string>& structNames, std::string& structTypes);
|
||||
void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData);
|
||||
void closeMinitaurLogFile(FILE* f);
|
||||
|
||||
#endif //ROBOT_LOGGING_UTIL_H
|
||||
Loading…
Reference in New Issue
Block a user