Tools/PotentialFields/PotentialfieldComposition.cpp

Go to the documentation of this file.

/**
* @file PotentialfieldComposition.cpp
* 
* Implementation of class PotentialfieldComposition
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/


#include <cassert>
#include "PotentialfieldComposition.h"
#include "Pfield.h"
#include "Motionfield.h"
#include "FieldObject.h"
#include "PfcParser.h"



PotentialfieldComposition::PotentialfieldComposition ()
{
  fileLoaded = false;
  lastResult.timeStamp = 0;
  lastResult.action = "none";
}


PotentialfieldComposition::~PotentialfieldComposition ()
{
  if(fileLoaded)
  {
    close();
  }
}


void PotentialfieldComposition::load (const std::string& filename)
{
  if(fileLoaded)
  {
    close();
  }
  parser = new Parser ();
  parser->parse(this, filename);
  delete parser;
  fileLoaded = true;
}


void PotentialfieldComposition::close()
{
  fileLoaded = false;
  std::vector < PotentialFieldsObject* >::iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    delete (*object);
  }
  objects.clear();
  std::vector < Potentialfield* >::iterator potentialfield;
  for (potentialfield = fields.begin (); potentialfield != fields.end (); ++potentialfield)
  {
    delete (*potentialfield);
  }
  fields.clear();
  results.clear();
  dynamicObjectStates.clear();
}


void PotentialfieldComposition::execute (PotentialfieldResult & result)
{
  updateDynamicObjects();
  if((lastResult.action != "none") && (fields[lastResult.fieldNumber]->hasToRemainActive()))
  {
    fields[lastResult.fieldNumber]->getResult(ownPose,result);
    result.fieldNumber = lastResult.fieldNumber;
  }
  else
  {
    for (unsigned int i=0; i<fields.size(); i++)
    {
      fields[i]->getResult(ownPose, results[i]);
      results[i].fieldNumber = i;
    }
    unsigned int searchIndex=0;
    unsigned int minIndex=0;
    while((!results[searchIndex].actionPossible) && (searchIndex<results.size()))
    {
      ++searchIndex;
    }
    if(searchIndex==results.size())
    {
      result.action = "none";
      result.value = 42.0;
      result.actionPossible = false;
      result.timeStamp = 0;
      result.fieldNumber = results.size()+1;
    }
    else
    {
      minIndex = searchIndex;
      for(unsigned int j=(searchIndex+1); j<results.size(); j++)
      {
        if((results[j].value < results[minIndex].value) && (results[j].actionPossible))
        {
          minIndex = j;
        }
      }
      result = results[minIndex];
    }
  }
  if(result.action != "none")
  {
    if(fields[result.fieldNumber]->isCombined())
    {
      std::vector<std::string> combinedFields
          (fields[result.fieldNumber]->getCombinedFields());
      for(unsigned int k=0; k<combinedFields.size(); k++)
      {
        NameToIndexMap::const_iterator pos;
        pos = fieldMap.find(combinedFields[k]);
        assert(pos != fieldMap.end());
        unsigned int fieldNum = pos->second;
        if(results[fieldNum].actionPossible)
        {
          result.motion.pos += results[fieldNum].motion.pos;
          result.motion.rotation += results[fieldNum].motion.rotation;
        }
      }
      result.motion.pos.normalize();
      result.motion.normRotation();
    }
  }
  addResultToList(result);
  result = selectNextResult();
  for(unsigned int m=0; m < fields.size(); m++)
  {
    fields[m]->setSelectionFeedback(m == result.fieldNumber);
  }
  lastResult = result;
}


PotentialfieldResult PotentialfieldComposition::selectNextResult()
{
  PotentialfieldResult result;
  if(selectionProcedure == BEST_OF_N)
  {
    unsigned int size(resultList.size());
    std::vector<int> resultCounter(size);
    std::list < PotentialfieldResult >::iterator m,n,bestResult;
    unsigned int counterPos(0), bestPos(0);
    bestResult = resultList.begin();
    for(m = resultList.begin(); m != resultList.end(); ++m, ++counterPos)
    {
      resultCounter[counterPos] = 1;
      n = m;
      ++n;
      while(n != resultList.end())
      {
        if( ((*n).fieldNumber == (*m).fieldNumber) &&
            ((*n).subAction == (*m).subAction) )
        {
          resultCounter[counterPos]++;
        }
        ++n;
      }
      if(resultCounter[counterPos] > resultCounter[bestPos])
      {
        bestPos = counterPos;
        bestResult = m;
      }
    }
    result = (*bestResult);
  }
  else //selection procedure == SUCCESSIVE_N_TIMES
  {
    //All elements of the list have to be from the same field
    bool resultIsSuccessive = true;
    unsigned int numOfFirstField = resultList.front().fieldNumber;
    std::string firstSubAction = resultList.front().subAction;
    std::list < PotentialfieldResult >::iterator i(resultList.begin());
    ++i;
    while((i != resultList.end()) && resultIsSuccessive)
    {
      if(((*i).fieldNumber != numOfFirstField) ||
         ((*i).subAction != firstSubAction))
      {
        resultIsSuccessive = false;
      }
      ++i;
    }
    if(resultIsSuccessive)
    {
      result = resultList.front();
    }
    else //otherwise keep last result:
    {
      result = lastResult;
    }
  }
  return result;
}


void PotentialfieldComposition::addResultToList(const PotentialfieldResult &result)
{
  resultList.push_front(result);
  if(resultList.size() > n)
  {
    resultList.pop_back();
  }
}


void PotentialfieldComposition::setObjectState(ObjectStateDescription& desc)
{
  if((desc.objectId >= 0) && ((int)dynamicObjectStates.size()>desc.objectId))
  {
    assert(desc.objectName == dynamicObjectStates[desc.objectId].objectName);
  }
  else  //find object by name
  {
    desc.objectId = getIdFromObjectStateSymbol(desc.objectName);
  }
  dynamicObjectStates[desc.objectId].pose = desc.pose;
  dynamicObjectStates[desc.objectId].isActive = desc.isActive;
}


void PotentialfieldComposition::setOwnPose(const PfPose& pose)
{
  ownPose = pose;
}


void PotentialfieldComposition::addField(Potentialfield* field)
{
  field->init();
  fieldMap[field->getName()] = fields.size();
  fields.push_back(field);
  PotentialfieldResult dummyResult;
  results.push_back(dummyResult);
}


void PotentialfieldComposition::addObject(PotentialFieldsObject* object)
{
  objectMap[object->getName()] = objects.size();
  objects.push_back(object);
}


void PotentialfieldComposition::addDynamicObjectState(const std::string& objectName)
{
  ObjectStateDescription desc;
  desc.objectName = objectName;
  desc.isActive = false;
  desc.objectId = dynamicObjectStates.size();
  objectStateMap[objectName] = desc.objectId;
  dynamicObjectStates.push_back(desc);
}


unsigned int PotentialfieldComposition::getIdFromObjectStateSymbol(const std::string& objectName)
{
  NameToIndexMap::const_iterator pos;
  pos = objectStateMap.find(objectName);
  assert(pos != objectStateMap.end());
  return pos->second;
}


ObjectStateDescription PotentialfieldComposition::getDescriptionFromId(unsigned int objectId)
{
  assert(objectId<dynamicObjectStates.size());
  return dynamicObjectStates[objectId];
}


std::vector<std::string> PotentialfieldComposition::getFieldNames()
{
  std::vector<std::string> names;
  std::vector < Potentialfield * >::const_iterator field;
  for (field = fields.begin (); field != fields.end (); ++field)
  {
    names.push_back((*field)->getName());
  }
  return names;
}


void PotentialfieldComposition::setFieldActivation(
    const std::string& fieldname, bool activation)
{
  fields[getFieldIndexFromName(fieldname)]->setActivation(activation);
}


inline unsigned int PotentialfieldComposition::getFieldIndexFromName(
    const std::string& fieldname)
{
  NameToIndexMap::const_iterator pos;
  pos = fieldMap.find(fieldname);
  assert(pos != fieldMap.end());
  return pos->second;
}


void PotentialfieldComposition::getValueArray(const std::string& fieldname,
    double x1, double y1, double x2, double y2,
    int xSteps, int ySteps, double value[], double& max)
{
  updateDynamicObjects();
  Potentialfield* valueField = fields[getFieldIndexFromName(fieldname)];
  valueField->getValueArray(x1,y1,x2,y2,xSteps,ySteps, value, max);
}


void PotentialfieldComposition::getDirectionArray(const std::string& fieldname,
    double x1, double y1, double x2, double y2,
    int xSteps, int ySteps, PfVec directions[])
{
  updateDynamicObjects();
  Potentialfield* directionField = fields[getFieldIndexFromName(fieldname)];
  directionField->getDirectionArray(x1,y1,x2,y2,xSteps,ySteps,directions);
}


void PotentialfieldComposition::updateDynamicObjects()
{
  std::vector < PotentialFieldsObject* >::iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    if(!(*object)->isStatic())
    {
      (*object)->updateData();
    }
  }
}

---