urbiscale.cc

00001 #include <cstdlib>
00002 #include "libport/cstdio"
00003 #include <vector>
00004 
00005 #include "urbi/uclient.hh"
00006 
00007 inline float fabs(float f)
00008 {
00009   if (f > 0)
00010     return f;
00011   return f * -1.0;
00012 }
00013 
00014 enum UType
00015 {
00016   TYPE_BOOL,
00017   TYPE_ANGLE,
00018   TYPE_NORM
00019 };
00020 
00021 union UJointValue
00022 {
00023   float angle;
00024   float normalized;
00025   bool boolean;
00026 };
00027 
00028 struct UCommand
00029 {
00030   int timestamp;
00031   short id;
00032   UJointValue value;
00033 };
00034 
00035 struct UDev
00036 {
00037   char* name;
00038   short id;
00039   UType type;
00040 };
00041 UDev* devices;
00042 int devCount;
00043 
00044 int parseHeader(FILE *f, FILE * of)
00045 {
00046   char buff[4];
00047   if (fread(buff, 4, 1, f) != 1)
00048     return 1;
00049   if (fwrite(buff, 4, 1, of) != 1)
00050     return 2;
00051   if (strncmp(buff, "URBI", 4))
00052     return 3;
00053   if (fread(&devCount, 4, 1, f) != 1)
00054     return 4;
00055   if (fwrite(&devCount, 4, 1, of) != 1)
00056     return 5;
00057 
00058   for (int i = 0; i < devCount; ++i)
00059   {
00060     char device[256];
00061     int pos = 0;
00062     int a = 0;
00063     int r = EOF;
00064     do {
00065       if ((r = fgetc(f)) == EOF)
00066         return 6;
00067       device[pos++] = r;
00068     }
00069     while (device[pos - 1]);
00070 
00071     if (fwrite(device, strlen(device) + 1, 1, of) != 1)
00072       return 7;
00073     if (fread(&a, 2, 1, f) != 1)
00074       return 8;
00075     if (fwrite(&a, 2, 1, of) != 1)
00076       return 9;
00077     int type;
00078     if ((type = fgetc(f)) == EOF)
00079       return 10;
00080     fputc(type, of);
00081   }
00082   return 0;
00083 }
00084 
00085 int main(int argc, char* argv[])
00086 {
00087   //cut static part of an urbi file
00088   if (argc < 4)
00089   {
00090     printf("usage %s infile outfile factor [interpolate]\n"
00091            "\ttime-scale the urbi file by given factor (>1 means slow-down) \n"
00092            "\tInterpolation will only work with integer factors\n", argv[0]);
00093     exit(1);
00094   }
00095   FILE* inf;
00096   FILE* ouf;
00097 
00098   if (STREQ(argv[1], "-"))
00099     inf = stdin;
00100   else
00101     inf = fopen(argv[1], "r");
00102   if (!inf)
00103   {
00104     printf("error opening file\n");
00105     exit(2);
00106   }
00107   if (STREQ(argv[2], "-"))
00108     ouf = stdout;
00109   else
00110     ouf = fopen(argv[2], "w");
00111   if (!ouf)
00112   {
00113     printf("error opening file\n");
00114     exit(2);
00115   }
00116   if (int a = parseHeader(inf, ouf))
00117   {
00118     printf("error parsing header: %d\n", a);
00119     exit(3);
00120   }
00121 
00122   float scale = 2;
00123   int interpolate = 0;
00124   if (argc > 4)
00125     interpolate = strtol(argv[4], NULL, 0);
00126   printf("interpolation set to %d\n", interpolate);
00127   //if no move greater than atresh cut the line
00128   sscanf(argv[3], "%f", &scale);
00129   UCommand uc;
00130   int starttime = -1;
00131   std::vector<UCommand> lastuc (devCount);
00132   std::vector<UCommand> nextuc (devCount);
00133   for (int i = 0; i < devCount; ++i)
00134   {
00135     lastuc[i].timestamp = -1;
00136   }
00137   for (int i = 0; i < devCount; ++i)
00138   {
00139     nextuc[i].timestamp =- 1;
00140   }
00141 
00142   int inittime = 0;
00143   bool first_time = true;
00144   while (fread(&uc, sizeof (UCommand), 1, inf) == 1)
00145   {
00146     if (starttime == -1)
00147       starttime = uc.timestamp;
00148     if (interpolate && (lastuc[uc.id].timestamp == -1))
00149     {
00150       if (first_time == true)
00151       {
00152         inittime = uc.timestamp;
00153         first_time = false;
00154       }
00155       fprintf(stderr, "first command for %d at %d   (inittime %d)\n",
00156               uc.id, uc.timestamp, inittime);
00157       if (inittime != uc.timestamp)
00158       {
00159         // trouble, missed some commands at start
00160         lastuc[uc.id] = uc;
00161         lastuc[uc.id].timestamp = (int) (((float)(inittime) - ((float)starttime)) * scale);
00162         fprintf(stderr, "miss: adding command for id %d at time %d\n",
00163                 uc.id, lastuc[uc.id].timestamp);
00164         //go on processing uc
00165       }
00166       else
00167       {
00168         uc.timestamp = (int) (((float)(uc.timestamp) - ((float)starttime)) * scale);
00169         fwrite(&uc, sizeof (UCommand), 1, ouf);
00170         lastuc[uc.id] = uc;
00171         continue;
00172       }
00173     }
00174     uc.timestamp = (int) (((float)(uc.timestamp) - ((float)starttime)) * scale);
00175     if (interpolate)
00176     {
00177       if (nextuc[uc.id].timestamp != -1)
00178       {
00179         //flush all!
00180         fprintf(stderr, "flush, %d -1 steps\n", (int) scale);
00181         for (int step = 1; step < (int)scale; ++step)
00182         {
00183           //step 0 already sent, last step will be sent after
00184           for (int dev = 0; dev < devCount; ++dev)
00185           {
00186             if (nextuc[dev].timestamp == -1)
00187               continue;
00188             UCommand suc = lastuc[dev];
00189             suc.timestamp = (lastuc[dev].timestamp * ((int)scale-step) + nextuc[dev].timestamp * step) / (int)scale;
00190             suc.value.angle = (lastuc[dev].value.angle * (float)((int)scale-step) + nextuc[dev].value.angle * (float)step) / (float)((int)scale);
00191             fwrite(&suc, sizeof (UCommand), 1, ouf);
00192           }
00193         }
00194         //send the last
00195         for (int dev = 0; dev < devCount; ++dev)
00196         {
00197           if (nextuc[dev].timestamp == -1)
00198             continue;
00199           fwrite(&nextuc[dev], sizeof (UCommand), 1, ouf);
00200           lastuc[dev] = nextuc[dev];
00201           nextuc[dev].timestamp = -1;
00202         }
00203       }
00204       nextuc[uc.id] = uc;
00205     }
00206     else
00207     {
00208       fwrite(&uc, sizeof (UCommand), 1, ouf);
00209     }
00210   }
00211   //send the last chunk
00212   if (interpolate)
00213     for (int dev = 0; dev < devCount; ++dev)
00214     {
00215       if (nextuc[dev].timestamp == -1)
00216         continue;
00217       fwrite(&nextuc[dev], sizeof (UCommand), 1, ouf);
00218     }
00219 
00220   fclose(inf);
00221   fclose(ouf);
00222 }

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