00001 #include "libport/cstdio"
00002 #include <cstdlib>
00003 #include <vector>
00004
00005 #include "urbi/uclient.hh"
00006
00007 inline float
00008 fabs(float f)
00009 {
00010 if (f > 0)
00011 return f;
00012 else
00013 return f * -1.0;
00014 }
00015
00016 enum UType
00017 {
00018 TYPE_BOOL,
00019 TYPE_ANGLE,
00020 TYPE_NORM
00021 };
00022
00023 union UJointValue
00024 {
00025 float angle;
00026 float normalized;
00027 bool boolean;
00028 };
00029
00030 struct UCommand
00031 {
00032 int timestamp;
00033 short id;
00034 UJointValue value;
00035 };
00036
00037 struct UDev
00038 {
00039 char * name;
00040 short id;
00041 UType type;
00042 };
00043
00044 UDev* devices;
00045 int devCount;
00046
00047 int parseHeader(FILE *f, FILE * of)
00048 {
00049 char buff[4];
00050 if (fread(buff, 4, 1, f) != 1) return 1;
00051 if (fwrite(buff, 4, 1, of) != 1) return 2;
00052 if (strncmp(buff, "URBI", 4)) return 3;
00053 if (fread(&devCount, 4, 1, f) != 1) return 4;
00054 if (fwrite(&devCount, 4, 1, of) != 1) return 5;
00055 for (int i = 0; i < devCount; ++i)
00056 {
00057 char device[256];
00058 int pos=0;
00059 int a;
00060 do {
00061 if ((device[pos++]=fgetc(f))==EOF) return 6;
00062 } while (device[pos-1]);
00063 if (fwrite(device, strlen(device)+1, 1, of)!=1) return 7;
00064 if (fread(&a, 2, 1, f)!=1) return 8;
00065 if (fwrite(&a, 2, 1, of)!=1) return 9;
00066 int type;
00067 if ((type=fgetc(f)) == EOF) return 10;
00068 fputc(type, of);
00069 }
00070 return 0;
00071 }
00072
00073 int main(int argc, char * argv[])
00074 {
00075 if (argc<3)
00076 {
00077 printf("usage %s "
00078 "infile outfile [jointid] [startval] [direction] [numcycle]\n"
00079 "\tDetect and extract cycles in an urbi recorded file\n"
00080 "\tJointid is the joint used to detect cycles (0-based value,"
00081 " see urbirecord.cpp for id/name correspondance)\n"
00082 "\tStartval is the value that will mark the beginning of a cycle"
00083 " when reached by the joint, in the direction defined by the"
00084 " 'direction' parameter\n"
00085 "\tnumcycle is the number of the cycle that will be written to"
00086 " 'outfile'\n"
00087 "\tSet startval to '-' for 'first value seen'\n", argv[0]);
00088 exit(1);
00089 }
00090
00091 FILE* inf = 0;
00092 FILE* ouf = 0;
00093
00094 if (STREQ(argv[1], "-"))
00095 inf = stdin;
00096 else
00097 inf = fopen(argv[1], "r");
00098
00099 if (!inf)
00100 {
00101 printf("error opening file\n");
00102 exit(2);
00103 }
00104
00105 if (STREQ(argv[2], "-"))
00106 ouf = stdout;
00107 else
00108 ouf = fopen(argv[2], "w");
00109 if (!ouf)
00110 {
00111 printf("error opening file\n");
00112 exit(2);
00113 }
00114
00115 if (int a=parseHeader(inf, ouf))
00116 {
00117 printf("error parsing header: %d\n", a);
00118 exit(3);
00119 }
00120
00121 int joint = 0;
00122 int wantedcycle = 2;
00123 float startval = 0.0;
00124 bool init = false;
00125 bool gotSign = false;
00126 bool gotLastVal = false;
00127 bool cyclesgn = false;
00128
00129 if (argc > 3)
00130 sscanf(argv[3], "%d", &joint);
00131 if (argc > 4)
00132 {
00133 if (STREQ(argv[4], "-"))
00134 init = true;
00135 else
00136 sscanf(argv[4], "%f", &startval);
00137 }
00138 if (argc > 5)
00139 {
00140 int v = 0;
00141 sscanf(argv[5], "%d", &v);
00142 if (v != 0)
00143 {
00144 gotSign = true;
00145 cyclesgn = (v < 0);
00146 }
00147 }
00148 if (argc > 6)
00149 sscanf(argv[6], "%d", &wantedcycle);
00150 UCommand uc;
00151
00152 float lastval = 0.0;
00153 std::vector<UCommand> buff (devCount);
00154 int cycle = 0;
00155
00156 int buffTime = 0;
00157
00158 int basetime = 0;
00159 for (int i = 0; i < devCount; ++i)
00160 buff[i].timestamp = 0;
00161
00162
00163 fread(&uc, sizeof (UCommand), 1, inf);
00164 buffTime = uc.timestamp;
00165 buff[uc.id] = uc;
00166 while (true)
00167 {
00168 int ok = fread(&uc, sizeof (UCommand), 1, inf);
00169 if (ok && !basetime)
00170 basetime = uc.timestamp;
00171 if (ok && buffTime == 0)
00172 buffTime = uc.timestamp;
00173 if (ok && uc.timestamp == buffTime)
00174 {
00175 buff[uc.id] = uc;
00176 continue;
00177 }
00178
00179 if (init)
00180 {
00181
00182
00183 if (buff[joint].timestamp == 0)
00184 {
00185
00186 for (int i = 0; i < devCount; ++i)
00187 buff[i].timestamp = 0;
00188 buff[uc.id] = uc;
00189 buffTime = 0;
00190 continue;
00191 }
00192
00193 startval = buff[joint].value.angle;
00194 gotSign = false;
00195 init = false;
00196 gotLastVal = true;
00197 lastval = startval;
00198 ++cycle;
00199 fprintf(stderr, "cycle %d starts at %d\n", cycle, buffTime - basetime);
00200 }
00201
00202
00203 if (gotLastVal
00204 && (!gotSign || (cyclesgn ^ (lastval<startval)))
00205 && ((lastval < startval && buff[joint].value.angle >= startval)
00206 || (lastval > startval && buff[joint].value.angle <= startval)))
00207 {
00208 cyclesgn = (lastval > startval);
00209 gotSign = true;
00210 ++cycle;
00211 fprintf(stderr, "cycle %d starts at %d\n", cycle, buffTime - basetime);
00212 }
00213
00214 if (buff[joint].timestamp != 0)
00215 {
00216 lastval = buff[joint].value.angle;
00217 gotLastVal = true;
00218 }
00219 if (cycle == wantedcycle)
00220 for (int i = 0; i < devCount; ++i)
00221 if (buff[i].timestamp!=0)
00222 {
00223 buff[i].timestamp -= basetime;
00224 fwrite(&buff[i], sizeof (UCommand), 1, ouf);
00225 }
00226
00227
00228 for (int i = 0; i < devCount; ++i)
00229 buff[i].timestamp = 0;
00230 buff[uc.id] = uc;
00231 buffTime = 0;
00232 if (!ok)
00233 break;
00234 }
00235 fclose(inf);
00236 fclose(ouf);
00237 }