00001 #include <cstdlib>
00002 #include "libport/cstdio"
00003
00004 #include "urbi/uclient.hh"
00005
00006 namespace urbi
00007 {
00008 inline float fabs (float f )
00009 {
00010 if (f>0) return f;
00011 else return f*(-1.0);
00012 }
00013 enum UType
00014 {
00015 TYPE_BOOL,
00016 TYPE_ANGLE,
00017 TYPE_NORM
00018 };
00019 union UJointValue
00020 {
00021 float angle;
00022 float normalized;
00023 bool boolean;
00024 };
00025 struct UCommand
00026 {
00027 int timestamp;
00028 short id;
00029 UJointValue value;
00030 };
00031
00032 struct UDev
00033 {
00034 char * name;
00035 short id;
00036 UType type;
00037 };
00038 UDev* devices;
00039 int devCount;
00040
00041 int parseHeader(FILE *f,FILE * of)
00042 {
00043 char buff[4];
00044 if (fread(buff,4,1,f)!=1) return 1;
00045 if (fwrite(buff,4,1,of)!=1) return 2;
00046 if (strncmp(buff,"URBI",4)) return 3;
00047 if (fread(&devCount,4,1,f)!=1) return 4;
00048 if (fwrite(&devCount,4,1,of)!=1) return 5;
00049 for (int i=0;i<devCount; ++i)
00050 {
00051 char device[256];
00052 int pos=0;
00053 int a;
00054 do {
00055 if ((device[pos++]=fgetc(f))==EOF) return 6;
00056 }
00057 while (device[pos-1]);
00058 if (fwrite(device,strlen(device)+1,1,of)!=1) return 7;
00059 if (fread(&a,2,1,f)!=1) return 8;
00060 if (fwrite(&a,2,1,of)!=1) return 9;
00061 int type;
00062 if ( (type=fgetc(f)) ==EOF) return 10;
00063 fputc(type,of);
00064 }
00065 return 0;
00066 }
00067
00068 }
00069
00070 int main(int argc, char * argv[])
00071 {
00072
00073 if (argc<3)
00074 {
00075 printf("usage %s infile outfile \nreverse the urbi file\n",argv[0]);
00076 exit(1);
00077 }
00078 FILE * inf;
00079 FILE * ouf;
00080 if (STREQ(argv[1],"-")) inf=stdin;
00081 else
00082 inf=fopen(argv[1],"r");
00083 if (!inf)
00084 {
00085 printf("error opening file\n");
00086 exit(2);
00087 }
00088 if (STREQ(argv[2],"-"))
00089 ouf=stdout;
00090 else
00091 ouf=fopen(argv[2],"w");
00092 if (!ouf)
00093 {
00094 printf("error opening file\n");
00095 exit(2);
00096 }
00097 if (int a=urbi::parseHeader(inf,ouf))
00098 {
00099 printf("error parsing header: %d\n",a);
00100 exit(3);
00101 }
00102 long endheader=ftell(inf);
00103
00104 urbi::UCommand uc;
00105 int starttime=-1;
00106 fseek(inf,sizeof (urbi::UCommand)*(-1),SEEK_END);
00107 while (ftell(inf)!=endheader)
00108 {
00109 if (fread(&uc,sizeof (urbi::UCommand),1,inf)!=1)
00110 {
00111 printf("error reading from file\n");
00112 exit(1);
00113 }
00114 fseek(inf,sizeof (urbi::UCommand)*(-2),SEEK_CUR);
00115 if (starttime==-1)
00116 starttime=uc.timestamp;
00117 uc.timestamp = starttime-uc.timestamp;
00118 fwrite(&uc,sizeof (urbi::UCommand),1,ouf);
00119 }
00120
00121 fclose(inf);
00122 fclose(ouf);
00123 }