directional_led_equipped_entity.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/plugins/simulator/media/directional_led_medium.h>
11 
12 namespace argos {
13 
14  /****************************************/
15  /****************************************/
16 
18  SAnchor& s_anchor,
19  const CVector3& c_position_offset,
20  const CQuaternion& c_orientation_offset) :
21  LED(c_led),
22  Anchor(s_anchor),
23  PositionOffset(c_position_offset),
24  OrientationOffset(c_orientation_offset) {}
25 
26  /****************************************/
27  /****************************************/
28 
30  CComposableEntity(pc_parent) {
31  Disable();
32  }
33 
34  /****************************************/
35  /****************************************/
36 
38  const std::string& str_id) :
39  CComposableEntity(pc_parent, str_id) {
40  Disable();
41  }
42 
43  /****************************************/
44  /****************************************/
45 
47  try {
48  /* Init parent */
50  /* Go through the led entries */
51  TConfigurationNodeIterator itLED("directional_led");
52  for(itLED = itLED.begin(&t_tree);
53  itLED != itLED.end();
54  ++itLED) {
55  /* Initialise the LED using the XML */
57  pcLED->Init(*itLED);
58  CVector3 cPositionOffset;
59  GetNodeAttribute(*itLED, "position", cPositionOffset);
60  CQuaternion cOrientationOffset;
61  GetNodeAttribute(*itLED, "orientation", cOrientationOffset);
62  /* Parse and look up the anchor */
63  std::string strAnchorId;
64  GetNodeAttribute(*itLED, "anchor", strAnchorId);
65  /*
66  * NOTE: here we get a reference to the embodied entity
67  * This line works under the assumption that:
68  * 1. the DirectionalLEDEquippedEntity has a parent;
69  * 2. the parent has a child whose id is "body"
70  * 3. the "body" is an embodied entity
71  * If any of the above is false, this line will bomb out.
72  */
73  CEmbodiedEntity& cBody =
75  /* Add the LED to this container */
76  m_vecInstances.emplace_back(*pcLED,
77  cBody.GetAnchor(strAnchorId),
78  cPositionOffset,
79  cOrientationOffset);
80  AddComponent(*pcLED);
81  }
83  }
84  catch(CARGoSException& ex) {
85  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize directional LED equipped entity \"" <<
86  GetContext() + GetId() << "\".", ex);
87  }
88  }
89 
90  /****************************************/
91  /****************************************/
92 
94  /* Perform generic enable behavior */
96  /* Enable anchors */
97  for(SInstance& s_instance : m_vecInstances) {
98  s_instance.Anchor.Enable();
99  }
100  }
101 
102  /****************************************/
103  /****************************************/
104 
106  /* Perform generic disable behavior */
108  /* Disable anchors */
109  for(SInstance& s_instance : m_vecInstances) {
110  s_instance.Anchor.Disable();
111  }
112  }
113 
114  /****************************************/
115  /****************************************/
116 
118  ARGOS_ASSERT(un_index < m_vecInstances.size(),
119  "CLEDEquippedEntity::GetLED(), id=\"" <<
120  GetContext() + GetId() <<
121  "\": index out of bounds: un_index = " <<
122  un_index <<
123  ", m_vecInstances.size() = " <<
124  m_vecInstances.size());
125  return m_vecInstances[un_index].LED;
126  }
127 
128  /****************************************/
129  /****************************************/
130 
132  const CColor& c_color) {
133  ARGOS_ASSERT(un_index < m_vecInstances.size(),
134  "CLEDEquippedEntity::SetLEDColor(), id=\"" <<
135  GetContext() + GetId() <<
136  "\": index out of bounds: un_index = " <<
137  un_index <<
138  ", m_vecInstances.size() = " <<
139  m_vecInstances.size());
140  m_vecInstances[un_index].LED.SetColor(c_color);
141  }
142 
143  /****************************************/
144  /****************************************/
145 
147  for(SInstance& s_instance : m_vecInstances) {
148  s_instance.LED.SetColor(c_color);
149  }
150  }
151 
152  /****************************************/
153  /****************************************/
154 
155  void CDirectionalLEDEquippedEntity::SetLEDColors(const std::vector<CColor>& vec_colors) {
156  if(vec_colors.size() == m_vecInstances.size()) {
157  for(UInt32 i = 0; i < vec_colors.size(); ++i) {
158  m_vecInstances[i].LED.SetColor(vec_colors[i]);
159  }
160  }
161  else {
163  "CDirectionalLEDEquippedEntity::SetLEDColors(), id=\"" <<
164  GetContext() + GetId() <<
165  "\": number of LEDs (" <<
166  m_vecInstances.size() <<
167  ") does not equal the passed color vector size (" <<
168  vec_colors.size() <<
169  ")");
170  }
171  }
172 
173  /****************************************/
174  /****************************************/
175 
177  /* LED position wrt global reference frame */
178  CVector3 cLEDPosition;
179  CQuaternion cLEDOrientation;
180  for(SInstance& s_instance : m_vecInstances) {
181  if(s_instance.LED.IsEnabled()) {
182  cLEDPosition = s_instance.PositionOffset;
183  cLEDPosition.Rotate(s_instance.Anchor.Orientation);
184  cLEDPosition += s_instance.Anchor.Position;
185  cLEDOrientation = s_instance.Anchor.Orientation *
186  s_instance.OrientationOffset;
187  s_instance.LED.MoveTo(cLEDPosition, cLEDOrientation);
188  }
189  }
190  }
191 
192  /****************************************/
193  /****************************************/
194 
196  for(SInstance& s_instance : m_vecInstances) {
197  s_instance.LED.SetMedium(c_medium);
198  }
199  }
200 
201  /****************************************/
202  /****************************************/
203 
205 
206  /****************************************/
207  /****************************************/
208 
209 }
virtual void UpdateComponents()
Calls the Update() method on all the components.
A 3D vector class.
Definition: vector3.h:29
void Enable()
Enables the entity.
Definition: entity.h:265
SInstance::TVector m_vecInstances
List of the LEDs managed by this entity.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
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...
void SetLEDColor(UInt32 un_index, const CColor &c_color)
Sets the color of an LED.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
#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
Basic class for an entity that contains other entities.
void SetLEDColors(const CColor &c_color)
Sets the color of all the LEDs to the same value.
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 Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
CDirectionalLEDEntity & GetLED(UInt32 un_index)
Returns an LED by numeric index.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
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
CDirectionalLEDEquippedEntity(CComposableEntity *pc_parent)
Class constructor.
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
A container of CDirectionalLEDEntity.
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
std::string GetContext() const
Returns the context of this entity.
Definition: entity.cpp:79
void SetMedium(CDirectionalLEDMedium &c_medium)
Sets the medium associated to this entity.
SInstance(CDirectionalLEDEntity &c_led, SAnchor &s_anchor, const CVector3 &c_position_offset, const CQuaternion &c_orientation_offset)
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.