bullet3/examples/Utils/RobotLoggingUtil.cpp

#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) *(unsigned int*)blaat;
						if (s==1)
						{
							if (verbose)
							{
								printf("%s = %d\n",structNames[i].c_str(),v);
							}
							record.m_values.push_back(v);
						}
						break;
					}
					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':
				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);
	}
}