move.cc

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     // This is required to parse our configuration file correctly.
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; //empty line
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           // Not an absolute path, happen dirname as a prefix.
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         // Read the file.
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           //std::cerr << offset<<" / "<<filelength<<std::endl;
00149           //usleep(200000);
00150           //std::cerr << block<<std::endl;
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     //register our callback
00167     robot->setCallback(*this, &Move::moveEnd, tag);
00168     //fill the properties structs by parsing our loadedfiles.
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     /* Looks hairy but that's the right way of declaring a matrix
00223      * values[N][M] where N = length / 2U and M = 2 */
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     //std::cerr << "values: ";
00236     for (unsigned int i = 0; i < length / 2U; ++i, ++it, ++it2)
00237     {
00238       values[i][0] = it->value;
00239       // The minus is in the negative value in move.
00240       values[i][1] = it2->value * (-1);
00241       //std::cerr << values[i][0] << " " << values[i][1] <<" , ";
00242       move[i] = 0;
00243     }
00244     //std::cerr <<endl;
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         //calculate best value for move[index]
00253         float dist = distance - currentval;
00254         // -12/5 = -2 en division entiere.
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          std::cerr << "eval ";
00265          for (unsigned int k = 0; k < length / 2; ++k)
00266          std::cerr << move[k] << " ";
00267          std::cerr << "(" << currentval << ")";
00268          std::cerr << endl;
00269          */
00270         //calculate nummoves
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         //get back one or more level
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         //DEBUG:recalculate currentval
00292         /*
00293          std::cerr << "cv: " << currentval;
00294          currentval = 0;
00295          for (unsigned int k = 0; k < length / 2U; ++k)
00296          currentval += (float) move[k] * values[k][direction[k]];
00297          std::cerr << " " << currentval << std::endl;
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     //end, apply best match
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     /* Looks hairy but that's the right way of declaring a matrix
00356      * values[N][M] where N = length / 2U and M = 2 */
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       // The minus is in the negative value in move.
00371       values[i][1] = it2->value * -1;
00372       //std::cerr << values[i][0] << " " << values[i][1] <<" , ";
00373       move[i] = 0;
00374     }
00375     //std::cerr <<endl;
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       //calculate best value for move[index]
00384       float dist = distance - currentval;
00385       // -12/5 = -2 en division entiere.
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        std::cerr << "eval ";
00396        for (int k = 0;k<length/2; ++k)
00397        std::cerr << move[k] <<" ";
00398        std::cerr << "(" <<currentval <<")";
00399        std::cerr <<endl;
00400        */
00401       //calculate nummoves
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       //get back one or more level
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       //DEBUG:recalculate currentval
00424       /*
00425        std::cerr << "cv: "<<currentval;
00426        currentval = 0;
00427        for (int k = 0;k<length/2; ++k)
00428        currentval+=(float)move[k]*values[k][direction[k]];
00429        std::cerr << " " <<currentval <<std::endl;
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   //end, apply best match
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     //  std::cerr << bestmove[i] << " ";
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   //  std::cerr <<endl;
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 } // namespace urbi

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