00001 #include "urbi/uclient.hh"
00002
00003 #include <cassert>
00004 #include <signal.h>
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
00136
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 }