00001 #include "libport/sys/stat.h"
00002 #include <cstdlib>
00003 #include "libport/cstdio"
00004 #include <cmath>
00005 #include <iostream>
00006 #include <vector>
00007 #include <locale.h>
00008 #include <cassert>
00009
00010 #include "move.hh"
00011
00012 #ifdef MAX_PATH
00013 # define M_MAX_PATH MAX_PATH
00014 #else
00015 # define M_MAX_PATH 1024
00016 #endif
00017
00018 namespace urbi
00019 {
00020 inline float ffloatpart(float a)
00021 {
00022 return a - (int) a;
00023 }
00024
00025 static char* dirname(char* src)
00026 {
00027 static char result[M_MAX_PATH];
00028 if (!strchr(src, '/'))
00029 {
00030 strcpy(result, ".");
00031 return result;
00032 }
00033 strcpy(result, src);
00034 size_t p;
00035 for (p = strlen(result) - 1; result[p] != '/'; --p)
00036 ;
00037 result[p] = 0;
00038 return result;
00039 }
00040
00041 int Move::initialize(UClient* client,
00042 bool uploadFiles,
00043 const char* configFile,
00044 bool enableInterrupt)
00045 {
00046
00047 setlocale(LC_ALL, "C");
00048 walks.clear();
00049 turns.clear();
00050 robot = client;
00051 robot->makeUniqueTag(tag);
00052 robot->makeUniqueTag(execTag);
00053 if (enableInterrupt)
00054 {
00055 interruptConnection = new UClient(robot->getServerName());
00056 }
00057 else
00058 interruptConnection = 0;
00059 strcat(tag, "mov");
00060 FILE * cf = fopen(configFile, "r");
00061 if (!cf)
00062 {
00063 fprintf(stderr, "Failed to open main configuration file %s\n",
00064 configFile);
00065 return 1;
00066 }
00067 char configPath[M_MAX_PATH];
00068 char filePath[M_MAX_PATH];
00069 strcpy(configPath, configFile);
00070 char* path = dirname(configPath);
00071 char line[1024];
00072 int idx = 0;
00073
00074 while (fgets(line, 1024, cf))
00075 {
00076 char type[64];
00077 char name[M_MAX_PATH];
00078 LoadedFile lf;
00079 if (line[0] == '\n' || line[0] == 0)
00080 continue;
00081 if (line[strlen(line) - 1] == '\n')
00082 line[strlen(line) - 1] = 0;
00083 int pos = 0;
00084 while (line[pos] == ' ')
00085 ++pos;
00086 if (line[pos] == 0)
00087 continue;
00088 static const char format[] = "%s%s%f%f%f";
00089 int s = sscanf(line, format,
00090 type, name, &lf.value, &lf.speed, &lf.precision);
00091 if (s >= 1 && type[0] == '#')
00092 continue;
00093 if (s >= 1 && type[0] == 0)
00094 continue;
00095 if (s != 5)
00096 {
00097 fprintf(stderr, "parse error at line %s, skipping\n", line);
00098 continue;
00099 }
00100
00101 sprintf(lf.name, "mov%d", idx++);
00102
00103 if (uploadFiles)
00104 {
00105 if (name[0] != '/')
00106 {
00107
00108 strcpy(filePath, path);
00109 strcat(filePath, "/");
00110 strcat(filePath, name);
00111 }
00112 else
00113 strcpy(filePath, name);
00114 FILE* cmdf = fopen(filePath, "r");
00115 if (!cmdf)
00116 {
00117 fprintf(stderr, "cannot open file %s, skipping\n", name);
00118 continue;
00119 }
00120
00121 struct stat st;
00122 stat(filePath, &st);
00123 assert (st.st_size >= 0);
00124 size_t filelength = st.st_size;
00125 char* buffer = static_cast<char*> (malloc (filelength + 200));
00126
00127 size_t left = filelength;
00128 while (left)
00129 {
00130 size_t rc = fread(buffer, 1, left, cmdf);
00131 if (!rc)
00132 {
00133 fprintf(stderr, "error reading file %s\n", name);
00134 return 3;
00135 }
00136 left -= rc;
00137 }
00138 sprintf(&buffer[filelength], "%s :ping;", tag);
00139 filelength += strlen(tag) + strlen(" :ping;");
00140 fclose(cmdf);
00141 char block[101];
00142 block[100] = 0;
00143 size_t offset = 0;
00144
00145 robot->send("%s = \"%s: {\";", lf.name, execTag);
00146 while (filelength > offset)
00147 {
00148
00149
00150
00151 strncpy(block, &buffer[offset], 100);
00152 offset += 100;
00153 robot->send("%s = %s + \"%s\";", lf.name, lf.name, block);
00154 }
00155 robot->send("%s = %s + \"};\";", lf.name, lf.name);
00156 free(buffer);
00157 }
00158
00159 if (tolower(type[0]) == 'w')
00160 walks.push_back(lf);
00161 else
00162 turns.push_back(lf);
00163 }
00164 fclose(cf);
00165
00166
00167 robot->setCallback(*this, &Move::moveEnd, tag);
00168
00169 pwalk.minSpeed = pwalk.resolution = pwalk.precision = 300000000;
00170 pwalk.maxSpeed = 0;
00171 for (std::list<LoadedFile>::iterator it = walks.begin();
00172 it != walks.end();
00173 ++it)
00174 {
00175 if (it->speed > pwalk.maxSpeed)
00176 pwalk.maxSpeed = it->speed;
00177 if (it->speed < pwalk.minSpeed)
00178 pwalk.minSpeed = it->speed;
00179 if (it->value < pwalk.resolution)
00180 {
00181 pwalk.resolution = it->value;
00182 pwalk.precision = it->precision;
00183 }
00184 }
00185 pturn.minSpeed = pturn.resolution = pturn.precision = 300000000;
00186 pturn.maxSpeed = 0;
00187 for (std::list<LoadedFile>::iterator it = turns.begin();
00188 it != turns.end();
00189 ++it)
00190 {
00191 if (it->speed > pturn.maxSpeed)
00192 pturn.maxSpeed = it->speed;
00193 if (it->speed < pturn.minSpeed)
00194 pturn.minSpeed = it->speed;
00195 if (it->value < pturn.resolution)
00196 {
00197 pturn.resolution = it->value;
00198 pturn.precision = it->precision;
00199 }
00200 }
00201 moving = 0;
00202 return 0;
00203 };
00204
00205 void Move::interrupt(bool notifyEndMove)
00206 {
00207 if (!interruptConnection)
00208 return;
00209 if (moving <= 0)
00210 return;
00211 moving = -2;
00212
00213 interruptConnection->send("stop %s;", execTag);
00214 if (notifyEndMove)
00215 robot->send("%s: ping;", tag);
00216 }
00217
00218 int Move::walk(float& distance, float precision, const char* tag)
00219 {
00220 float absoluteprecision = fabs(distance * precision);
00221 const unsigned int length = walks.size();
00222
00223
00224 std::vector<std::vector<float> > values (length / 2U,
00225 std::vector<float> (2));
00226 std::vector<int> move (length);
00227 std::vector<int> bestmove (length);
00228 std::vector<int> direction (length);
00229 int bestnummoves = -1;
00230 float currentval = 0;
00231 std::list<LoadedFile>::iterator it = walks.begin();
00232 std::list<LoadedFile>::iterator it2 = it;
00233 for (unsigned int i = 0; i < length / 2U; ++i, ++it2)
00234 continue;
00235
00236 for (unsigned int i = 0; i < length / 2U; ++i, ++it, ++it2)
00237 {
00238 values[i][0] = it->value;
00239
00240 values[i][1] = it2->value * (-1);
00241
00242 move[i] = 0;
00243 }
00244
00245 direction[0] = (distance > 0) ? 0 : 1;
00246 move[0] = (distance > 0) ? -1 : 1;
00247 int index = 0;
00248 while (true)
00249 {
00250 if (index == static_cast<int>(length) / 2 - 1)
00251 {
00252
00253 float dist = distance - currentval;
00254
00255 int cnt = static_cast<int> (dist / values[index][direction[index]]);
00256 float rest = ffloatpart(dist / values[index][direction[index]]);
00257 if (rest > 0.5)
00258 ++cnt;
00259 if (rest < -0.5)
00260 --cnt;
00261 move[index] = cnt;
00262 currentval += (float) cnt * values[index][direction[index]];
00263
00264
00265
00266
00267
00268
00269
00270
00271 int nummoves = 0;
00272 for (int i = 0; i <= index; ++i)
00273 nummoves += abs(move[i]);
00274 if (fabs(currentval - distance) < absoluteprecision
00275 && (nummoves < bestnummoves || bestnummoves == -1))
00276 {
00277 bestnummoves = nummoves;
00278 for (int k = 0; k < static_cast<int>(length) / 2; ++k)
00279 bestmove[k] = move[k];
00280 }
00281
00282 do {
00283 currentval -= (float) move[index] * values[index][direction[index]];
00284 move[index] = 0;
00285 --index;
00286 } while (index >= 0
00287 && ((currentval>distance && direction[index] == 0)
00288 || (currentval<distance && direction[index] == 1)));
00289 if (index < 0)
00290 break;
00291
00292
00293
00294
00295
00296
00297
00298
00299 }
00300
00301 move[index] += ((direction[index] == 0) ? 1 : -1);
00302 if (move[index] != 0)
00303 currentval += values[index][direction[index]]
00304 * (float) ((direction[index] == 0) ? 1 : -1);
00305 if (currentval > distance)
00306 direction[index + 1] = 1;
00307 else
00308 direction[index + 1] = 0;
00309 move[index + 1]= ((direction[index + 1] == 0) ? -1 : 1);
00310 ++index;
00311 }
00312
00313
00314
00315 if (tag)
00316 strcpy(usertag, tag);
00317 else
00318 strcpy(usertag, "notag");
00319 if (bestnummoves == -1 || bestnummoves == 0)
00320 {
00321 distance = 0;
00322 moving = 0;
00323 robot->send("%s: ping;", this->tag);
00324 return 0;
00325 }
00326 char command[1024];
00327 float realmove = 0;
00328 command[0] = 0;
00329 moving = 0;
00330 it = walks.begin();
00331 it2 = it;
00332 sequence.clear();
00333 for (unsigned int i = 0; i < length / 2U; ++i, ++it, ++it2)
00334 {
00335 char* name = (bestmove[i] > 0) ? it->name : it2->name;
00336 realmove += bestmove[i] * values[i][(bestmove[i] > 0) ? 0 : 1];
00337 for (int j = 0; j < abs(bestmove[i]); ++j)
00338 {
00339 ++moving;
00340 if (moving < 4)
00341 sprintf(&command[strlen(command)], "exec(%s);", name);
00342 else
00343 sequence.push_back(std::string (name));
00344 }
00345 }
00346 robot->send(command);
00347 distance = realmove;
00348 return 0;
00349 }
00350
00351 int Move::turn(float& distance, float precision, const char* tag)
00352 {
00353 float absoluteprecision = fabs(distance * precision);
00354 unsigned int length = turns.size();
00355
00356
00357 std::vector<std::vector<float> > values (length / 2U,
00358 std::vector<float> (2));
00359 std::vector<int> move (length);
00360 std::vector<int> bestmove (length);
00361 std::vector<int> direction (length);
00362 int bestnummoves = -1;
00363 float currentval = 0;
00364 std::list<LoadedFile>::iterator it = turns.begin();
00365 std::list<LoadedFile>::iterator it2 = it;
00366
00367 for (int i = 0; i < static_cast<int>(length) / 2; ++i, ++it, ++it2)
00368 {
00369 values[i][0] = it->value;
00370
00371 values[i][1] = it2->value * -1;
00372
00373 move[i] = 0;
00374 }
00375
00376 direction[0] = (distance > 0) ? 0 : 1;
00377 move[0] = (distance > 0) ? -1 : 1;
00378 int index = 0;
00379 while (true)
00380 {
00381 if (index == static_cast<int> (length) / 2 - 1)
00382 {
00383
00384 float dist = distance - currentval;
00385
00386 int cnt = static_cast<int>(dist / values[index][direction[index]]);
00387 float rest = ffloatpart(dist / values[index][direction[index]]);
00388 if (rest > 0.5)
00389 ++cnt;
00390 if (rest < -0.5)
00391 --cnt;
00392 move[index] = cnt;
00393 currentval += (float) cnt * values[index][direction[index]];
00394
00395
00396
00397
00398
00399
00400
00401
00402 int nummoves = 0;
00403 for (int i = 0; i <= index; ++i)
00404 nummoves += abs(move[i]);
00405 if (fabs(currentval - distance) < absoluteprecision
00406 && (nummoves < bestnummoves || bestnummoves == -1))
00407 {
00408 bestnummoves = nummoves;
00409 for (int k = 0; k < static_cast<int>(length) / 2; ++k)
00410 bestmove[k] = move[k];
00411 }
00412
00413 do {
00414 currentval -= (float) move[index] * values[index][direction[index]];
00415 move[index] = 0;
00416 --index;
00417 }
00418 while (index >= 0
00419 && ((currentval>distance && direction[index] == 0)
00420 || (currentval<distance && direction[index] == 1)));
00421 if (index < 0)
00422 break;
00423
00424
00425
00426
00427
00428
00429
00430
00431 }
00432
00433 move[index] += (direction[index] == 0) ? 1 : -1;
00434 if (move[index] != 0)
00435 currentval += values[index][direction[index]]
00436 * (float)((direction[index] == 0) ? 1 : -1);
00437 if (currentval > distance)
00438 direction[index + 1] = 1;
00439 else
00440 direction[index + 1] = 0;
00441 move[index + 1] = ((direction[index + 1] == 0) ? -1 : 1);
00442 ++index;
00443 }
00444
00445
00446
00447 if (tag)
00448 strcpy(usertag, tag);
00449 else
00450 strcpy(usertag, "notag");
00451 if (bestnummoves == -1 || bestnummoves == 0)
00452 {
00453 distance = 0;
00454 moving = 0;
00455 robot->send("%s: ping;", this->tag);
00456 return 0;
00457 }
00458 char command[1024];
00459 float realmove = 0;
00460 command[0] = 0;
00461 moving = 0;
00462 it = turns.begin();
00463 it2 = it;
00464 sequence.clear();
00465 for (int i = 0; i < static_cast<int>(length) / 2; ++i, ++it, ++it2)
00466 {
00467 char* name = (bestmove[i] > 0) ? it->name : it2->name;
00468 realmove += bestmove[i] * values[i][(bestmove[i] > 0) ? 0 : 1];
00469
00470 for (int j = 0; j < abs(bestmove[i]); ++j)
00471 {
00472 ++moving;
00473 if (moving < 4)
00474 sprintf(&command[strlen(command)], "exec(%s);", name);
00475 else
00476 sequence.push_back(std::string (name));
00477 }
00478 }
00479
00480 robot->send(command);
00481 distance = realmove;
00482 return 0;
00483 }
00484
00485 UCallbackAction Move::moveEnd(const UMessage& msg)
00486 {
00487 --moving;
00488 std::cerr << "movend "<< moving << std::endl;
00489 if (moving > 0 && !sequence.empty())
00490 {
00491 std::string s = sequence.front();
00492 sequence.pop_front();
00493 msg.client.send("exec(%s);", s.c_str());
00494 }
00495 if (moving <= 0 && usertag[0])
00496 robot->notifyCallbacks(UMessage(*robot, msg.timestamp, usertag,
00497 "***end move", std::list<urbi::BinaryData>()));
00498 return URBI_CONTINUE;
00499 }
00500 }