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