ARGoS  3
A parallel, multi-engine simulator for swarm robotics
rab_equipped_entity.cpp
Go to the documentation of this file.
1 
7 #include "rab_equipped_entity.h"
8 #include <argos3/core/utility/string_utilities.h>
9 #include <argos3/core/simulator/simulator.h>
10 #include <argos3/core/simulator/space/space.h>
11 #include <argos3/core/simulator/entity/composable_entity.h>
12 #include <argos3/core/simulator/entity/embodied_entity.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
20  CPositionalEntity(pc_parent),
21  m_psAnchor(NULL),
22  m_fRange(0.0f),
23  m_pcEntityBody(NULL) {
24  Disable();
25  }
26 
27  /****************************************/
28  /****************************************/
29 
31  const std::string& str_id,
32  size_t un_msg_size,
33  Real f_range,
34  SAnchor& s_anchor,
35  CEmbodiedEntity& c_entity_body,
36  const CVector3& c_pos_offset,
37  const CQuaternion& c_rot_offset) :
38  CPositionalEntity(pc_parent,
39  str_id),
40  m_psAnchor(&s_anchor),
41  m_cPosOffset(c_pos_offset),
42  m_cRotOffset(c_rot_offset),
43  m_cData(un_msg_size),
44  m_fRange(f_range),
45  m_pcEntityBody(&c_entity_body) {
46  Disable();
47  CVector3 cPos = c_pos_offset;
48  cPos.Rotate(s_anchor.Orientation);
49  cPos += s_anchor.Position;
50  SetInitPosition(cPos);
51  SetPosition(cPos);
52  SetInitOrientation(s_anchor.Orientation * c_rot_offset);
54  }
55 
56  /****************************************/
57  /****************************************/
58 
60  try {
61  /*
62  * Init entity.
63  * Here we explicitly avoid to call CPositionalEntity::Init() because that
64  * would also initialize position and orientation, which, instead, must
65  * be calculated from reference entity and offsets.
66  */
67  CEntity::Init(t_tree);
68  /* Get offsets */
69  GetNodeAttributeOrDefault(t_tree, "pos_offset", m_cPosOffset, m_cPosOffset);
70  std::string strRotOffset;
71  GetNodeAttributeOrDefault(t_tree, "rot_offset", strRotOffset, strRotOffset);
72  if(strRotOffset != "") {
73  CDegrees cRotOffsetEuler[3];
74  ParseValues(strRotOffset, 3, cRotOffsetEuler, ',');
75  m_cRotOffset.FromEulerAngles(ToRadians(cRotOffsetEuler[0]),
76  ToRadians(cRotOffsetEuler[1]),
77  ToRadians(cRotOffsetEuler[2]));
78  }
79  /* Parse and look up the anchor */
80  std::string strAnchorId;
81  GetNodeAttribute(t_tree, "anchor", strAnchorId);
82  /*
83  * NOTE: here we get a reference to the embodied entity
84  * This line works under the assumption that:
85  * 1. the RABEquippedEntity has a parent;
86  * 2. the parent has a child whose id is "body"
87  * 3. the "body" is an embodied entity
88  * If any of the above is false, this line will bomb out.
89  */
91  m_psAnchor = &m_pcEntityBody->GetAnchor(strAnchorId);
92  /* Get message size */
93  size_t unMsgSize;
94  GetNodeAttribute(t_tree, "msg_size", unMsgSize);
95  m_cData.Resize(unMsgSize);
96  /* Get transmission range */
97  GetNodeAttribute(t_tree, "range", m_fRange);
98  /* Set init position and orientation */
99  Update();
102  }
103  catch(CARGoSException& ex) {
104  THROW_ARGOSEXCEPTION_NESTED("Error initializing a range and bearing entity \"" << GetId() << "\"", ex);
105  }
106  }
107 
108  /****************************************/
109  /****************************************/
110 
112  CVector3 cPos = m_cPosOffset;
114  cPos += m_psAnchor->Position;
115  SetPosition(cPos);
117  }
118 
119  /****************************************/
120  /****************************************/
121 
123  m_cData.Zero();
124  }
125 
126  /****************************************/
127  /****************************************/
128 
131  }
132 
133  /****************************************/
134  /****************************************/
135 
138  }
139 
140  /****************************************/
141  /****************************************/
142 
144  if(m_cData.Size() == c_data.Size()) {
145  m_cData = c_data;
146  }
147  else {
148  THROW_ARGOSEXCEPTION("CRABEquippedEntity::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size());
149  }
150  }
151 
152  /****************************************/
153  /****************************************/
154 
156  m_cData.Zero();
157  }
158 
159  /****************************************/
160  /****************************************/
161 
163  CRABEquippedEntity& c_element) {
164  /* Calculate the position of the center of the RAB equipped entity in the space hash */
165  c_space_hash.SpaceToHashTable(m_nCenterI,
166  m_nCenterJ,
167  m_nCenterK,
168  c_element.GetPosition());
169  /* Update the cells in a sphere around it */
170  SInt32 nRangeI = c_space_hash.SpaceToHashTable(c_element.GetRange(), 0);
171  SInt32 nRangeJ;
172  SInt32 nRangeK;
173  for(SInt32 i = 0; i <= nRangeI; ++i) {
174  nRangeJ =
175  c_space_hash.SpaceToHashTable(
176  ::sqrt(
177  Square(c_element.GetRange()) -
178  Square(c_space_hash.HashTableToSpace(i, 0))
179  ),
180  1);
181  for(SInt32 j = 0; j <= nRangeJ; ++j) {
182  nRangeK =
183  c_space_hash.SpaceToHashTable(
184  ::sqrt(
185  Square(c_element.GetRange()) -
186  Square(c_space_hash.HashTableToSpace(j, 1))
187  ),
188  2);
189  for(SInt32 k = 0; k <= nRangeK; ++k) {
190  if(i > 0) {
191  /*
192  * i > 0
193  */
194  if(j > 0) {
195  /*
196  * i > 0
197  * j > 0
198  */
199  if(k > 0) {
200  /*
201  * i > 0
202  * j > 0
203  * k > 0
204  */
205  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK + k, c_element);
206  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK - k, c_element);
207  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK + k, c_element);
208  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK - k, c_element);
209  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK + k, c_element);
210  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK - k, c_element);
211  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK + k, c_element);
212  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK - k, c_element);
213  }
214  else {
215  /*
216  * i > 0
217  * j > 0
218  * k == 0
219  */
220  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK, c_element);
221  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK, c_element);
222  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK, c_element);
223  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK, c_element);
224  }
225  }
226  else {
227  /*
228  * i > 0
229  * j == 0
230  */
231  if(k > 0) {
232  /*
233  * i > 0
234  * j == 0
235  * k > 0
236  */
237  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK + k, c_element);
238  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK - k, c_element);
239  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK + k, c_element);
240  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK - k, c_element);
241  }
242  else {
243  /*
244  * i > 0
245  * j == 0
246  * k == 0
247  */
248  c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK, c_element);
249  c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK, c_element);
250  }
251  }
252  }
253  else {
254  /*
255  * i == 0
256  */
257  if(j > 0) {
258  /*
259  * i == 0
260  * j > 0
261  */
262  if(k > 0) {
263  /*
264  * i == 0
265  * j > 0
266  * k > 0
267  */
268  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK + k, c_element);
269  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK - k, c_element);
270  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK + k, c_element);
271  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK - k, c_element);
272  }
273  else {
274  /*
275  * i == 0
276  * j > 0
277  * k == 0
278  */
279  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK, c_element);
280  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK, c_element);
281  }
282  }
283  else {
284  /*
285  * i == 0
286  * j == 0
287  */
288  if(k > 0) {
289  /*
290  * i == 0
291  * j == 0
292  * k > 0
293  */
294  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK + k, c_element);
295  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK - k, c_element);
296  }
297  else {
298  /*
299  * i == 0
300  * j == 0
301  * k == 0
302  */
303  c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK, c_element);
304  }
305  }
306  }
307  }
308  }
309  }
310  }
311 
312  /****************************************/
313  /****************************************/
314 
316 
317  /****************************************/
318  /****************************************/
319 
321  m_cGrid(c_grid) {}
322 
324  SInt32 n_j,
325  SInt32 n_k,
327  /* Update cell */
328  m_cGrid.UpdateCell(n_i, n_j, n_k, *m_pcEntity);
329  /* Continue with other cells */
330  return true;
331  }
332 
334  m_pcEntity = &c_entity;
335  }
336 
338  m_cGrid(c_grid),
339  m_cCellUpdater(c_grid) {}
340 
342  try {
343  m_cCellUpdater.SetEntity(c_entity);
344  m_cGrid.ForCellsInBoxRange(c_entity.GetPosition(),
345  CVector3(c_entity.GetRange(),
346  c_entity.GetRange(),
347  c_entity.GetRange()),
348  m_cCellUpdater);
349  /* Continue with the other entities */
350  return true;
351  }
352  catch(CARGoSException& ex) {
353  THROW_ARGOSEXCEPTION_NESTED("While updating the RAB entity grid for RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex);
354  }
355  }
356 
357  /****************************************/
358  /****************************************/
359 
360 }
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::CAbstractSpaceHash
The abstract definition of a space hash.
Definition: space_hash.h:34
argos::CRABEquippedEntity::Enable
virtual void Enable()
Definition: rab_equipped_entity.cpp:129
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CRABEquippedEntityGridCellUpdater::SetEntity
void SetEntity(CRABEquippedEntity &c_entity)
Definition: rab_equipped_entity.cpp:333
argos::CPositionalEntity::SetInitOrientation
void SetInitOrientation(const CQuaternion c_orientation)
Definition: positional_entity.h:64
rab_equipped_entity.h
argos::CRABEquippedEntityGridCellUpdater::operator()
virtual bool operator()(SInt32 n_i, SInt32 n_j, SInt32 n_k, CGrid< CRABEquippedEntity >::SCell &s_cell)
Definition: rab_equipped_entity.cpp:323
argos::CRABEquippedEntitySpaceHashUpdater::operator()
virtual void operator()(CAbstractSpaceHash< CRABEquippedEntity > &c_space_hash, CRABEquippedEntity &c_element)
Updates the necessary cells of a space hash.
Definition: rab_equipped_entity.cpp:162
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::CPositionalEntity::GetInitOrientation
const CQuaternion & GetInitOrientation() const
Definition: positional_entity.h:56
argos::CGrid
Definition: grid.h:12
argos::CEmbodiedEntity::GetAnchor
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
Definition: embodied_entity.cpp:190
argos::ParseValues
void ParseValues(std::istream &str_input, UInt32 un_num_fields, T *pt_field_buffer, const char ch_delimiter='\n')
Definition: string_utilities.h:70
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CByteArray::Resize
void Resize(size_t un_size, UInt8 un_value=0)
Resizes the byte array to the wanted size.
Definition: byte_array.h:83
argos::CComposableEntity::GetComponent
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
Definition: composable_entity.cpp:109
argos::CAbstractSpaceHash::UpdateCell
virtual void UpdateCell(SInt32 n_x, SInt32 n_y, SInt32 n_z, ENTITY &c_entity)=0
Adds an entity to a cell of the space hash.
argos::REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CEntity)
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::CPositionalEntity::SetInitPosition
void SetInitPosition(const CVector3 &c_position)
Definition: positional_entity.h:48
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::SetData
void SetData(const CByteArray &c_data)
Definition: rab_equipped_entity.cpp:143
argos::CRABEquippedEntityGridEntityUpdater::CRABEquippedEntityGridEntityUpdater
CRABEquippedEntityGridEntityUpdater(CGrid< CRABEquippedEntity > &c_grid)
Definition: rab_equipped_entity.cpp:337
argos::CRABEquippedEntity
Definition: rab_equipped_entity.h:25
argos::CByteArray
Byte array utility class.
Definition: byte_array.h:28
argos::SAnchor::Enable
void Enable()
Enables this anchor.
Definition: physics_model.cpp:30
argos::CPositionalEntity::GetPosition
const CVector3 & GetPosition() const
Definition: positional_entity.h:36
argos::CRABEquippedEntity::m_cData
CByteArray m_cData
Definition: rab_equipped_entity.h:96
argos::CQuaternion
Definition: quaternion.h:14
argos::CPositionalEntity
Definition: positional_entity.h:20
argos::CRABEquippedEntity::m_cPosOffset
CVector3 m_cPosOffset
Definition: rab_equipped_entity.h:94
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::CRABEquippedEntity::Disable
virtual void Disable()
Definition: rab_equipped_entity.cpp:136
argos::CRABEquippedEntity::ClearData
void ClearData()
Definition: rab_equipped_entity.cpp:155
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::CRABEquippedEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: rab_equipped_entity.cpp:59
argos::CPositionalEntity::SetPosition
void SetPosition(const CVector3 &c_position)
Definition: positional_entity.h:44
argos::CAbstractSpaceHash::HashTableToSpace
virtual Real HashTableToSpace(SInt32 n_coord, UInt32 un_axis)
Converts a single space hash cell coordinate into a space coordinate.
Definition: space_hash.h:174
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CRABEquippedEntity::Update
virtual void Update()
Updates the state of this entity.
Definition: rab_equipped_entity.cpp:111
argos::CRABEquippedEntity::m_psAnchor
SAnchor * m_psAnchor
Definition: rab_equipped_entity.h:93
SInt32
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
argos::CAbstractSpaceHash::SpaceToHashTable
virtual SInt32 SpaceToHashTable(Real f_coord, UInt32 un_axis)
Converts a single space coordinate into a space hash cell coordinate.
Definition: space_hash.h:163
argos::CRABEquippedEntityGridEntityUpdater::operator()
virtual bool operator()(CRABEquippedEntity &c_entity)
Definition: rab_equipped_entity.cpp:341
argos::CRABEquippedEntity::CRABEquippedEntity
CRABEquippedEntity(CComposableEntity *pc_parent)
Definition: rab_equipped_entity.cpp:19
argos::CGrid::SCell
Definition: grid.h:18
argos::CPositionalEntity::GetOrientation
const CQuaternion & GetOrientation() const
Definition: positional_entity.h:52
argos::CEntity::GetParent
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:83
argos::CByteArray::Zero
void Zero()
Sets the contents of the byte array to all zeros.
Definition: byte_array.cpp:81
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::CRABEquippedEntity::m_cRotOffset
CQuaternion m_cRotOffset
Definition: rab_equipped_entity.h:95
argos::CPositionalEntity::SetOrientation
void SetOrientation(const CQuaternion c_orientation)
Definition: positional_entity.h:60
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::CEntity::GetContext
std::string GetContext() const
Returns the context of this entity.
Definition: entity.cpp:71
argos::Square
T Square(const T &t_v)
Returns the square of the value of the passed argument.
Definition: general.h:128
argos::CVector3::Rotate
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
argos::CDegrees
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
argos::CRABEquippedEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
Definition: rab_equipped_entity.cpp:122
argos::CQuaternion::FromEulerAngles
CQuaternion & FromEulerAngles(const CRadians &c_z_angle, const CRadians &c_y_angle, const CRadians &c_x_angle)
Definition: quaternion.h:162
argos::CRABEquippedEntity::m_fRange
Real m_fRange
Definition: rab_equipped_entity.h:97
argos::ToRadians
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
argos::SAnchor::Disable
void Disable()
Disables this anchor.
Definition: physics_model.cpp:37
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:36
argos::CRABEquippedEntityGridCellUpdater::CRABEquippedEntityGridCellUpdater
CRABEquippedEntityGridCellUpdater(CGrid< CRABEquippedEntity > &c_grid)
Definition: rab_equipped_entity.cpp:320
argos::CRABEquippedEntity::m_pcEntityBody
CEmbodiedEntity * m_pcEntityBody
Definition: rab_equipped_entity.h:98
argos::CByteArray::Size
size_t Size() const
Returns the current size of the byte array.
Definition: byte_array.h:66