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  /* Perform generic enable behavior */
111  /* Enable anchors */
112  for(size_t i = 0; i < m_tLEDs.size(); ++i) {
113  m_tLEDs[i]->Anchor.Enable();
114  }
115  }
116 
117  /****************************************/
118  /****************************************/
119 
121  /* Perform generic disable behavior */
123  /* Disable anchors */
124  for(size_t i = 0; i < m_tLEDs.size(); ++i) {
125  m_tLEDs[i]->Anchor.Disable();
126  }
127  }
128 
129  /****************************************/
130  /****************************************/
131 
132  void CLEDEquippedEntity::AddLED(const CVector3& c_offset,
133  SAnchor& s_anchor,
134  const CColor& c_color) {
135  CLEDEntity* pcLED =
136  new CLEDEntity(
137  this,
138  std::string("led_") + ToString(m_tLEDs.size()),
139  c_offset,
140  c_color);
141  m_tLEDs.push_back(new SActuator(*pcLED, c_offset, s_anchor));
142  AddComponent(*pcLED);
143  }
144 
145  /****************************************/
146  /****************************************/
147 
149  Real f_radius,
150  const CRadians& c_start_angle,
151  UInt32 un_num_leds,
152  SAnchor& s_anchor,
153  const CColor& c_color) {
154  CRadians cLEDSpacing = CRadians::TWO_PI / un_num_leds;
155  CRadians cAngle;
156  CVector3 cOffset;
157  for(UInt32 i = 0; i < un_num_leds; ++i) {
158  cAngle = c_start_angle + i * cLEDSpacing;
159  cAngle.SignedNormalize();
160  cOffset.Set(f_radius, 0.0f, 0.0f);
161  cOffset.RotateZ(cAngle);
162  cOffset += c_center;
163  AddLED(cOffset, s_anchor, c_color);
164  }
165  }
166 
167  /****************************************/
168  /****************************************/
169 
171  ARGOS_ASSERT(un_index < m_tLEDs.size(),
172  "CLEDEquippedEntity::GetLED(), id=\"" <<
173  GetId() <<
174  "\": index out of bounds: un_index = " <<
175  un_index <<
176  ", m_tLEDs.size() = " <<
177  m_tLEDs.size());
178  return m_tLEDs[un_index]->LED;
179  }
180 
181  /****************************************/
182  /****************************************/
183 
185  const CVector3& c_offset) {
186  ARGOS_ASSERT(un_index < m_tLEDs.size(),
187  "CLEDEquippedEntity::SetLEDPosition(), id=\"" <<
188  GetId() <<
189  "\": index out of bounds: un_index = " <<
190  un_index <<
191  ", m_tLEDs.size() = " <<
192  m_tLEDs.size());
193  m_tLEDs[un_index]->Offset = c_offset;
194  }
195 
196  /****************************************/
197  /****************************************/
198 
200  const CColor& c_color) {
201  ARGOS_ASSERT(un_index < m_tLEDs.size(),
202  "CLEDEquippedEntity::SetLEDColor(), id=\"" <<
203  GetId() <<
204  "\": index out of bounds: un_index = " <<
205  un_index <<
206  ", m_tLEDs.size() = " <<
207  m_tLEDs.size());
208  m_tLEDs[un_index]->LED.SetColor(c_color);
209  }
210 
211  /****************************************/
212  /****************************************/
213 
215  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
216  m_tLEDs[i]->LED.SetColor(c_color);
217  }
218  }
219 
220  /****************************************/
221  /****************************************/
222 
223  void CLEDEquippedEntity::SetAllLEDsColors(const std::vector<CColor>& vec_colors) {
224  if(vec_colors.size() == m_tLEDs.size()) {
225  for(UInt32 i = 0; i < vec_colors.size(); ++i) {
226  m_tLEDs[i]->LED.SetColor(vec_colors[i]);
227  }
228  }
229  else {
231  "CLEDEquippedEntity::SetAllLEDsColors(), id=\"" <<
232  GetId() <<
233  "\": number of LEDs (" <<
234  m_tLEDs.size() <<
235  ") is lower than the passed color vector size (" <<
236  vec_colors.size() <<
237  ")");
238  }
239  }
240 
241  /****************************************/
242  /****************************************/
243 
245  /* LED position wrt global reference frame */
246  CVector3 cLEDPosition;
247  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
248  if(m_tLEDs[i]->LED.IsEnabled()) {
249  cLEDPosition = m_tLEDs[i]->Offset;
250  cLEDPosition.Rotate(m_tLEDs[i]->Anchor.Orientation);
251  cLEDPosition += m_tLEDs[i]->Anchor.Position;
252  m_tLEDs[i]->LED.SetPosition(cLEDPosition);
253  }
254  }
255  }
256 
257  /****************************************/
258  /****************************************/
259 
261  for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
262  m_tLEDs[i]->LED.SetMedium(c_medium);
263  }
264  }
265 
266  /****************************************/
267  /****************************************/
268 
270 
271  /****************************************/
272  /****************************************/
273 
274 }
CLEDEntity & GetLED(UInt32 un_index)
Returns an LED by numeric index.
A 3D vector class.
Definition: vector3.h:29
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:265
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void Enable()
Enables the entity.
Definition: entity.h:265
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
void AddLED(const CVector3 &c_offset, SAnchor &s_anchor, const CColor &c_color=CColor::BLACK)
Adds an LED to this entity.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
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.
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: led_entity.cpp:40
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
This entity is a link to a body in the physics engine.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
void SetMedium(CLEDMedium &c_medium)
Sets the medium associated to this entity.
~CLEDEquippedEntity()
Class destructor.
virtual void UpdateComponents()
Calls the Update() method on all the components.
#define ARGOS_ASSERT(condition, message)
When code is compiled in debug, this macro throws an ARGoS exception with the passed message if the s...
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
A container of CLEDEntity.
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
Basic class for an entity that contains other entities.
An anchor related to the body of an entity.
Definition: physics_model.h:38
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
void SetLEDOffset(UInt32 un_index, const CVector3 &c_offset)
Sets the position of an LED.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
std::string ToString(const T &t_value)
Converts the given parameter to a std::string.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
CLEDEquippedEntity(CComposableEntity *pc_parent)
Class constructor.
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
The basic color type.
Definition: color.h:25
void Disable()
Disables the entity.
Definition: entity.h:275
void SetLEDColor(UInt32 un_index, const CColor &c_color)
Sets the color of an LED.
SActuator::TList m_tLEDs
List of the LEDs managed by this entity.
void SetAllLEDsColors(const CColor &c_color)
Sets the color of all the LEDs to the same value.
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
SActuator(CLEDEntity &c_led, const CVector3 &c_offset, SAnchor &s_anchor)
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.