ARGoS  3
A parallel, multi-engine simulator for swarm robotics
led_equipped_entity.cpp
Go to the documentation of this file.
1 
7 #include "led_equipped_entity.h"
8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/plugins/simulator/media/led_medium.h>
11 
12 namespace argos {
13 
14  /****************************************/
15  /****************************************/
16 
18  const CVector3& c_offset,
19  SAnchor& s_anchor) :
20  LED(c_led),
21  Offset(c_offset),
22  Anchor(s_anchor) {}
23 
24  /****************************************/
25  /****************************************/
26 
28  CComposableEntity(pc_parent) {
29  Disable();
30  }
31 
32  /****************************************/
33  /****************************************/
34 
36  const std::string& str_id) :
37  CComposableEntity(pc_parent, str_id) {
38  Disable();
39  }
40 
41  /****************************************/
42  /****************************************/
43 
45  while(! m_tLEDs.empty()) {
46  delete m_tLEDs.back();
47  m_tLEDs.pop_back();
48  }
49  }
50 
51  /****************************************/
52  /****************************************/
53 
55  try {
56  /* Init parent */
58  /* Go through the led entries */
59  CVector3 cPosition;
60  CColor cColor;
61  TConfigurationNodeIterator itLED("led");
62  for(itLED = itLED.begin(&t_tree);
63  itLED != itLED.end();
64  ++itLED) {
65  /* Initialise the LED using the XML */
66  CLEDEntity* pcLED = new CLEDEntity(this);
67  pcLED->Init(*itLED);
68  /* Parse the offset */
69  CVector3 cOffset;
70  GetNodeAttribute(*itLED, "offset", cOffset);
71  /* Parse and look up the anchor */
72  std::string strAnchorId;
73  GetNodeAttribute(*itLED, "anchor", strAnchorId);
74  /*
75  * NOTE: here we get a reference to the embodied entity
76  * This line works under the assumption that:
77  * 1. the LEDEquippedEntity has a parent;
78  * 2. the parent has a child whose id is "body"
79  * 3. the "body" is an embodied entity
80  * If any of the above is false, this line will bomb out.
81  */
83  /* Add the LED to this container */
84  m_tLEDs.push_back(new SActuator(*pcLED, cOffset, cBody.GetAnchor(strAnchorId)));
85  AddComponent(*pcLED);
86  }
88  }
89  catch(CARGoSException& ex) {
90  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize LED equipped entity \"" << GetId() << "\".", ex);
91  }
92  }
93 
94  /****************************************/
95  /****************************************/
96 
98  for(SActuator::TList::iterator it = m_tLEDs.begin();
99  it != m_tLEDs.end();
100  ++it) {
101  (*it)->LED.Reset();
102  }
103  }
104 
105  /****************************************/
106  /****************************************/
107 
109  CEntity::Enable();
110  for(size_t i = 0; i < m_tLEDs.size(); ++i) {
111  m_tLEDs[i]->Anchor.Enable();
112  }
113  }
114 
115  /****************************************/
116  /****************************************/
117 
120  for(size_t i = 0; i < m_tLEDs.size(); ++i) {
121  m_tLEDs[i]->Anchor.Disable();
122  }
123  }
124 
125  /****************************************/
126  /****************************************/
127 
128  void CLEDEquippedEntity::AddLED(const CVector3& c_offset,
129  SAnchor& s_anchor,
130  const CColor& c_color) {
131  CLEDEntity* pcLED =
132  new CLEDEntity(
133  this,
134  std::string("led_") + ToString(m_tLEDs.size()),
135  c_offset,
136  c_color);
137  m_tLEDs.push_back(new SActuator(*pcLED, c_offset, s_anchor));
138  AddComponent(*pcLED);
139  }
140 
141  /****************************************/
142  /****************************************/
143 
145  Real f_radius,
146  const CRadians& c_start_angle,
147  UInt32 un_num_leds,
148  SAnchor& s_anchor,
149  const CColor& c_color) {
150  CRadians cLEDSpacing = CRadians::TWO_PI / un_num_leds;
151  CRadians cAngle;
152  CVector3 cOffset;
153  for(UInt32 i = 0; i < un_num_leds; ++i) {
154  cAngle = c_start_angle + i * cLEDSpacing;
155  cAngle.SignedNormalize();
156  cOffset.Set(f_radius, 0.0f, 0.0f);
157  cOffset.RotateZ(cAngle);
158  cOffset += c_center;
159  AddLED(cOffset, s_anchor, c_color);
160  }
161  }
162 
163  /****************************************/
164  /****************************************/
165 
167  ARGOS_ASSERT(un_index < m_tLEDs.size(),
168  "CLEDEquippedEntity::GetLED(), id=\"" <<
169  GetId() <<
170  "\": index out of bounds: un_index = " <<
171  un_index <<
172  ", m_tLEDs.size() = " <<
173  m_tLEDs.size());
174  return m_tLEDs[un_index]->LED;
175  }
176 
177  /****************************************/
178  /****************************************/
179 
181  const CVector3& c_offset) {
182  ARGOS_ASSERT(un_index < m_tLEDs.size(),
183  "CLEDEquippedEntity::SetLEDPosition(), id=\"" <<
184  GetId() <<
185  "\": index out of bounds: un_index = " <<
186  un_index <<
187  ", m_tLEDs.size() = " <<
188  m_tLEDs.size());
189  m_tLEDs[un_index]->Offset = c_offset;
190  }
191 
192  /****************************************/
193  /****************************************/
194 
196  const CColor& c_color) {
197  ARGOS_ASSERT(un_index < m_tLEDs.size(),
198  "CLEDEquippedEntity::SetLEDColor(), id=\"" <<
199  GetId() <<
200  "\": index out of bounds: un_index = " <<
201  un_index <<
202  ", m_tLEDs.size() = " <<
203  m_tLEDs.size());
204  m_tLEDs[un_index]->LED.SetColor(c_color);
205  }
206 
207  /****************************************/
208  /****************************************/
209 
211  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
212  m_tLEDs[i]->LED.SetColor(c_color);
213  }
214  }
215 
216  /****************************************/
217  /****************************************/
218 
219  void CLEDEquippedEntity::SetAllLEDsColors(const std::vector<CColor>& vec_colors) {
220  if(vec_colors.size() == m_tLEDs.size()) {
221  for(UInt32 i = 0; i < vec_colors.size(); ++i) {
222  m_tLEDs[i]->LED.SetColor(vec_colors[i]);
223  }
224  }
225  else {
227  "CLEDEquippedEntity::SetAllLEDsColors(), id=\"" <<
228  GetId() <<
229  "\": number of LEDs (" <<
230  m_tLEDs.size() <<
231  ") is lower than the passed color vector size (" <<
232  vec_colors.size() <<
233  ")");
234  }
235  }
236 
237  /****************************************/
238  /****************************************/
239 
241  /* LED position wrt global reference frame */
242  CVector3 cLEDPosition;
243  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
244  if(m_tLEDs[i]->LED.IsEnabled()) {
245  cLEDPosition = m_tLEDs[i]->Offset;
246  cLEDPosition.Rotate(m_tLEDs[i]->Anchor.Orientation);
247  cLEDPosition += m_tLEDs[i]->Anchor.Position;
248  m_tLEDs[i]->LED.SetPosition(cLEDPosition);
249  }
250  }
251  }
252 
253  /****************************************/
254  /****************************************/
255 
257  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
258  m_tLEDs[i]->LED.AddToMedium(c_medium);
259  }
260  Enable();
261  }
262 
263  /****************************************/
264  /****************************************/
265 
267  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
268  m_tLEDs[i]->LED.RemoveFromMedium();
269  }
270  Disable();
271  }
272 
273  /****************************************/
274  /****************************************/
275 
277 
278  /****************************************/
279  /****************************************/
280 
281 }
argos::CVector3::Set
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
argos::CLEDEquippedEntity::AddLEDRing
void AddLEDRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, UInt32 un_num_leds, SAnchor &s_anchor, const CColor &c_color=CColor::BLACK)
Adds a ring of LEDs to this entity.
Definition: led_equipped_entity.cpp:144
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
led_equipped_entity.h
argos::CLEDEquippedEntity::GetLED
CLEDEntity & GetLED(UInt32 un_index)
Returns an LED by numeric index.
Definition: led_equipped_entity.cpp:166
argos::CLEDEquippedEntity::CLEDEquippedEntity
CLEDEquippedEntity(CComposableEntity *pc_parent)
Class constructor.
Definition: led_equipped_entity.cpp:27
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::CLEDEquippedEntity::m_tLEDs
SActuator::TList m_tLEDs
List of the LEDs managed by this entity.
Definition: led_equipped_entity.h:202
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::CLEDEquippedEntity::SetAllLEDsColors
void SetAllLEDsColors(const CColor &c_color)
Sets the color of all the LEDs to the same value.
Definition: led_equipped_entity.cpp:210
argos::CLEDEntity
Definition: led_entity.h:24
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
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::CComposableEntity::GetComponent
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
Definition: composable_entity.cpp:109
argos::CLEDEquippedEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
Definition: led_equipped_entity.cpp:97
argos::CLEDEquippedEntity::SActuator
Definition: led_equipped_entity.h:46
argos::CLEDEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: led_entity.cpp:40
argos::CEntity::Enable
void Enable()
Enables the entity.
Definition: entity.h:239
argos::CRadians::SignedNormalize
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
argos::CVector3::RotateZ
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:265
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CLEDMedium
Definition: led_medium.h:15
argos::CComposableEntity::AddComponent
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
Definition: composable_entity.cpp:72
argos::CLEDEquippedEntity
A container of CLEDEntity.
Definition: led_equipped_entity.h:36
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::REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
argos::TConfigurationNodeIterator
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
Definition: argos_configuration.h:29
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::CLEDEquippedEntity::SActuator::SActuator
SActuator(CLEDEntity &c_led, const CVector3 &c_offset, SAnchor &s_anchor)
Definition: led_equipped_entity.cpp:17
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CLEDEquippedEntity::~CLEDEquippedEntity
~CLEDEquippedEntity()
Class destructor.
Definition: led_equipped_entity.cpp:44
argos::CLEDEquippedEntity::AddToMedium
void AddToMedium(CLEDMedium &c_medium)
Adds the LEDs to the wanted LED medium.
Definition: led_equipped_entity.cpp:256
argos::CLEDEquippedEntity::SetLEDColor
void SetLEDColor(UInt32 un_index, const CColor &c_color)
Sets the color of an LED.
Definition: led_equipped_entity.cpp:195
argos::CLEDEquippedEntity::Enable
virtual void Enable()
Definition: led_equipped_entity.cpp:108
argos::CLEDEquippedEntity::UpdateComponents
virtual void UpdateComponents()
Calls the Update() method on all the components.
Definition: led_equipped_entity.cpp:240
argos::ToString
std::string ToString(const T &t_value)
Converts the given parameter to a std::string.
Definition: string_utilities.h:36
argos::CEntity::GetParent
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:83
ARGOS_ASSERT
#define ARGOS_ASSERT(condition, message)
When code is compiled in debug, this macro throws an ARGoS exception with the passed message if the s...
Definition: argos_exception.h:122
argos::CLEDEquippedEntity::SetLEDOffset
void SetLEDOffset(UInt32 un_index, const CVector3 &c_offset)
Sets the position of an LED.
Definition: led_equipped_entity.cpp:180
argos::CLEDEquippedEntity::Disable
virtual void Disable()
Definition: led_equipped_entity.cpp:118
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos::CLEDEquippedEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: led_equipped_entity.cpp:54
argos::CEntity::Disable
void Disable()
Disables the entity.
Definition: entity.h:249
argos::CColor
The basic color type.
Definition: color.h:25
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::CVector3::Rotate
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
argos::CRadians::TWO_PI
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
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::CLEDEquippedEntity::AddLED
void AddLED(const CVector3 &c_offset, SAnchor &s_anchor, const CColor &c_color=CColor::BLACK)
Adds an LED to this entity.
Definition: led_equipped_entity.cpp:128
argos::CLEDEquippedEntity::RemoveFromMedium
void RemoveFromMedium()
Removes the LEDs from the associated LED medium.
Definition: led_equipped_entity.cpp:266