urbiplay.cc

00001 #include <csignal>
00002 #include "libport/windows.hh"
00003 
00004 #include "urbi/uclient.hh"
00005 
00006 enum UType
00007 {
00008   TYPE_BOOL,
00009   TYPE_ANGLE,
00010   TYPE_NORM
00011 };
00012 union UJointValue
00013 {
00014   float angle;
00015   float normalized;
00016   bool boolean;
00017 };
00018 struct UCommand
00019 {
00020   int timestamp;
00021   short id;
00022   UJointValue value;
00023 };
00024 
00025 struct UDev
00026 {
00027   char * name;
00028   short id;
00029   UType type;
00030 };
00031 UDev* devices;
00032 int devCount;
00033 char dumpMode;
00034 
00035 bool parseHeader(FILE *f)
00036 {
00037   char buff[4];
00038   if (fread(buff,4,1,f)!=1) return false;
00039   if (strncmp(buff,"URBI",4)) return false;
00040   if (fread(&devCount,4,1,f)!=1) return false;
00041   devices=static_cast<UDev*> (malloc (devCount*sizeof (UDev)));
00042   for (int i=0;i<devCount; ++i)
00043     {
00044       char device[256];
00045       int pos=0;
00046       do {
00047         if ((device[pos++]=fgetc(f)) == EOF)
00048           return false;
00049       } while (device[pos-1]);
00050       devices[i].name = strdup(device);
00051       if (fread(&devices[i].id,2,1,f)!=1)
00052         return false;
00053       int type;
00054       if ((type=fgetc(f)) == EOF)
00055         return false;
00056       devices[i].type=(UType)type;
00057     }
00058   return true;
00059 }
00060 
00061 void play(urbi::UClient * robot, FILE *f)
00062 {
00063   static int tick=0;
00064   static int ttime;
00065 
00066   UCommand uc;
00067   int starttime=0;
00068   bool commandPending = false;
00069   int lastCommandTime=0;
00070   while (fread(&uc,sizeof (uc),1,f)==1)
00071     {
00072       int sleeptime=uc.timestamp;
00073       if (!starttime)
00074         {
00075           starttime=robot->getCurrentTime()-sleeptime-1;
00076           lastCommandTime = 0;
00077         }
00078       int sleepstop=sleeptime+starttime; //when command should be executed in our timeframe
00079       //find the device
00080       UDev * dev=NULL;
00081       for (int i=0;i<devCount; ++i)
00082         if (devices[i].id==uc.id)
00083           {
00084             dev=&devices[i];
00085             break;
00086           }
00087       if (!dev)
00088         {
00089           fprintf(stderr,"device id %d not found\n",(int)uc.id);
00090           continue;
00091         }
00092       if (robot)
00093         {
00094           if (lastCommandTime != uc.timestamp)
00095             {
00096               if (commandPending)
00097                 robot->send("noop;");
00098               if (sleepstop-robot->getCurrentTime() > 500)
00099                 //queue no more than 500 ms in advance
00100                 usleep((sleepstop-robot->getCurrentTime()-500)*1000);
00101             }
00102           commandPending = true;
00103           lastCommandTime = uc.timestamp;
00104           robot->send("%s.val = %f&",dev->name,uc.value.angle);
00105         }
00106       else
00107         {
00108           if (dumpMode=='-')
00109             printf("%d %s.val = %f\n",sleepstop, dev->name,uc.value.angle);
00110           else
00111             {
00112               if ( uc.timestamp!=lastCommandTime)
00113                 printf("noop;\n");
00114               lastCommandTime = uc.timestamp;
00115               printf("%s.val = %f&\n", dev->name,uc.value.angle);
00116             }
00117         }
00118       if (!(tick%1000))
00119         {
00120           if (tick)
00121             fprintf( stderr,"%f cps\n",
00122                      1000000.0/(float)(robot->getCurrentTime()-ttime));
00123           ttime=robot->getCurrentTime();
00124         }
00125       ++tick;
00126     }
00127 
00128   if (robot && commandPending)
00129     {
00130       robot->send("noop;");
00131       commandPending = false;
00132     }
00133   if (!robot && dumpMode !='-')
00134     printf("noop;\n");
00135 }
00136 
00137 
00138 int main(int argc, char * argv[])
00139 {
00140   if (argc<3)
00141     {
00142     printf("usage: %s robot file [loop] \n\tPass '-' as 'robotname' to dump to stdout in human-readable format,\n\t or '+' to dump to stdout in urbi format.\n",argv[0]);
00143 
00144     exit(1);
00145   }
00146 
00147   int loop=0;
00148   if (argc>3)
00149     {
00150       loop=1;
00151       loop=strtol(argv[3],NULL,0);
00152     }
00153 
00154   urbi::UClient * robot;
00155 
00156   if ( (STREQ(argv[1],"-")) || (STREQ(argv[1],"+")))
00157     {
00158       robot=NULL;
00159       dumpMode=argv[1][0];
00160     }
00161   else
00162     {
00163       robot=new urbi::UClient(argv[1]);
00164       robot->start();
00165       if (robot->error()) exit(4);
00166     }
00167 
00168   if (robot)
00169     robot->send("motoron;");
00170 
00171   FILE * f;
00172   do {
00173     if (STREQ(argv[2],"-"))
00174       f=stdin;
00175     else
00176       f=fopen(argv[2],"r");
00177 
00178     if (!f)
00179       {
00180         fprintf(stderr, "error opening file\n");
00181         exit(2);
00182       }
00183     if (!parseHeader(f))
00184       {
00185         fprintf(stderr, "error parsing header\n");
00186         exit(3);
00187       }
00188 
00189     play(robot,f);
00190     fclose(f);
00191   }
00192   while (loop)
00193     ;
00194 }

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