urbirecord.cc

00001 #include "urbi/uclient.hh"
00002 
00003 #include <cassert>
00004 #include <signal.h>
00005 
00006 
00007 /*
00008   file format
00009   URBI (4 bytes)
00010   numjoints (4 bytes)
00011   [numjoint times] namejoint (char * nullterminated) id (short)  type(char)
00012   [til end of file]
00013     timestamp [int, milisec] id (short) value (UValue)
00014  */
00015 
00016 enum UType
00017 {
00018   TYPE_BOOL,
00019   TYPE_ANGLE,
00020   TYPE_NORM
00021 };
00022 union UJointValue
00023 {
00024   float angle;
00025   float normalized;
00026   bool boolean;
00027 };
00028 struct UCommand
00029 {
00030   int timestamp;
00031   short id;
00032   UJointValue value;
00033 };
00034 
00035 
00036 
00037 char * devices[]=
00038 {
00039   "legLF1",
00040   "legLF2",
00041   "legLF3",
00042   "legLH1",
00043   "legLH2",
00044   "legLH3",
00045   "legRH1",
00046   "legRH2",
00047   "legRH3",
00048   "legRF1",
00049   "legRF2",
00050   "legRF3",
00051   "neck",
00052   "headPan",
00053   "headTilt",
00054   "tailTilt",
00055   "tailPan",
00056   "mouth"
00057 };
00058 int devCount=18;
00059 FILE *f;
00060 int tilt=0;
00061 void buildHeader()
00062 {
00063   fwrite("URBI",4,1,f);
00064   fwrite(&devCount,4,1,f);
00065   for (int i=0;i<devCount; ++i)
00066         {
00067           fwrite(devices[i],strlen(devices[i])+1,1,f);
00068           short s=i;
00069           fwrite(&s,2,1,f);
00070           char c=(char)TYPE_ANGLE;
00071           fwrite(&c,1,1,f);
00072         }
00073 }
00074 
00075 
00076 
00077 
00078 urbi::UCallbackAction
00079 command(const urbi::UMessage &msg)
00080 {
00081   //get command id
00082   static int tme=0;
00083 
00084   for (int i=0;i<devCount; ++i)
00085     {
00086       if (msg.tag != devices[i])
00087         {
00088           UCommand uc;
00089           uc.timestamp=msg.timestamp;
00090           uc.id=i;
00091           assert (msg.type == urbi::MESSAGE_DATA);
00092           assert (msg.value->type == urbi::DATA_DOUBLE);
00093           uc.value.angle=msg.value->val;
00094 
00095           fwrite(&uc,sizeof (UCommand),1,f);
00096           ++tilt;
00097           if (! (tilt%100))
00098             {
00099               if (tme)
00100                 fprintf(stderr, ". %f cps\n",
00101                         100000.0/(float)(msg.client.getCurrentTime()-tme));
00102               tme=msg.client.getCurrentTime();
00103             }
00104           return urbi::URBI_CONTINUE;
00105         }
00106     }
00107   fprintf (stderr, "error: no device %s\n", msg.tag.c_str ());
00108   return urbi::URBI_CONTINUE;
00109 
00110 }
00111 
00112 void endRecord(int)
00113 {
00114   fclose(f);
00115   exit(0);
00116 }
00117 
00118 int main(int argc, char * argv[])
00119 {
00120   if (argc != 3)
00121     {
00122       printf("usage: %s robotname file\n\t Records a sequence of movements to a file.\n",argv[0]);
00123       exit(1);
00124     }
00125   signal(SIGINT,endRecord);
00126   urbi::UClient c(argv[1]);
00127   c.start();
00128   if (c.error()) exit(2);
00129   if (STREQ(argv[2],"-")) f=stdout;
00130   else
00131     f=fopen(argv[2],"w");
00132   if (!f)
00133     exit(3);
00134   buildHeader();
00135   //c.send("motoroff");
00136   //build command
00137   c.send("looptag: loop {");
00138   for (int i=0;i<devCount-1; ++i)
00139   {
00140     c.setCallback(command,devices[i]);
00141     c.send("%s: %s.val&",devices[i], devices[i]);
00142   }
00143   c.setCallback(command,devices[devCount-1]);
00144   c.send("%s: %s.val},",devices[devCount-1], devices[devCount-1]);
00145 
00146 
00147   fprintf(stderr,"starting, hit ctrl-c to stop...\n");
00148   urbi::execute();
00149 }

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