light_sensor_equipped_entity.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/simulator/space/space.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
17  const CVector3& c_direction,
18  Real f_range,
19  SAnchor& s_anchor) :
20  Position(c_position),
21  Direction(c_direction),
22  Anchor(s_anchor) {
24  Direction *= f_range;
25  }
26 
27  /****************************************/
28  /****************************************/
29 
31  CEntity(pc_parent) {
32  Disable();
33  }
34 
35  /****************************************/
36  /****************************************/
37 
39  const std::string& str_id) :
40  CEntity(pc_parent, str_id) {
41  Disable();
42  }
43 
44  /****************************************/
45  /****************************************/
46 
48  while(! m_tSensors.empty()) {
49  delete m_tSensors.back();
50  m_tSensors.pop_back();
51  }
52  }
53 
54  /****************************************/
55  /****************************************/
56 
58  try {
59  /*
60  * Parse basic entity stuff
61  */
62  CEntity::Init(t_tree);
63  /*
64  * Parse light sensors
65  */
66  /* Not adding any sensor is a fatal error */
67  if(t_tree.NoChildren()) {
68  THROW_ARGOSEXCEPTION("No sensors defined");
69  }
70  /* Go through children */
72  for(it = it.begin(&t_tree); it != it.end(); ++it) {
73  std::string strAnchorId;
74  GetNodeAttribute(*it, "anchor", strAnchorId);
75  /*
76  * NOTE: here we get a reference to the embodied entity
77  * This line works under the assumption that:
78  * 1. the entity has a parent;
79  * 2. the parent has a child whose id is "body"
80  * 3. the "body" is an embodied entity
81  * If any of the above is false, this line will bomb out.
82  */
83  auto& cBody = GetParent().GetComponent<CEmbodiedEntity>("body");
84  if(it->Value() == "sensor") {
85  CVector3 cPos, cDir;
86  Real fRange;
87  GetNodeAttribute(*it, "position", cPos);
88  GetNodeAttribute(*it, "direction", cDir);
89  GetNodeAttribute(*it, "range", fRange);
90  AddSensor(cPos, cDir, fRange, cBody.GetAnchor(strAnchorId));
91  }
92  else if(it->Value() == "ring") {
93  CVector3 cRingCenter;
94  GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter);
95  Real fRadius;
96  GetNodeAttribute(t_tree, "radius", fRadius);
97  CDegrees cRingStartAngleDegrees;
98  GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees);
99  CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees);
100  Real fRange;
101  GetNodeAttribute(t_tree, "range", fRange);
102  UInt32 unNumSensors;
103  GetNodeAttribute(t_tree, "num_sensors", unNumSensors);
104  AddSensorRing(cRingCenter,
105  fRadius,
106  cRingStartAngleRadians,
107  fRange,
108  unNumSensors,
109  cBody.GetAnchor(strAnchorId));
110  }
111  else {
112  THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\"");
113  }
114  }
115  }
116  catch(CARGoSException& ex) {
117  THROW_ARGOSEXCEPTION_NESTED("Initialization error in light sensor equipped entity", ex);
118  }
119  }
120 
121  /****************************************/
122  /****************************************/
123 
125  CEntity::Enable();
126  for(size_t i = 0; i < m_tSensors.size(); ++i) {
127  m_tSensors[i]->Anchor.Enable();
128  }
129  }
130 
131  /****************************************/
132  /****************************************/
133 
136  for(size_t i = 0; i < m_tSensors.size(); ++i) {
137  m_tSensors[i]->Anchor.Disable();
138  }
139  }
140 
141  /****************************************/
142  /****************************************/
143 
145  const CVector3& c_direction,
146  Real f_range,
147  SAnchor& s_anchor) {
148  m_tSensors.push_back(new SSensor(c_position, c_direction, f_range, s_anchor));
149  }
150 
151  /****************************************/
152  /****************************************/
153 
155  Real f_radius,
156  const CRadians& c_start_angle,
157  Real f_range,
158  UInt32 un_num_sensors,
159  SAnchor& s_anchor) {
160  CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
161  CRadians cAngle;
162  CVector3 cPos, cDir;
163  for(UInt32 i = 0; i < un_num_sensors; ++i) {
164  cAngle = c_start_angle + i * cSensorSpacing;
165  cAngle.SignedNormalize();
166  cPos.Set(f_radius, 0.0f, 0.0f);
167  cPos.RotateZ(cAngle);
168  cPos += c_center;
169  cDir.Set(f_range, 0.0f, 0.0f);
170  cDir.RotateZ(cAngle);
171  AddSensor(cPos, cDir, f_range, s_anchor);
172  }
173  }
174 
175  /****************************************/
176  /****************************************/
177 
179 
180  /****************************************/
181  /****************************************/
182 
183 }
#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.
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
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.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CEntity)
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.
The basic entity type.
Definition: entity.h:90
void Disable()
Disables the entity.
Definition: entity.h:275
void Enable()
Enables the entity.
Definition: entity.h:265
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
An anchor related to the body of an entity.
Definition: physics_model.h:38
The exception that wraps all errors in ARGoS.
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
A 3D vector class.
Definition: vector3.h:31
CVector3 & Normalize()
Normalizes this vector.
Definition: vector3.h:237
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:155
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:287
void AddSensor(const CVector3 &c_position, const CVector3 &c_direction, Real f_range, SAnchor &s_anchor)
void AddSensorRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, Real f_range, UInt32 un_num_sensors, SAnchor &s_anchor)
CLightSensorEquippedEntity(CComposableEntity *pc_parent)
SSensor::TList m_tSensors
The list of sensors.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
SSensor(const CVector3 &c_position, const CVector3 &c_direction, Real f_range, SAnchor &s_anchor)