Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/PotentialFields/PotentialfieldComposition.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file PotentialfieldComposition.cpp
    00003 *
    00004 * Implementation of class PotentialfieldComposition
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009
    00010 #include <cassert>
    00011 #include "PotentialfieldComposition.h"
    00012 #include "Pfield.h"
    00013 #include "Motionfield.h"
    00014 #include "FieldObject.h"
    00015 #include "PfcParser.h"
    00016
    00017
    00018
    00019 PotentialfieldComposition::PotentialfieldComposition ()
    00020 {
    00021   fileLoaded = false;
    00022   lastResult.timeStamp = 0;
    00023   lastResult.action = "none";
    00024 }
    00025
    00026
    00027 PotentialfieldComposition::~PotentialfieldComposition ()
    00028 {
    00029   if(fileLoaded)
    00030   {
    00031     close();
    00032   }
    00033 }
    00034
    00035
    00036 void PotentialfieldComposition::load (const std::string& filename)
    00037 {
    00038   if(fileLoaded)
    00039   {
    00040     close();
    00041   }
    00042   parser = new Parser ();
    00043   parser->parse(this, filename);
    00044   delete parser;
    00045   fileLoaded = true;
    00046 }
    00047
    00048
    00049 void PotentialfieldComposition::close()
    00050 {
    00051   fileLoaded = false;
    00052   std::vector < PotentialFieldsObject* >::iterator object;
    00053   for (object = objects.begin (); object != objects.end (); ++object)
    00054   {
    00055     delete (*object);
    00056   }
    00057   objects.clear();
    00058   std::vector < Potentialfield* >::iterator potentialfield;
    00059   for (potentialfield = fields.begin (); potentialfield != fields.end (); ++potentialfield)
    00060   {
    00061     delete (*potentialfield);
    00062   }
    00063   fields.clear();
    00064   results.clear();
    00065   dynamicObjectStates.clear();
    00066 }
    00067
    00068
    00069 void PotentialfieldComposition::execute (PotentialfieldResult & result)
    00070 {
    00071   updateDynamicObjects();
    00072   if((lastResult.action != "none") && (fields[lastResult.fieldNumber]->hasToRemainActive()))
    00073   {
    00074     fields[lastResult.fieldNumber]->getResult(ownPose,result);
    00075     result.fieldNumber = lastResult.fieldNumber;
    00076   }
    00077   else
    00078   {
    00079     for (unsigned int i=0; i<fields.size(); i++)
    00080     {
    00081       fields[i]->getResult(ownPose, results[i]);
    00082       results[i].fieldNumber = i;
    00083     }
    00084     unsigned int searchIndex=0;
    00085     unsigned int minIndex=0;
    00086     while((!results[searchIndex].actionPossible) && (searchIndex<results.size()))
    00087     {
    00088       ++searchIndex;
    00089     }
    00090     if(searchIndex==results.size())
    00091     {
    00092       result.action = "none";
    00093       result.value = 42.0;
    00094       result.actionPossible = false;
    00095       result.timeStamp = 0;
    00096       result.fieldNumber = results.size()+1;
    00097     }
    00098     else
    00099     {
    00100       minIndex = searchIndex;
    00101       for(unsigned int j=(searchIndex+1); j<results.size(); j++)
    00102       {
    00103         if((results[j].value < results[minIndex].value) && (results[j].actionPossible))
    00104         {
    00105           minIndex = j;
    00106         }
    00107       }
    00108       result = results[minIndex];
    00109     }
    00110   }
    00111   if(result.action != "none")
    00112   {
    00113     if(fields[result.fieldNumber]->isCombined())
    00114     {
    00115       std::vector<std::string> combinedFields
    00116           (fields[result.fieldNumber]->getCombinedFields());
    00117       for(unsigned int k=0; k<combinedFields.size(); k++)
    00118       {
    00119         NameToIndexMap::const_iterator pos;
    00120         pos = fieldMap.find(combinedFields[k]);
    00121         assert(pos != fieldMap.end());
    00122         unsigned int fieldNum = pos->second;
    00123         if(results[fieldNum].actionPossible)
    00124         {
    00125           result.motion.pos += results[fieldNum].motion.pos;
    00126           result.motion.rotation += results[fieldNum].motion.rotation;
    00127         }
    00128       }
    00129       result.motion.pos.normalize();
    00130       result.motion.normRotation();
    00131     }
    00132   }
    00133   addResultToList(result);
    00134   result = selectNextResult();
    00135   for(unsigned int m=0; m < fields.size(); m++)
    00136   {
    00137     fields[m]->setSelectionFeedback(m == result.fieldNumber);
    00138   }
    00139   lastResult = result;
    00140 }
    00141
    00142
    00143 PotentialfieldResult PotentialfieldComposition::selectNextResult()
    00144 {
    00145   PotentialfieldResult result;
    00146   if(selectionProcedure == BEST_OF_N)
    00147   {
    00148     unsigned int size(resultList.size());
    00149     std::vector<int> resultCounter(size);
    00150     std::list < PotentialfieldResult >::iterator m,n,bestResult;
    00151     unsigned int counterPos(0), bestPos(0);
    00152     bestResult = resultList.begin();
    00153     for(m = resultList.begin(); m != resultList.end(); ++m, ++counterPos)
    00154     {
    00155       resultCounter[counterPos] = 1;
    00156       n = m;
    00157       ++n;
    00158       while(n != resultList.end())
    00159       {
    00160         if( ((*n).fieldNumber == (*m).fieldNumber) &&
    00161             ((*n).subAction == (*m).subAction) )
    00162         {
    00163           resultCounter[counterPos]++;
    00164         }
    00165         ++n;
    00166       }
    00167       if(resultCounter[counterPos] > resultCounter[bestPos])
    00168       {
    00169         bestPos = counterPos;
    00170         bestResult = m;
    00171       }
    00172     }
    00173     result = (*bestResult);
    00174   }
    00175   else //selection procedure == SUCCESSIVE_N_TIMES
    00176   {
    00177     //All elements of the list have to be from the same field
    00178     bool resultIsSuccessive = true;
    00179     unsigned int numOfFirstField = resultList.front().fieldNumber;
    00180     std::string firstSubAction = resultList.front().subAction;
    00181     std::list < PotentialfieldResult >::iterator i(resultList.begin());
    00182     ++i;
    00183     while((i != resultList.end()) && resultIsSuccessive)
    00184     {
    00185       if(((*i).fieldNumber != numOfFirstField) ||
    00186          ((*i).subAction != firstSubAction))
    00187       {
    00188         resultIsSuccessive = false;
    00189       }
    00190       ++i;
    00191     }
    00192     if(resultIsSuccessive)
    00193     {
    00194       result = resultList.front();
    00195     }
    00196     else //otherwise keep last result:
    00197     {
    00198       result = lastResult;
    00199     }
    00200   }
    00201   return result;
    00202 }
    00203
    00204
    00205 void PotentialfieldComposition::addResultToList(const PotentialfieldResult &result)
    00206 {
    00207   resultList.push_front(result);
    00208   if(resultList.size() > n)
    00209   {
    00210     resultList.pop_back();
    00211   }
    00212 }
    00213
    00214
    00215 void PotentialfieldComposition::setObjectState(ObjectStateDescription& desc)
    00216 {
    00217   if((desc.objectId >= 0) && ((int)dynamicObjectStates.size()>desc.objectId))
    00218   {
    00219     assert(desc.objectName == dynamicObjectStates[desc.objectId].objectName);
    00220   }
    00221   else  //find object by name
    00222   {
    00223     desc.objectId = getIdFromObjectStateSymbol(desc.objectName);
    00224   }
    00225   dynamicObjectStates[desc.objectId].pose = desc.pose;
    00226   dynamicObjectStates[desc.objectId].isActive = desc.isActive;
    00227 }
    00228
    00229
    00230 void PotentialfieldComposition::setOwnPose(const PfPose& pose)
    00231 {
    00232   ownPose = pose;
    00233 }
    00234
    00235
    00236 void PotentialfieldComposition::addField(Potentialfield* field)
    00237 {
    00238   field->init();
    00239   fieldMap[field->getName()] = fields.size();
    00240   fields.push_back(field);
    00241   PotentialfieldResult dummyResult;
    00242   results.push_back(dummyResult);
    00243 }
    00244
    00245
    00246 void PotentialfieldComposition::addObject(PotentialFieldsObject* object)
    00247 {
    00248   objectMap[object->getName()] = objects.size();
    00249   objects.push_back(object);
    00250 }
    00251
    00252
    00253 void PotentialfieldComposition::addDynamicObjectState(const std::string& objectName)
    00254 {
    00255   ObjectStateDescription desc;
    00256   desc.objectName = objectName;
    00257   desc.isActive = false;
    00258   desc.objectId = dynamicObjectStates.size();
    00259   objectStateMap[objectName] = desc.objectId;
    00260   dynamicObjectStates.push_back(desc);
    00261 }
    00262
    00263
    00264 unsigned int PotentialfieldComposition::getIdFromObjectStateSymbol(const std::string& objectName)
    00265 {
    00266   NameToIndexMap::const_iterator pos;
    00267   pos = objectStateMap.find(objectName);
    00268   assert(pos != objectStateMap.end());
    00269   return pos->second;
    00270 }
    00271
    00272
    00273 ObjectStateDescription PotentialfieldComposition::getDescriptionFromId(unsigned int objectId)
    00274 {
    00275   assert(objectId<dynamicObjectStates.size());
    00276   return dynamicObjectStates[objectId];
    00277 }
    00278
    00279
    00280 std::vector<std::string> PotentialfieldComposition::getFieldNames()
    00281 {
    00282   std::vector<std::string> names;
    00283   std::vector < Potentialfield * >::const_iterator field;
    00284   for (field = fields.begin (); field != fields.end (); ++field)
    00285   {
    00286     names.push_back((*field)->getName());
    00287   }
    00288   return names;
    00289 }
    00290
    00291
    00292 void PotentialfieldComposition::setFieldActivation(
    00293     const std::string& fieldname, bool activation)
    00294 {
    00295   fields[getFieldIndexFromName(fieldname)]->setActivation(activation);
    00296 }
    00297
    00298
    00299 inline unsigned int PotentialfieldComposition::getFieldIndexFromName(
    00300     const std::string& fieldname)
    00301 {
    00302   NameToIndexMap::const_iterator pos;
    00303   pos = fieldMap.find(fieldname);
    00304   assert(pos != fieldMap.end());
    00305   return pos->second;
    00306 }
    00307
    00308
    00309 void PotentialfieldComposition::getValueArray(const std::string& fieldname,
    00310     double x1, double y1, double x2, double y2,
    00311     int xSteps, int ySteps, double value[], double& max)
    00312 {
    00313   updateDynamicObjects();
    00314   Potentialfield* valueField = fields[getFieldIndexFromName(fieldname)];
    00315   valueField->getValueArray(x1,y1,x2,y2,xSteps,ySteps, value, max);
    00316 }
    00317
    00318
    00319 void PotentialfieldComposition::getDirectionArray(const std::string& fieldname,
    00320     double x1, double y1, double x2, double y2,
    00321     int xSteps, int ySteps, PfVec directions[])
    00322 {
    00323   updateDynamicObjects();
    00324   Potentialfield* directionField = fields[getFieldIndexFromName(fieldname)];
    00325   directionField->getDirectionArray(x1,y1,x2,y2,xSteps,ySteps,directions);
    00326 }
    00327
    00328
    00329 void PotentialfieldComposition::updateDynamicObjects()
    00330 {
    00331   std::vector < PotentialFieldsObject* >::iterator object;
    00332   for (object = objects.begin (); object != objects.end (); ++object)
    00333   {
    00334     if(!(*object)->isStatic())
    00335     {
    00336       (*object)->updateData();
    00337     }
    00338   }
    00339 }