00001 #include <cstdlib>
00002 #include "libport/cstdio"
00003 #include <vector>
00004
00005 #include "urbi/uclient.hh"
00006
00007 inline float fabs(float f)
00008 {
00009 if (f > 0)
00010 return f;
00011 return f * -1.0;
00012 }
00013
00014 enum UType
00015 {
00016 TYPE_BOOL,
00017 TYPE_ANGLE,
00018 TYPE_NORM
00019 };
00020
00021 union UJointValue
00022 {
00023 float angle;
00024 float normalized;
00025 bool boolean;
00026 };
00027
00028 struct UCommand
00029 {
00030 int timestamp;
00031 short id;
00032 UJointValue value;
00033 };
00034
00035 struct UDev
00036 {
00037 char* name;
00038 short id;
00039 UType type;
00040 };
00041 UDev* devices;
00042 int devCount;
00043
00044 int parseHeader(FILE *f, FILE * of)
00045 {
00046 char buff[4];
00047 if (fread(buff, 4, 1, f) != 1)
00048 return 1;
00049 if (fwrite(buff, 4, 1, of) != 1)
00050 return 2;
00051 if (strncmp(buff, "URBI", 4))
00052 return 3;
00053 if (fread(&devCount, 4, 1, f) != 1)
00054 return 4;
00055 if (fwrite(&devCount, 4, 1, of) != 1)
00056 return 5;
00057
00058 for (int i = 0; i < devCount; ++i)
00059 {
00060 char device[256];
00061 int pos = 0;
00062 int a = 0;
00063 int r = EOF;
00064 do {
00065 if ((r = fgetc(f)) == EOF)
00066 return 6;
00067 device[pos++] = r;
00068 }
00069 while (device[pos - 1]);
00070
00071 if (fwrite(device, strlen(device) + 1, 1, of) != 1)
00072 return 7;
00073 if (fread(&a, 2, 1, f) != 1)
00074 return 8;
00075 if (fwrite(&a, 2, 1, of) != 1)
00076 return 9;
00077 int type;
00078 if ((type = fgetc(f)) == EOF)
00079 return 10;
00080 fputc(type, of);
00081 }
00082 return 0;
00083 }
00084
00085 int main(int argc, char* argv[])
00086 {
00087
00088 if (argc < 4)
00089 {
00090 printf("usage %s infile outfile factor [interpolate]\n"
00091 "\ttime-scale the urbi file by given factor (>1 means slow-down) \n"
00092 "\tInterpolation will only work with integer factors\n", argv[0]);
00093 exit(1);
00094 }
00095 FILE* inf;
00096 FILE* ouf;
00097
00098 if (STREQ(argv[1], "-"))
00099 inf = stdin;
00100 else
00101 inf = fopen(argv[1], "r");
00102 if (!inf)
00103 {
00104 printf("error opening file\n");
00105 exit(2);
00106 }
00107 if (STREQ(argv[2], "-"))
00108 ouf = stdout;
00109 else
00110 ouf = fopen(argv[2], "w");
00111 if (!ouf)
00112 {
00113 printf("error opening file\n");
00114 exit(2);
00115 }
00116 if (int a = parseHeader(inf, ouf))
00117 {
00118 printf("error parsing header: %d\n", a);
00119 exit(3);
00120 }
00121
00122 float scale = 2;
00123 int interpolate = 0;
00124 if (argc > 4)
00125 interpolate = strtol(argv[4], NULL, 0);
00126 printf("interpolation set to %d\n", interpolate);
00127
00128 sscanf(argv[3], "%f", &scale);
00129 UCommand uc;
00130 int starttime = -1;
00131 std::vector<UCommand> lastuc (devCount);
00132 std::vector<UCommand> nextuc (devCount);
00133 for (int i = 0; i < devCount; ++i)
00134 {
00135 lastuc[i].timestamp = -1;
00136 }
00137 for (int i = 0; i < devCount; ++i)
00138 {
00139 nextuc[i].timestamp =- 1;
00140 }
00141
00142 int inittime = 0;
00143 bool first_time = true;
00144 while (fread(&uc, sizeof (UCommand), 1, inf) == 1)
00145 {
00146 if (starttime == -1)
00147 starttime = uc.timestamp;
00148 if (interpolate && (lastuc[uc.id].timestamp == -1))
00149 {
00150 if (first_time == true)
00151 {
00152 inittime = uc.timestamp;
00153 first_time = false;
00154 }
00155 fprintf(stderr, "first command for %d at %d (inittime %d)\n",
00156 uc.id, uc.timestamp, inittime);
00157 if (inittime != uc.timestamp)
00158 {
00159
00160 lastuc[uc.id] = uc;
00161 lastuc[uc.id].timestamp = (int) (((float)(inittime) - ((float)starttime)) * scale);
00162 fprintf(stderr, "miss: adding command for id %d at time %d\n",
00163 uc.id, lastuc[uc.id].timestamp);
00164
00165 }
00166 else
00167 {
00168 uc.timestamp = (int) (((float)(uc.timestamp) - ((float)starttime)) * scale);
00169 fwrite(&uc, sizeof (UCommand), 1, ouf);
00170 lastuc[uc.id] = uc;
00171 continue;
00172 }
00173 }
00174 uc.timestamp = (int) (((float)(uc.timestamp) - ((float)starttime)) * scale);
00175 if (interpolate)
00176 {
00177 if (nextuc[uc.id].timestamp != -1)
00178 {
00179
00180 fprintf(stderr, "flush, %d -1 steps\n", (int) scale);
00181 for (int step = 1; step < (int)scale; ++step)
00182 {
00183
00184 for (int dev = 0; dev < devCount; ++dev)
00185 {
00186 if (nextuc[dev].timestamp == -1)
00187 continue;
00188 UCommand suc = lastuc[dev];
00189 suc.timestamp = (lastuc[dev].timestamp * ((int)scale-step) + nextuc[dev].timestamp * step) / (int)scale;
00190 suc.value.angle = (lastuc[dev].value.angle * (float)((int)scale-step) + nextuc[dev].value.angle * (float)step) / (float)((int)scale);
00191 fwrite(&suc, sizeof (UCommand), 1, ouf);
00192 }
00193 }
00194
00195 for (int dev = 0; dev < devCount; ++dev)
00196 {
00197 if (nextuc[dev].timestamp == -1)
00198 continue;
00199 fwrite(&nextuc[dev], sizeof (UCommand), 1, ouf);
00200 lastuc[dev] = nextuc[dev];
00201 nextuc[dev].timestamp = -1;
00202 }
00203 }
00204 nextuc[uc.id] = uc;
00205 }
00206 else
00207 {
00208 fwrite(&uc, sizeof (UCommand), 1, ouf);
00209 }
00210 }
00211
00212 if (interpolate)
00213 for (int dev = 0; dev < devCount; ++dev)
00214 {
00215 if (nextuc[dev].timestamp == -1)
00216 continue;
00217 fwrite(&nextuc[dev], sizeof (UCommand), 1, ouf);
00218 }
00219
00220 fclose(inf);
00221 fclose(ouf);
00222 }