urbireverse.cc

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 } // namespace urbi
00069 
00070 int main(int argc, char * argv[])
00071 {
00072   //cut static part of an urbi file
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 }

Generated on Tue Apr 10 17:45:45 2007 for URBISDK by  doxygen 1.5.1