ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/media/rab_medium.cpp
Go to the documentation of this file.
00001 #include "rab_medium.h"
00002 #include <argos3/core/simulator/entity/embodied_entity.h>
00003 #include <argos3/core/simulator/simulator.h>
00004 #include <argos3/core/simulator/space/space.h>
00005 #include <argos3/core/simulator/space/positional_indices/grid.h>
00006 #include <argos3/core/utility/configuration/argos_exception.h>
00007 #include <argos3/core/utility/logging/argos_log.h>
00008 
00009 namespace argos {
00010 
00011    /****************************************/
00012    /****************************************/
00013 
00014    CRABMedium::CRABMedium() {
00015    }
00016 
00017    /****************************************/
00018    /****************************************/
00019 
00020    CRABMedium::~CRABMedium() {
00021    }
00022 
00023    /****************************************/
00024    /****************************************/
00025 
00026    void CRABMedium::Init(TConfigurationNode& t_tree) {
00027       try {
00028          CMedium::Init(t_tree);
00029          /* Get the positional index method */
00030          std::string strPosIndexMethod("grid");
00031          GetNodeAttributeOrDefault(t_tree, "index", strPosIndexMethod, strPosIndexMethod);
00032          /* Get the arena center and size */
00033          CVector3 cArenaCenter;
00034          CVector3 cArenaSize;
00035          TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
00036          GetNodeAttribute(tArena, "size", cArenaSize);
00037          GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
00038          /* Create the positional index for embodied entities */
00039          if(strPosIndexMethod == "grid") {
00040             size_t punGridSize[3];
00041             if(!NodeAttributeExists(t_tree, "grid_size")) {
00042                punGridSize[0] = cArenaSize.GetX();
00043                punGridSize[1] = cArenaSize.GetY();
00044                punGridSize[2] = cArenaSize.GetZ();
00045             }
00046             else {
00047                std::string strPosGridSize;
00048                GetNodeAttribute(t_tree, "grid_size", strPosGridSize);
00049                ParseValues<size_t>(strPosGridSize, 3, punGridSize, ',');
00050             }
00051             CGrid<CRABEquippedEntity>* pcGrid = new CGrid<CRABEquippedEntity>(
00052                cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
00053                punGridSize[0], punGridSize[1], punGridSize[2]);
00054             m_pcRABEquippedEntityGridUpdateOperation = new CRABEquippedEntityGridEntityUpdater(*pcGrid);
00055             pcGrid->SetUpdateEntityOperation(m_pcRABEquippedEntityGridUpdateOperation);
00056             m_pcRABEquippedEntityIndex = pcGrid;
00057          }
00058          else {
00059             THROW_ARGOSEXCEPTION("Unknown method \"" << strPosIndexMethod << "\" for the positional index.");
00060          }
00061       }
00062       catch(CARGoSException& ex) {
00063          THROW_ARGOSEXCEPTION_NESTED("Error in initialization of the range-and-bearing medium", ex);
00064       }
00065    }
00066 
00067    /****************************************/
00068    /****************************************/
00069 
00070    void CRABMedium::PostSpaceInit() {
00071       Update();
00072    }
00073 
00074    /****************************************/
00075    /****************************************/
00076 
00077    void CRABMedium::Reset() {
00078       /* Reset positional index of RAB entities */
00079       m_pcRABEquippedEntityIndex->Reset();
00080       /* Delete routing table */
00081       for(TRoutingTable::iterator it = m_tRoutingTable.begin();
00082           it != m_tRoutingTable.end();
00083           ++it) {
00084          it->second.clear();
00085       }
00086    }
00087 
00088    /****************************************/
00089    /****************************************/
00090 
00091    void CRABMedium::Destroy() {
00092       delete m_pcRABEquippedEntityIndex;
00093       if(m_pcRABEquippedEntityGridUpdateOperation != NULL) {
00094          delete m_pcRABEquippedEntityGridUpdateOperation;
00095       }
00096    }
00097 
00098    /****************************************/
00099    /****************************************/
00100 
00101    static UInt64 HashRABPair(const std::pair<CRABEquippedEntity*, CRABEquippedEntity*>& c_pair) {
00102       UInt64 unA = *reinterpret_cast<unsigned long long*>(c_pair.first) & 0xFFFFFFFF;
00103       UInt64 unB = *reinterpret_cast<unsigned long long*>(c_pair.second) & 0xFFFFFFFF;
00104       return (unA << 32) | unB;
00105    }
00106 
00107    void CRABMedium::Update() {
00108       /* Update positional index of RAB entities */
00109       m_pcRABEquippedEntityIndex->Update();
00110       /* Delete routing table */
00111       for(TRoutingTable::iterator it = m_tRoutingTable.begin();
00112           it != m_tRoutingTable.end();
00113           ++it) {
00114          it->second.clear();
00115       }
00116       /* This map contains the pairs that have already been checked */
00117       std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> > mapPairsAlreadyChecked;
00118       /* Iterator for the above structure */
00119       std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> >::iterator itPair;
00120       /* Used as test key */
00121       std::pair<CRABEquippedEntity*, CRABEquippedEntity*> cTestKey;
00122       /* Used as hash for the test key */
00123       UInt64 unTestHash;
00124       /* The ray to use for occlusion checking */
00125       CRay3 cOcclusionCheckRay;
00126       /* Buffer for the communicating entities */
00127       CSet<CRABEquippedEntity*> cOtherRABs;
00128       /* Buffer to store the intersection data */
00129       SEmbodiedEntityIntersectionItem sIntersectionItem;
00130       /* The distance between two RABs in line of sight */
00131       Real fDistance;
00132       /* Go through the RAB entities */
00133       for(TRoutingTable::iterator it = m_tRoutingTable.begin();
00134           it != m_tRoutingTable.end();
00135           ++it) {
00136          /* Get a reference to the current RAB entity */
00137          CRABEquippedEntity& cRAB = *(it->first);
00138          /* Initialize the occlusion check ray start to the position of the robot */
00139          cOcclusionCheckRay.SetStart(cRAB.GetPosition());
00140          /* For each RAB entity, get the list of RAB entities in range */
00141          cOtherRABs.clear();
00142          m_pcRABEquippedEntityIndex->GetEntitiesAt(cOtherRABs, cRAB.GetPosition());
00143          /* Go through the RAB entities in range */
00144          for(CSet<CRABEquippedEntity*>::iterator it2 = cOtherRABs.begin();
00145              it2 != cOtherRABs.end();
00146              ++it2) {
00147             /* Get a reference to the RAB entity */
00148             CRABEquippedEntity& cOtherRAB = **it2;
00149             /* First, make sure the entities are not the same */
00150             if(&cRAB != &cOtherRAB) {
00151                /* Proceed if the pair has not been checked already */
00152                if(&cRAB < &cOtherRAB) {
00153                   cTestKey.first = &cRAB;
00154                   cTestKey.second = &cOtherRAB;
00155                }
00156                else {
00157                   cTestKey.first = &cOtherRAB;
00158                   cTestKey.second = &cRAB;
00159                }
00160                unTestHash = HashRABPair(cTestKey);
00161                itPair = mapPairsAlreadyChecked.find(unTestHash);
00162                if(itPair == mapPairsAlreadyChecked.end() ||   /* Pair does not exist */
00163                   itPair->second.first != cTestKey.first ||   /* Pair exists, but first RAB involved is different */
00164                   itPair->second.second != cTestKey.second) { /* Pair exists, but second RAB involved is different */
00165                   /* Mark this pair as already checked */
00166                   mapPairsAlreadyChecked[unTestHash] = cTestKey;
00167                   /* Proceed if the message size is compatible */
00168                   if(cRAB.GetMsgSize() == cOtherRAB.GetMsgSize()) {
00169                      /* Proceed if the two entities are not obstructed by another object */
00170                      cOcclusionCheckRay.SetEnd(cOtherRAB.GetPosition());
00171                      if((!GetClosestEmbodiedEntityIntersectedByRay(sIntersectionItem,
00172                                                                    cOcclusionCheckRay,
00173                                                                    cRAB.GetReference())) ||
00174                         (&cOtherRAB.GetReference() == sIntersectionItem.IntersectedEntity)) {
00175                         /* If we get here, the two RAB entities are in direct line of sight */
00176                         /* cRAB can receive cOtherRAB's message if it is in range, and viceversa */
00177                         /* Calculate square distance */
00178                         fDistance = cOcclusionCheckRay.GetLength();
00179                         if(fDistance < cOtherRAB.GetRange()) {
00180                            /* cRAB receives cOtherRAB's message */
00181                            it->second.insert(&cOtherRAB);
00182                         }
00183                         if(fDistance < cRAB.GetRange()) {
00184                            /* cOtherRAB receives cRAB's message */
00185                            m_tRoutingTable[&cOtherRAB].insert(&cRAB);
00186                         }
00187                      }
00188                   }
00189                }
00190             }
00191          }
00192       }
00193    }
00194 
00195    /****************************************/
00196    /****************************************/
00197 
00198    void CRABMedium::AddEntity(CRABEquippedEntity& c_entity) {
00199       m_tRoutingTable.insert(
00200          std::make_pair<CRABEquippedEntity*, CSet<CRABEquippedEntity*> >(
00201             &c_entity, CSet<CRABEquippedEntity*>()));
00202       m_pcRABEquippedEntityIndex->AddEntity(c_entity);
00203    }
00204 
00205    /****************************************/
00206    /****************************************/
00207 
00208    void CRABMedium::RemoveEntity(CRABEquippedEntity& c_entity) {
00209       TRoutingTable::iterator it = m_tRoutingTable.find(&c_entity);
00210       if(it != m_tRoutingTable.end()) {
00211          m_pcRABEquippedEntityIndex->RemoveEntity(c_entity);
00212          m_tRoutingTable.erase(it);
00213       }
00214       else {
00215          THROW_ARGOSEXCEPTION("Can't erase entity \"" << c_entity.GetId() << "\" from RAB medium \"" << GetId() << "\"");
00216       }
00217    }
00218 
00219    /****************************************/
00220    /****************************************/
00221 
00222    const CSet<CRABEquippedEntity*>& CRABMedium::GetRABsCommunicatingWith(CRABEquippedEntity& c_entity) const {
00223       TRoutingTable::const_iterator it = m_tRoutingTable.find(&c_entity);
00224       if(it != m_tRoutingTable.end()) {
00225          return it->second;
00226       }
00227       else {
00228          THROW_ARGOSEXCEPTION("RAB entity \"" << c_entity.GetId() << "\" is not managed by the RAB medium \"" << GetId() << "\"");
00229       }
00230    }
00231 
00232    /****************************************/
00233    /****************************************/
00234 
00235    REGISTER_MEDIUM(CRABMedium,
00236                    "range_and_bearing",
00237                    "Carlo Pinciroli [ilpincy@gmail.com]",
00238                    "1.0",
00239                    "It simulates the communication across range-and-bearing-equipped robots.",
00240                    "This medium is required to simulate communication across range-and-bearing-\n"
00241                    "equipped robots. You need to add it to the <media> section every time you add\n"
00242                    "a range-and-bearing-equipped entity whose controller has a range-and-bearing\n"
00243                    "device activated.\n\n"
00244                    "REQUIRED XML CONFIGURATION\n\n"
00245                    "<range_and_bearing id=\"rab\" />\n\n"
00246                    "OPTIONAL XML CONFIGURATION\n\n"
00247                    "None for the time being\n",
00248                    "Under development"
00249       );
00250 
00251    /****************************************/
00252    /****************************************/
00253 
00254 }