ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/simulator/space/space.cpp
Go to the documentation of this file.
00001 
00011 #include <argos3/core/utility/string_utilities.h>
00012 #include <argos3/core/utility/math/range.h>
00013 #include <argos3/core/utility/logging/argos_log.h>
00014 #include <argos3/core/utility/math/rng.h>
00015 #include <argos3/core/simulator/simulator.h>
00016 #include <argos3/core/simulator/entity/composable_entity.h>
00017 #include <cstring>
00018 #include "space.h"
00019 
00020 namespace argos {
00021 
00022    /****************************************/
00023    /****************************************/
00024 
00025    CSpace::CSpace() :
00026       m_unSimulationClock(0),
00027       m_pcFloorEntity(NULL),
00028       m_ptPhysicsEngines(NULL),
00029       m_ptMedia(NULL) {}
00030    
00031    /****************************************/
00032    /****************************************/
00033 
00034    void CSpace::Init(TConfigurationNode& t_tree) {
00035       /* Get reference to physics engine and media vectors */
00036       m_ptPhysicsEngines = &(CSimulator::GetInstance().GetPhysicsEngines());
00037       m_ptMedia = &(CSimulator::GetInstance().GetMedia());
00038       /* Get the arena center and size */
00039       GetNodeAttributeOrDefault(t_tree, "center", m_cArenaCenter, m_cArenaCenter);
00040       GetNodeAttribute(t_tree, "size", m_cArenaSize);
00041       /*
00042        * Add and initialize all entities in XML
00043        */
00044       /* Start from the entities placed manually */
00045       TConfigurationNodeIterator itArenaItem;
00046       for(itArenaItem = itArenaItem.begin(&t_tree);
00047           itArenaItem != itArenaItem.end();
00048           ++itArenaItem) {
00049          if(itArenaItem->Value() != "distribute") {
00050             CEntity* pcEntity = CFactory<CEntity>::New(itArenaItem->Value());
00051             pcEntity->Init(*itArenaItem);
00052             CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity);
00053          }
00054       }
00055       /* Place the entities to distribute automatically */
00056       for(itArenaItem = itArenaItem.begin(&t_tree);
00057           itArenaItem != itArenaItem.end();
00058           ++itArenaItem) {
00059          if(itArenaItem->Value() == "distribute") {
00060             Distribute(*itArenaItem);
00061          }
00062       }
00063    }
00064 
00065    /****************************************/
00066    /****************************************/
00067 
00068    void CSpace::Reset() {
00069       /* Reset the simulation clock */
00070       m_unSimulationClock = 0;
00071       /* Reset the entities */
00072       for(UInt32 i = 0; i < m_vecEntities.size(); ++i) {
00073          m_vecEntities[i]->Reset();
00074       }
00075    }
00076 
00077    /****************************************/
00078    /****************************************/
00079 
00080    void CSpace::Destroy() {
00081       /* Remove all entities */
00082       while(!m_vecRootEntities.empty()) {
00083          CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*this, *m_vecRootEntities.back());
00084       }
00085    }
00086 
00087    /****************************************/
00088    /****************************************/
00089 
00090    void CSpace::GetEntitiesMatching(CEntity::TVector& t_buffer,
00091                                     const std::string& str_pattern) {
00092       for(CEntity::TVector::iterator it = m_vecEntities.begin();
00093           it != m_vecEntities.end(); ++it) {
00094          if(MatchPattern((*it)->GetId(), str_pattern)) {
00095             t_buffer.push_back(*it);
00096          }
00097       }
00098    }
00099 
00100    /****************************************/
00101    /****************************************/
00102 
00103    CSpace::TMapPerType& CSpace::GetEntitiesByType(const std::string& str_type) {
00104       TMapPerTypePerId::iterator itEntities = m_mapEntitiesPerTypePerId.find(str_type);
00105       if (itEntities != m_mapEntitiesPerTypePerId.end()){
00106          return itEntities->second;
00107       }
00108       else {
00109          THROW_ARGOSEXCEPTION("Entity map for type \"" << str_type << "\" not found.");
00110       }
00111    }
00112 
00113    /****************************************/
00114    /****************************************/
00115 
00116    void CSpace::Update() {
00117       /* Update the controllable entities */
00118       UpdateControllableEntities();
00119       /* Update the physics engines */
00120       UpdatePhysics();
00121       /* Update media */
00122       UpdateMedia();
00123    }
00124 
00125    /****************************************/
00126    /****************************************/
00127 
00128    void CSpace::AddControllableEntity(CControllableEntity& c_entity) {
00129       m_vecControllableEntities.push_back(&c_entity);
00130    }
00131 
00132    /****************************************/
00133    /****************************************/
00134 
00135    void CSpace::RemoveControllableEntity(CControllableEntity& c_entity) {
00136       CControllableEntity::TVector::iterator it = find(m_vecControllableEntities.begin(),
00137                                                        m_vecControllableEntities.end(),
00138                                                        &c_entity);
00139       if(it != m_vecControllableEntities.end()) {
00140          m_vecControllableEntities.erase(it);
00141       }
00142    }
00143       
00144    /****************************************/
00145    /****************************************/
00146 
00147    void CSpace::AddEntityToPhysicsEngine(CEmbodiedEntity& c_entity) {
00148       /* Get a reference to the root entity */
00149       CEntity* pcToAdd = &c_entity;
00150       while(pcToAdd->HasParent()) {
00151          pcToAdd = &pcToAdd->GetParent();
00152       }
00153       /* Get a reference to the position of the entity */
00154       const CVector3& cPos = c_entity.GetPosition();
00155       /* Go through engines and check which ones could house the entity */
00156       CPhysicsEngine::TVector vecPotentialEngines;
00157       for(size_t i = 0; i < m_ptPhysicsEngines->size(); ++i) {
00158          if((*m_ptPhysicsEngines)[i]->IsPointContained(cPos)) {
00159             vecPotentialEngines.push_back((*m_ptPhysicsEngines)[i]);
00160          }
00161       }
00162       /* If no engine can house the entity, bomb out */
00163       if(vecPotentialEngines.empty()) {
00164          THROW_ARGOSEXCEPTION("No physics engine can house entity \"" << pcToAdd->GetId() << "\".");
00165       }
00166       /* If the entity is not movable, add the entity to all the matching engines */
00167       if(! c_entity.IsMovable()) {
00168          for(size_t i = 0; i < vecPotentialEngines.size(); ++i) {
00169             vecPotentialEngines[i]->AddEntity(*pcToAdd);
00170          }
00171       }
00172       /* If the entity is movable, only one engine can be associated to the embodied entity */
00173       else if(vecPotentialEngines.size() == 1) {
00174          /* Only one engine matches, bingo! */
00175          vecPotentialEngines[0]->AddEntity(*pcToAdd);
00176       }
00177       else {
00178          /* More than one engine matches, bomb out */
00179          std::ostringstream ossEngines;
00180          ossEngines << "\"" << vecPotentialEngines[0]->GetId() << "\"";
00181          for(size_t i = 1; i < vecPotentialEngines.size(); ++i) {
00182             ossEngines << ", \"" << vecPotentialEngines[i]->GetId() << "\"";
00183          }
00184          THROW_ARGOSEXCEPTION("Multiple engines can house \"" << c_entity.GetId() << "\", but a movable entity and can only be added to a single engine. Conflicting engines: " << ossEngines);
00185       }
00186    }
00187       
00188    /****************************************/
00189    /****************************************/
00190 
00191    class RealNumberGenerator {
00192    public:
00193       virtual ~RealNumberGenerator() {}
00194       virtual CVector3 operator()(bool b_is_retry) = 0;
00195    };
00196 
00197    class ConstantGenerator : public RealNumberGenerator {
00198    public:
00199       ConstantGenerator(const CVector3& c_value) :
00200          m_cValue(c_value) {}
00201 
00202       inline virtual CVector3 operator()(bool b_is_retry) {
00203          return m_cValue;
00204       }
00205    private:
00206       CVector3 m_cValue;
00207 
00208    };
00209 
00210    class UniformGenerator : public RealNumberGenerator {
00211    public:
00212       UniformGenerator(const CVector3& c_min,
00213                        const CVector3& c_max) :
00214          m_cMin(c_min),
00215          m_cMax(c_max) {}
00216       inline virtual CVector3 operator()(bool b_is_retry) {
00217          Real fRandX =
00218             m_cMax.GetX() > m_cMin.GetX() ?
00219             CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetX(), m_cMax.GetX())) :
00220             m_cMax.GetX();
00221          Real fRandY =
00222             m_cMax.GetY() > m_cMin.GetY() ?
00223             CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetY(), m_cMax.GetY())) :
00224             m_cMax.GetY();
00225          Real fRandZ =
00226             m_cMax.GetZ() > m_cMin.GetZ() ?
00227             CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetZ(), m_cMax.GetZ())) :
00228             m_cMax.GetZ();
00229          return CVector3(fRandX, fRandY, fRandZ);
00230       }
00231    private:
00232       CVector3 m_cMin;
00233       CVector3 m_cMax;
00234    };
00235 
00236    class GaussianGenerator : public RealNumberGenerator {
00237    public:
00238       GaussianGenerator(const CVector3& c_mean,
00239                         const CVector3& c_std_dev) :
00240          m_cMean(c_mean),
00241          m_cStdDev(c_std_dev) {}
00242       inline virtual CVector3 operator()(bool b_is_retry) {
00243          return CVector3(CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetX(), m_cMean.GetX()),
00244                          CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetY(), m_cMean.GetY()),
00245                          CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetZ(), m_cMean.GetZ()));
00246       }
00247    private:
00248       CVector3 m_cMean;
00249       CVector3 m_cStdDev;
00250    };
00251 
00252    class GridGenerator : public RealNumberGenerator {
00253    public:
00254       GridGenerator(const CVector3 c_center,
00255                     const UInt32 un_layout[],
00256                     const CVector3 c_distances):
00257          m_cCenter(c_center),
00258          m_cDistances(c_distances),
00259          m_unNumEntityPlaced(0) {
00260          m_unLayout[0] = un_layout[0];
00261          m_unLayout[1] = un_layout[1];
00262          m_unLayout[2] = un_layout[2];
00263          /* Check if layout is sane */
00264          if( m_unLayout[0] == 0 || m_unLayout[1] == 0 || m_unLayout[2] == 0 ) {
00265             THROW_ARGOSEXCEPTION("'layout' values (distribute position, method 'grid') must all be different than 0");
00266          }
00267       }
00268 
00269       virtual CVector3 operator()(bool b_is_retry) {
00270          if(b_is_retry) {
00271             THROW_ARGOSEXCEPTION("Impossible to place entity #" << m_unNumEntityPlaced << " in grid");
00272          }
00273          CVector3 cReturn;
00274          if(m_unNumEntityPlaced < m_unLayout[0] * m_unLayout[1] * m_unLayout[2]) {
00275             cReturn.SetX(m_cCenter.GetX() + ( m_unLayout[0] - 1 ) * m_cDistances.GetX() * 0.5 - ( m_unNumEntityPlaced  % m_unLayout[0] ) * m_cDistances.GetX());
00276             cReturn.SetY(m_cCenter.GetY() + ( m_unLayout[1] - 1 ) * m_cDistances.GetY() * 0.5 - ( m_unNumEntityPlaced  / m_unLayout[0] ) % m_unLayout[1] * m_cDistances.GetY());
00277             cReturn.SetZ(m_cCenter.GetZ() + ( m_unLayout[2] - 1 ) * m_cDistances.GetZ() * 0.5 - ( m_unNumEntityPlaced / ( m_unLayout[0] * m_unLayout[1] ) ) * m_cDistances.GetZ());
00278             ++m_unNumEntityPlaced;
00279          }
00280          else {
00281             THROW_ARGOSEXCEPTION("Distribute position, method 'grid': trying to place more entities than allowed "
00282                                  "by the 'layout', check your 'quantity' tag");
00283          }
00284          return cReturn;
00285       }
00286 
00287    private:
00288       CVector3 m_cCenter;
00289       UInt32 m_unLayout[3];
00290       CVector3 m_cDistances;
00291       UInt32 m_unNumEntityPlaced;
00292    };
00293 
00294    /****************************************/
00295    /****************************************/
00296 
00297    RealNumberGenerator* CreateGenerator(TConfigurationNode& t_tree) {
00298       std::string strMethod;
00299       GetNodeAttribute(t_tree, "method", strMethod);
00300       if(strMethod == "uniform") {
00301          CVector3 cMin, cMax;
00302          GetNodeAttribute(t_tree, "min", cMin);
00303          GetNodeAttribute(t_tree, "max", cMax);
00304          if(! (cMin <= cMax)) {
00305             THROW_ARGOSEXCEPTION("Uniform generator: the min is not less than or equal to max: " << cMin << " / " << cMax);
00306          }
00307          return new UniformGenerator(cMin, cMax);
00308       }
00309       else if(strMethod == "gaussian") {
00310          CVector3 cMean, cStdDev;
00311          GetNodeAttribute(t_tree, "mean", cMean);
00312          GetNodeAttribute(t_tree, "std_dev", cStdDev);
00313          return new GaussianGenerator(cMean, cStdDev);
00314       }
00315       else if(strMethod == "constant") {
00316          CVector3 cValues;
00317          GetNodeAttribute(t_tree, "values", cValues);
00318          return new ConstantGenerator(cValues);
00319       }
00320       else if(strMethod == "grid") {
00321          CVector3 cCenter,cDistances;
00322          GetNodeAttribute(t_tree, "center", cCenter);
00323          GetNodeAttribute(t_tree, "distances", cDistances);
00324          UInt32 unLayout[3];
00325          std::string strLayout;
00326          GetNodeAttribute(t_tree, "layout", strLayout);
00327          ParseValues<UInt32> (strLayout, 3, unLayout, ',');
00328          return new GridGenerator(cCenter, unLayout, cDistances);
00329       }
00330       else {
00331          THROW_ARGOSEXCEPTION("Unknown distribution method \"" << strMethod << "\"");
00332       }
00333    }
00334 
00335    /****************************************/
00336    /****************************************/
00337 
00338    static CEmbodiedEntity* GetEmbodiedEntity(CEntity* pc_entity) {
00339       /* Is the entity embodied itself? */
00340       CEmbodiedEntity* pcEmbodiedTest = dynamic_cast<CEmbodiedEntity*>(pc_entity);
00341       if(pcEmbodiedTest != NULL) {
00342          return pcEmbodiedTest;
00343       }
00344       /* Is the entity composable with an embodied component? */
00345       CComposableEntity* pcComposableTest = dynamic_cast<CComposableEntity*>(pc_entity);
00346       if(pcComposableTest != NULL) {
00347          if(pcComposableTest->HasComponent("body")) {
00348             return &(pcComposableTest->GetComponent<CEmbodiedEntity>("body"));
00349          }
00350       }
00351       /* No embodied entity found */
00352       return NULL;
00353    }
00354 
00355    /****************************************/
00356    /****************************************/
00357 
00358    static CPositionalEntity* GetPositionalEntity(CEntity* pc_entity) {
00359       /* Is the entity positional itself? */
00360       CPositionalEntity* pcPositionalTest = dynamic_cast<CPositionalEntity*>(pc_entity);
00361       if(pcPositionalTest != NULL) {
00362          return pcPositionalTest;
00363       }
00364       /* Is the entity composable with a positional component? */
00365       CComposableEntity* pcComposableTest = dynamic_cast<CComposableEntity*>(pc_entity);
00366       if(pcComposableTest != NULL) {
00367          if(pcComposableTest->HasComponent("position")) {
00368             return &(pcComposableTest->GetComponent<CPositionalEntity>("position"));
00369          }
00370       }
00371       /* No positional entity found */
00372       return NULL;
00373    }
00374 
00375    /****************************************/
00376    /****************************************/
00377 
00378    void CSpace::Distribute(TConfigurationNode& t_tree) {
00379       try {
00380          /* Get the needed nodes */
00381          TConfigurationNode cPositionNode;
00382          cPositionNode = GetNode(t_tree, "position");
00383          TConfigurationNode cOrientationNode;
00384          cOrientationNode = GetNode(t_tree, "orientation");
00385          TConfigurationNode cEntityNode;
00386          cEntityNode = GetNode(t_tree, "entity");
00387          /* Create the real number generators */
00388          RealNumberGenerator* pcPositionGenerator = CreateGenerator(cPositionNode);
00389          RealNumberGenerator* pcOrientationGenerator = CreateGenerator(cOrientationNode);
00390          /* How many entities? */
00391          UInt32 unQuantity;
00392          GetNodeAttribute(cEntityNode, "quantity", unQuantity);
00393          /* How many trials before failing? */
00394          UInt32 unMaxTrials;
00395          GetNodeAttribute(cEntityNode, "max_trials", unMaxTrials);
00396          /* Get the (optional) entity base numbering */
00397          UInt64 unBaseNum = 0;
00398          GetNodeAttributeOrDefault(cEntityNode, "base_num", unBaseNum, unBaseNum);
00399          /* Get the entity type to add (take only the first, ignore additional if any) */
00400          TConfigurationNodeIterator itEntity;
00401          itEntity = itEntity.begin(&cEntityNode);
00402          if(itEntity == itEntity.end()) {
00403             THROW_ARGOSEXCEPTION("No entity to distribute specified.");
00404          }
00405          /* Get the entity base ID */
00406          std::string strBaseId;
00407          GetNodeAttribute(*itEntity, "id", strBaseId);
00408          /* Add the requested entities */
00409          for(UInt32 i = 0; i < unQuantity; ++i) {
00410             /* Copy the entity XML tree */
00411             TConfigurationNode tEntityTree = *itEntity;
00412             /* Set progressive ID */
00413             SetNodeAttribute(tEntityTree, "id", strBaseId + ToString(i+unBaseNum));
00414             /* Go on until the entity is placed with no collisions or
00415                the max number of trials has been exceeded */
00416             UInt32 unTrials = 0;
00417             bool bDone = false;
00418             bool bRetry = false;
00419             CEntity* pcEntity;
00420             do {
00421                /* Create entity */
00422                pcEntity = CFactory<CEntity>::New(tEntityTree.Value());
00423                /* If the tree does not have a 'body' node, create a new one */
00424                if(!NodeExists(tEntityTree, "body")) {
00425                   TConfigurationNode tBodyNode("body");
00426                   AddChildNode(tEntityTree, tBodyNode);
00427                }
00428                /* Get 'body' node */
00429                TConfigurationNode& tBodyNode = GetNode(tEntityTree, "body");
00430                /* Set the position */
00431                SetNodeAttribute(tBodyNode, "position", (*pcPositionGenerator)(bRetry));
00432                /* Set the orientation */
00433                SetNodeAttribute(tBodyNode, "orientation", (*pcOrientationGenerator)(bRetry));
00434                /* Init the entity (this also creates the components, if pcEntity is a composable) */
00435                pcEntity->Init(tEntityTree);
00436                /*
00437                 * Now that you have the entity and its components, check whether the entity is positional or embodied
00438                 * or has one such component.
00439                 * In case the entity is positional but not embodied, there's no need to check for collisions
00440                 * In case the entity is embodied, we must check for collisions
00441                 * To check for collisions, we add the entity in the place where it's supposed to be,
00442                 * then we ask the engine if that entity is colliding with something
00443                 * In case of collision, we remove the entity and try a different position/orientation
00444                 */
00445                /* Check for embodied */
00446                CEmbodiedEntity* pcEmbodiedEntity = GetEmbodiedEntity(pcEntity);
00447                if(pcEmbodiedEntity == NULL) {
00448                   /* Check failed, then check for positional */
00449                   CPositionalEntity* pcPositionalEntity = GetPositionalEntity(pcEntity);
00450                   if(pcPositionalEntity == NULL) {
00451                      THROW_ARGOSEXCEPTION("Cannot distribute entities that are not positional nor embodied, and \"" << tEntityTree.Value() << "\" is neither.");
00452                   }
00453                   else {
00454                      /* Wherever we want to put the entity, it's OK, add it */
00455                      CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity);
00456                   }
00457                }
00458                else {
00459                   /* The entity is embodied */
00460                   /* Add it to the space and to the designated physics engine */
00461                   CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity);
00462                   /* Check if it's colliding with anything else */
00463                   if(pcEmbodiedEntity->IsCollidingWithSomething()) {
00464                      /* Set retry to true */
00465                      bRetry = true;
00466                      /* Get rid of the entity */
00467                      CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*this, *pcEntity);
00468                      /* Increase the trial count */
00469                      ++unTrials;
00470                      /* Too many trials? */
00471                      if(unTrials > unMaxTrials) {
00472                         /* Yes, bomb out */
00473                         THROW_ARGOSEXCEPTION("Exceeded max trials when trying to distribute objects of type " <<
00474                                              tEntityTree.Value() << " with base id \"" <<
00475                                              strBaseId << "\". I managed to place only " << i << " objects.");
00476                      }
00477                      /* Retry with a new position */
00478                   }
00479                   else {
00480                      /* No collision, we're done with this entity */
00481                      bDone = true;
00482                   }
00483                }
00484             }
00485             while(!bDone);
00486          }
00487          /* Delete the generators, now unneeded */
00488          delete pcPositionGenerator;
00489          delete pcOrientationGenerator;
00490       }
00491       catch(CARGoSException& ex) {
00492          THROW_ARGOSEXCEPTION_NESTED("Error while trying to distribute entities", ex);
00493       }
00494    }
00495 
00496    /****************************************/
00497    /****************************************/
00498 
00499 }