urbicycle.cc

00001 #include "libport/cstdio"
00002 #include <cstdlib>
00003 #include <vector>
00004 
00005 #include "urbi/uclient.hh"
00006 
00007 inline float
00008 fabs(float f)
00009 {
00010   if (f > 0)
00011     return f;
00012   else
00013     return f * -1.0;
00014 }
00015 
00016 enum UType
00017 {
00018   TYPE_BOOL,
00019   TYPE_ANGLE,
00020   TYPE_NORM
00021 };
00022 
00023 union UJointValue
00024 {
00025   float angle;
00026   float normalized;
00027   bool boolean;
00028 };
00029 
00030 struct UCommand
00031 {
00032   int timestamp;
00033   short id;
00034   UJointValue value;
00035 };
00036 
00037 struct UDev
00038 {
00039   char * name;
00040   short id;
00041   UType type;
00042 };
00043 
00044 UDev* devices;
00045 int devCount;
00046 
00047 int parseHeader(FILE *f, FILE * of)
00048 {
00049   char buff[4];
00050   if (fread(buff, 4, 1, f) != 1) return 1;
00051   if (fwrite(buff, 4, 1, of) != 1) return 2;
00052   if (strncmp(buff, "URBI", 4)) return 3;
00053   if (fread(&devCount, 4, 1, f) != 1) return 4;
00054   if (fwrite(&devCount, 4, 1, of) != 1) return 5;
00055   for (int i = 0; i < devCount; ++i)
00056     {
00057       char device[256];
00058       int pos=0;
00059       int a;
00060       do {
00061         if ((device[pos++]=fgetc(f))==EOF) return 6;
00062       } while (device[pos-1]);
00063       if (fwrite(device, strlen(device)+1, 1, of)!=1) return 7;
00064       if (fread(&a, 2, 1, f)!=1) return 8;
00065       if (fwrite(&a, 2, 1, of)!=1) return 9;
00066       int type;
00067       if ((type=fgetc(f)) == EOF) return 10;
00068       fputc(type, of);
00069     }
00070   return 0;
00071 }
00072 
00073 int main(int argc, char * argv[])
00074 {
00075   if (argc<3)
00076   {
00077     printf("usage %s "
00078            "infile outfile [jointid] [startval] [direction] [numcycle]\n"
00079            "\tDetect and extract cycles in an urbi recorded file\n"
00080            "\tJointid is the joint used to detect cycles (0-based value,"
00081            " see urbirecord.cpp for id/name correspondance)\n"
00082            "\tStartval is the value that will mark the beginning of a cycle"
00083            " when reached by the joint, in the direction defined by the"
00084            " 'direction' parameter\n"
00085            "\tnumcycle is the number of the cycle that will be written to"
00086            " 'outfile'\n"
00087            "\tSet startval to '-' for 'first value seen'\n", argv[0]);
00088     exit(1);
00089   }
00090 
00091   FILE* inf = 0;
00092   FILE* ouf = 0;
00093 
00094   if (STREQ(argv[1], "-"))
00095     inf = stdin;
00096   else
00097     inf = fopen(argv[1], "r");
00098 
00099   if (!inf)
00100   {
00101     printf("error opening file\n");
00102     exit(2);
00103   }
00104 
00105   if (STREQ(argv[2], "-"))
00106     ouf = stdout;
00107   else
00108     ouf = fopen(argv[2], "w");
00109   if (!ouf)
00110   {
00111     printf("error opening file\n");
00112     exit(2);
00113   }
00114 
00115   if (int a=parseHeader(inf, ouf))
00116   {
00117     printf("error parsing header: %d\n", a);
00118     exit(3);
00119   }
00120 
00121   int joint = 0;
00122   int wantedcycle = 2;
00123   float startval = 0.0;
00124   bool init = false; //not found yet startval on joint
00125   bool gotSign = false;
00126   bool gotLastVal = false;
00127   bool cyclesgn = false;
00128 
00129   if (argc > 3)
00130     sscanf(argv[3], "%d", &joint);
00131   if (argc > 4)
00132   {
00133     if (STREQ(argv[4], "-"))
00134       init = true;
00135     else
00136       sscanf(argv[4], "%f", &startval);
00137   }
00138   if (argc > 5)
00139   {
00140     int v = 0;
00141     sscanf(argv[5], "%d", &v);
00142     if (v != 0)
00143     {
00144       gotSign = true;
00145       cyclesgn = (v < 0);
00146     }
00147   }
00148   if (argc > 6)
00149     sscanf(argv[6], "%d", &wantedcycle);
00150   UCommand uc;
00151 
00152   float lastval = 0.0;
00153   std::vector<UCommand> buff (devCount);
00154   int cycle = 0;
00155 
00156   int buffTime = 0;
00157 
00158   int basetime = 0;
00159   for (int i = 0; i < devCount; ++i)
00160     buff[i].timestamp = 0;
00161   //read and handle by block of commands with same timestamp.
00162   //init:
00163   fread(&uc, sizeof (UCommand), 1, inf);
00164   buffTime = uc.timestamp;
00165   buff[uc.id] = uc;
00166   while (true)
00167   {
00168     int ok = fread(&uc, sizeof (UCommand), 1, inf);
00169     if (ok && !basetime)
00170       basetime = uc.timestamp;
00171     if (ok && buffTime == 0)
00172       buffTime = uc.timestamp;
00173     if (ok && uc.timestamp == buffTime)
00174     {
00175       buff[uc.id] = uc;
00176       continue;
00177     }
00178 
00179     if (init)
00180     {
00181       //initialize asap<-now
00182 
00183       if (buff[joint].timestamp == 0)
00184       {
00185         //cant do anything
00186         for (int i = 0; i < devCount; ++i)
00187           buff[i].timestamp = 0;
00188         buff[uc.id] = uc;
00189         buffTime = 0;
00190         continue;
00191       }
00192 
00193       startval = buff[joint].value.angle;
00194       gotSign = false;
00195       init = false;
00196       gotLastVal = true;
00197       lastval = startval;
00198       ++cycle;
00199       fprintf(stderr, "cycle %d starts at %d\n", cycle, buffTime - basetime);
00200     }
00201 
00202 
00203     if (gotLastVal
00204         && (!gotSign || (cyclesgn ^ (lastval<startval)))
00205         && ((lastval < startval && buff[joint].value.angle >= startval)
00206             || (lastval > startval && buff[joint].value.angle <= startval)))
00207     {
00208       cyclesgn = (lastval > startval);
00209       gotSign = true;
00210       ++cycle;
00211       fprintf(stderr, "cycle %d starts at %d\n", cycle, buffTime - basetime);
00212     }
00213 
00214     if (buff[joint].timestamp != 0)
00215     {
00216       lastval = buff[joint].value.angle;
00217       gotLastVal = true;
00218     }
00219     if (cycle == wantedcycle)
00220       for (int i = 0; i < devCount; ++i)
00221         if (buff[i].timestamp!=0)
00222         {
00223           buff[i].timestamp -= basetime;
00224           fwrite(&buff[i], sizeof (UCommand), 1, ouf);
00225         }
00226 
00227     //flush buffer
00228     for (int i = 0; i < devCount; ++i)
00229       buff[i].timestamp = 0;
00230     buff[uc.id] = uc;
00231     buffTime = 0;
00232     if (!ok)
00233       break;
00234   }
00235   fclose(inf);
00236   fclose(ouf);
00237 }

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