ARGoS  3
A parallel, multi-engine simulator for swarm robotics
rab_medium.cpp
Go to the documentation of this file.
1 #include "rab_medium.h"
2 #include <argos3/core/simulator/entity/embodied_entity.h>
3 #include <argos3/core/simulator/simulator.h>
4 #include <argos3/core/simulator/space/space.h>
5 #include <argos3/core/simulator/space/positional_indices/grid.h>
6 #include <argos3/core/utility/configuration/argos_exception.h>
7 #include <argos3/core/utility/logging/argos_log.h>
8 
9 namespace argos {
10 
11  /****************************************/
12  /****************************************/
13 
15  }
16 
17  /****************************************/
18  /****************************************/
19 
21  }
22 
23  /****************************************/
24  /****************************************/
25 
27  try {
28  CMedium::Init(t_tree);
29  /* Get the positional index method */
30  std::string strPosIndexMethod("grid");
31  GetNodeAttributeOrDefault(t_tree, "index", strPosIndexMethod, strPosIndexMethod);
32  /* Get the arena center and size */
33  CVector3 cArenaCenter;
34  CVector3 cArenaSize;
35  TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
36  GetNodeAttribute(tArena, "size", cArenaSize);
37  GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
38  /* Create the positional index for embodied entities */
39  if(strPosIndexMethod == "grid") {
40  size_t punGridSize[3];
41  if(!NodeAttributeExists(t_tree, "grid_size")) {
42  punGridSize[0] = cArenaSize.GetX();
43  punGridSize[1] = cArenaSize.GetY();
44  punGridSize[2] = cArenaSize.GetZ();
45  }
46  else {
47  std::string strPosGridSize;
48  GetNodeAttribute(t_tree, "grid_size", strPosGridSize);
49  ParseValues<size_t>(strPosGridSize, 3, punGridSize, ',');
50  }
52  cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
53  punGridSize[0], punGridSize[1], punGridSize[2]);
54  m_pcRABEquippedEntityGridUpdateOperation = new CRABEquippedEntityGridEntityUpdater(*pcGrid);
55  pcGrid->SetUpdateEntityOperation(m_pcRABEquippedEntityGridUpdateOperation);
56  m_pcRABEquippedEntityIndex = pcGrid;
57  }
58  else {
59  THROW_ARGOSEXCEPTION("Unknown method \"" << strPosIndexMethod << "\" for the positional index.");
60  }
61  }
62  catch(CARGoSException& ex) {
63  THROW_ARGOSEXCEPTION_NESTED("Error in initialization of the range-and-bearing medium", ex);
64  }
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  Update();
72  }
73 
74  /****************************************/
75  /****************************************/
76 
78  /* Reset positional index of RAB entities */
79  m_pcRABEquippedEntityIndex->Reset();
80  /* Delete routing table */
81  for(TRoutingTable::iterator it = m_tRoutingTable.begin();
82  it != m_tRoutingTable.end();
83  ++it) {
84  it->second.clear();
85  }
86  }
87 
88  /****************************************/
89  /****************************************/
90 
92  delete m_pcRABEquippedEntityIndex;
93  if(m_pcRABEquippedEntityGridUpdateOperation != NULL) {
94  delete m_pcRABEquippedEntityGridUpdateOperation;
95  }
96  }
97 
98  /****************************************/
99  /****************************************/
100 
101  static UInt64 HashRABPair(const std::pair<CRABEquippedEntity*, CRABEquippedEntity*>& c_pair) {
102  UInt64 unA = *reinterpret_cast<unsigned long long*>(c_pair.first) & 0xFFFFFFFF;
103  UInt64 unB = *reinterpret_cast<unsigned long long*>(c_pair.second) & 0xFFFFFFFF;
104  return (unA << 32) | unB;
105  }
106 
108  /* Update positional index of RAB entities */
109  m_pcRABEquippedEntityIndex->Update();
110  /* Delete routing table */
111  for(TRoutingTable::iterator it = m_tRoutingTable.begin();
112  it != m_tRoutingTable.end();
113  ++it) {
114  it->second.clear();
115  }
116  /* This map contains the pairs that have already been checked */
117  std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> > mapPairsAlreadyChecked;
118  /* Iterator for the above structure */
119  std::map<UInt64, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> >::iterator itPair;
120  /* Used as test key */
121  std::pair<CRABEquippedEntity*, CRABEquippedEntity*> cTestKey;
122  /* Used as hash for the test key */
123  UInt64 unTestHash;
124  /* The ray to use for occlusion checking */
125  CRay3 cOcclusionCheckRay;
126  /* Buffer for the communicating entities */
127  CSet<CRABEquippedEntity*> cOtherRABs;
128  /* Buffer to store the intersection data */
129  SEmbodiedEntityIntersectionItem sIntersectionItem;
130  /* The distance between two RABs in line of sight */
131  Real fDistance;
132  /* Go through the RAB entities */
133  for(TRoutingTable::iterator it = m_tRoutingTable.begin();
134  it != m_tRoutingTable.end();
135  ++it) {
136  /* Get a reference to the current RAB entity */
137  CRABEquippedEntity& cRAB = *(it->first);
138  /* Initialize the occlusion check ray start to the position of the robot */
139  cOcclusionCheckRay.SetStart(cRAB.GetPosition());
140  /* For each RAB entity, get the list of RAB entities in range */
141  cOtherRABs.clear();
142  m_pcRABEquippedEntityIndex->GetEntitiesAt(cOtherRABs, cRAB.GetPosition());
143  /* Go through the RAB entities in range */
144  for(CSet<CRABEquippedEntity*>::iterator it2 = cOtherRABs.begin();
145  it2 != cOtherRABs.end();
146  ++it2) {
147  /* Get a reference to the RAB entity */
148  CRABEquippedEntity& cOtherRAB = **it2;
149  /* First, make sure the entities are not the same */
150  if(&cRAB != &cOtherRAB) {
151  /* Proceed if the pair has not been checked already */
152  if(&cRAB < &cOtherRAB) {
153  cTestKey.first = &cRAB;
154  cTestKey.second = &cOtherRAB;
155  }
156  else {
157  cTestKey.first = &cOtherRAB;
158  cTestKey.second = &cRAB;
159  }
160  unTestHash = HashRABPair(cTestKey);
161  itPair = mapPairsAlreadyChecked.find(unTestHash);
162  if(itPair == mapPairsAlreadyChecked.end() || /* Pair does not exist */
163  itPair->second.first != cTestKey.first || /* Pair exists, but first RAB involved is different */
164  itPair->second.second != cTestKey.second) { /* Pair exists, but second RAB involved is different */
165  /* Mark this pair as already checked */
166  mapPairsAlreadyChecked[unTestHash] = cTestKey;
167  /* Proceed if the message size is compatible */
168  if(cRAB.GetMsgSize() == cOtherRAB.GetMsgSize()) {
169  /* Proceed if the two entities are not obstructed by another object */
170  cOcclusionCheckRay.SetEnd(cOtherRAB.GetPosition());
171  if((!GetClosestEmbodiedEntityIntersectedByRay(sIntersectionItem,
172  cOcclusionCheckRay,
173  cRAB.GetEntityBody())) ||
174  (&cOtherRAB.GetEntityBody() == sIntersectionItem.IntersectedEntity)) {
175  /* If we get here, the two RAB entities are in direct line of sight */
176  /* cRAB can receive cOtherRAB's message if it is in range, and viceversa */
177  /* Calculate square distance */
178  fDistance = cOcclusionCheckRay.GetLength();
179  if(fDistance < cOtherRAB.GetRange()) {
180  /* cRAB receives cOtherRAB's message */
181  it->second.insert(&cOtherRAB);
182  }
183  if(fDistance < cRAB.GetRange()) {
184  /* cOtherRAB receives cRAB's message */
185  m_tRoutingTable[&cOtherRAB].insert(&cRAB);
186  }
187  } // occlusion found?
188  } // is msg size the same?
189  } // is check necessary?
190  } // are entities the same?
191  } // for entities in range
192  } // for routing table
193  }
194 
195  /****************************************/
196  /****************************************/
197 
199  m_tRoutingTable.insert(
201  &c_entity, CSet<CRABEquippedEntity*>()));
202  m_pcRABEquippedEntityIndex->AddEntity(c_entity);
203  }
204 
205  /****************************************/
206  /****************************************/
207 
209  TRoutingTable::iterator it = m_tRoutingTable.find(&c_entity);
210  if(it != m_tRoutingTable.end()) {
211  m_pcRABEquippedEntityIndex->RemoveEntity(c_entity);
212  m_tRoutingTable.erase(it);
213  }
214  else {
215  THROW_ARGOSEXCEPTION("Can't erase entity \"" << c_entity.GetId() << "\" from RAB medium \"" << GetId() << "\"");
216  }
217  }
218 
219  /****************************************/
220  /****************************************/
221 
223  TRoutingTable::const_iterator it = m_tRoutingTable.find(&c_entity);
224  if(it != m_tRoutingTable.end()) {
225  return it->second;
226  }
227  else {
228  THROW_ARGOSEXCEPTION("RAB entity \"" << c_entity.GetId() << "\" is not managed by the RAB medium \"" << GetId() << "\"");
229  }
230  }
231 
232  /****************************************/
233  /****************************************/
234 
236  "range_and_bearing",
237  "Carlo Pinciroli [ilpincy@gmail.com]",
238  "1.0",
239  "It simulates the communication across range-and-bearing-equipped robots.",
240  "This medium is required to simulate communication across range-and-bearing-\n"
241  "equipped robots. You need to add it to the <media> section every time you add\n"
242  "a range-and-bearing-equipped entity whose controller has a range-and-bearing\n"
243  "device activated.\n\n"
244  "REQUIRED XML CONFIGURATION\n\n"
245  "<range_and_bearing id=\"rab\" />\n\n"
246  "OPTIONAL XML CONFIGURATION\n\n"
247  "None for the time being\n",
248  "Under development"
249  );
250 
251  /****************************************/
252  /****************************************/
253 
254 }
argos::CVector3::GetZ
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
argos::CSimulator::GetInstance
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:87
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CRABMedium::CRABMedium
CRABMedium()
Class constructor.
Definition: rab_medium.cpp:14
argos::CRABMedium::Init
virtual void Init(TConfigurationNode &t_tree)
Initialized the medium.
Definition: rab_medium.cpp:26
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CVector3
A 3D vector class.
Definition: vector3.h:29
argos::CGrid
Definition: grid.h:12
argos::CRABMedium::~CRABMedium
virtual ~CRABMedium()
Class destructor.
Definition: rab_medium.cpp:20
argos::GetNode
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
Definition: argos_configuration.h:63
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CRay3
Definition: ray3.h:19
argos::SEmbodiedEntityIntersectionItem
Definition: physics_engine.h:32
argos::CRABMedium::Update
virtual void Update()
Updates the state of this medium.
Definition: rab_medium.cpp:107
argos::CRABMedium::Destroy
virtual void Destroy()
Undoes whatever was done by Init().
Definition: rab_medium.cpp:91
argos::CSet::clear
void clear()
Erases the contents of the list.
Definition: set.h:350
rab_medium.h
argos::REGISTER_MEDIUM
REGISTER_MEDIUM(CLEDMedium, "led", "Carlo Pinciroli [ilpincy@gmail.com]", "1.0", "Manages the LEDs.", "This medium is required to manage the LED entities, thus allowing the\n" "associated camera sensors to work properly. If you intend to use a camera\n" "sensor that detects colored blobs, you must add this medium to the XML\n" "configuration file.\n\n" "REQUIRED XML CONFIGURATION\n\n" "<led id=\"led\" />\n\n" "OPTIONAL XML CONFIGURATION\n\n" "None for the time being\n", "Under development")
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CRABEquippedEntity::GetRange
Real GetRange() const
Definition: rab_equipped_entity.h:75
argos::CRABEquippedEntity
Definition: rab_equipped_entity.h:25
argos::CRABEquippedEntity::GetEntityBody
CEmbodiedEntity & GetEntityBody()
Definition: rab_equipped_entity.h:59
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
argos::CSet::end
iterator end() const
Returns an invalid iterator.
Definition: set.h:396
argos::CPositionalEntity::GetPosition
const CVector3 & GetPosition() const
Definition: positional_entity.h:36
argos::GetClosestEmbodiedEntityIntersectedByRay
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
Definition: physics_engine.cpp:41
THROW_ARGOSEXCEPTION_NESTED
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
Definition: argos_exception.h:115
argos::CRABMedium::PostSpaceInit
virtual void PostSpaceInit()
Executes extra initialization activities after the space has been initialized.
Definition: rab_medium.cpp:70
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::SEmbodiedEntityIntersectionItem::IntersectedEntity
CEmbodiedEntity * IntersectedEntity
Definition: physics_engine.h:33
argos::CRABEquippedEntity::GetMsgSize
size_t GetMsgSize() const
Definition: rab_equipped_entity.h:63
argos::CSetIterator
The CSet iterator.
Definition: set.h:39
argos::CMedium::Init
virtual void Init(TConfigurationNode &t_tree)
Initialized the medium.
Definition: medium.cpp:15
argos::CRABMedium::GetRABsCommunicatingWith
const CSet< CRABEquippedEntity * > & GetRABsCommunicatingWith(CRABEquippedEntity &c_entity) const
Returns an immutable vector of RAB entities that can communicated with the given entity.
Definition: rab_medium.cpp:222
argos::GetNodeAttributeOrDefault
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
Definition: argos_configuration.h:318
argos::CRay3::SetStart
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
argos::CRABMedium::AddEntity
void AddEntity(CRABEquippedEntity &c_entity)
Adds the specified entity to the list of managed entities.
Definition: rab_medium.cpp:198
argos::GetNodeAttribute
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
Definition: argos_configuration.h:208
argos::CRABMedium
Definition: rab_medium.h:16
argos::CSet::begin
iterator begin() const
Returns an iterator to the first element.
Definition: set.h:388
argos::NodeAttributeExists
bool NodeAttributeExists(TConfigurationNode &t_node, const std::string &str_attribute)
Returns true if the specified attribute of a node exists.
Definition: argos_configuration.h:172
argos::CMedium::GetId
const std::string & GetId() const
Returns the id of this medium.
Definition: medium.h:75
argos::CRay3::GetLength
Real GetLength() const
Definition: ray3.h:96
argos::CRay3::SetEnd
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
argos::CRABEquippedEntityGridEntityUpdater
Definition: rab_equipped_entity.h:140
argos::CGrid::SetUpdateEntityOperation
void SetUpdateEntityOperation(CEntityOperation *pc_operation)
Definition: grid_impl.h:907
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
UInt64
unsigned long long UInt64
64-bit unsigned integer.
Definition: datatypes.h:107
argos::CRABMedium::RemoveEntity
void RemoveEntity(CRABEquippedEntity &c_entity)
Removes the specified entity from the list of managed entities.
Definition: rab_medium.cpp:208
argos::CSet
Defines a very simple double-linked list that stores unique elements.
Definition: set.h:100
argos::CRABMedium::Reset
virtual void Reset()
Resets the resource.
Definition: rab_medium.cpp:77