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;
00079
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
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 }