ARGoS  3
A parallel, multi-engine simulator for swarm robotics
light_default_sensor.cpp
Go to the documentation of this file.
1 
7 #include <argos3/core/simulator/simulator.h>
8 #include <argos3/core/simulator/entity/embodied_entity.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 #include <argos3/plugins/simulator/entities/light_entity.h>
11 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
12 
13 #include "light_default_sensor.h"
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  static CRange<Real> UNIT(0.0f, 1.0f);
21 
22  /****************************************/
23  /****************************************/
24 
26  m_bShowRays(false),
27  m_pcRNG(NULL),
28  m_bAddNoise(false),
29  m_cSpace(CSimulator::GetInstance().GetSpace()) {}
30 
31  /****************************************/
32  /****************************************/
33 
35  try {
36  m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
37  m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors"));
39  }
40  catch(CARGoSException& ex) {
41  THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the light default sensor", ex);
42  }
43  }
44 
45  /****************************************/
46  /****************************************/
47 
49  try {
50  CCI_LightSensor::Init(t_tree);
51  /* Show rays? */
52  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
53  /* Parse noise level */
54  Real fNoiseLevel = 0.0f;
55  GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
56  if(fNoiseLevel < 0.0f) {
57  THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor");
58  }
59  else if(fNoiseLevel > 0.0f) {
60  m_bAddNoise = true;
61  m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
62  m_pcRNG = CRandom::CreateRNG("argos");
63  }
65  }
66  catch(CARGoSException& ex) {
67  THROW_ARGOSEXCEPTION_NESTED("Initialization error in default light sensor", ex);
68  }
69  }
70 
71  /****************************************/
72  /****************************************/
73 
75  /* Erase readings */
76  for(size_t i = 0; i < m_tReadings.size(); ++i) m_tReadings[i] = 0.0f;
77  /* Ray used for scanning the environment for obstacles */
78  CRay3 cScanningRay;
79  CVector3 cRayStart;
80  CVector3 cSensorToLight;
81  /* Buffers to contain data about the intersection */
82  SEmbodiedEntityIntersectionItem sIntersection;
83  /* Get the map of light entities */
84  CSpace::TMapPerTypePerId::iterator itLights = m_cSpace.GetEntityMapPerTypePerId().find("light");
85  if (itLights != m_cSpace.GetEntityMapPerTypePerId().end()) {
86  CSpace::TMapPerType& mapLights = itLights->second;
87  /* Go through the sensors */
88  for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
89  /* Set ray start */
90  cRayStart = m_pcLightEntity->GetSensor(i).Position;
92  cRayStart += m_pcLightEntity->GetSensor(i).Anchor.Position;
93  /* Go through all the light entities */
94  for(CSpace::TMapPerType::iterator it = mapLights.begin();
95  it != mapLights.end();
96  ++it) {
97  /* Get a reference to the light */
98  CLightEntity& cLight = *any_cast<CLightEntity*>(it->second);
99  /* Consider the light only if it has non zero intensity */
100  if(cLight.GetIntensity() > 0.0f) {
101  /* Set ray end to light position */
102  cScanningRay.Set(cRayStart, cLight.GetPosition());
103  /* Check occlusions */
104  if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
105  cScanningRay)) {
106  /* No occlusion, the light is visibile */
107  if(m_bShowRays) {
108  m_pcControllableEntity->AddCheckedRay(false, cScanningRay);
109  }
110  /* Calculate reading */
111  cScanningRay.ToVector(cSensorToLight);
112  m_tReadings[i] += CalculateReading(cSensorToLight.Length(),
113  cLight.GetIntensity());
114  }
115  else {
116  /* There is an occlusion, the light is not visible */
117  if(m_bShowRays) {
119  sIntersection.TOnRay);
120  m_pcControllableEntity->AddCheckedRay(true, cScanningRay);
121  }
122  }
123  }
124  }
125  /* Apply noise to the sensor */
126  if(m_bAddNoise) {
128  }
129  /* Trunc the reading between 0 and 1 */
130  UNIT.TruncValue(m_tReadings[i]);
131  }
132  }
133  else {
134  /* There are no lights in the environment */
135  if(m_bAddNoise) {
136  /* Go through the sensors */
137  for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
138  /* Apply noise to the sensor */
140  /* Trunc the reading between 0 and 1 */
141  UNIT.TruncValue(m_tReadings[i]);
142  }
143  }
144  }
145  }
146 
147  /****************************************/
148  /****************************************/
149 
151  for(UInt32 i = 0; i < GetReadings().size(); ++i) {
152  m_tReadings[i] = 0.0f;
153  }
154  }
155 
156  /****************************************/
157  /****************************************/
158 
160  return (f_intensity * f_intensity) / (f_distance * f_distance);
161  }
162 
163  /****************************************/
164  /****************************************/
165 
167  "light", "default",
168  "Carlo Pinciroli [ilpincy@gmail.com]",
169  "1.0",
170  "A generic light sensor.",
171  "This sensor accesses a set of light sensors. The sensors all return a value\n"
172  "between 0 and 1, where 0 means nothing within range and 1 means the perceived\n"
173  "light saturates the sensor. Values between 0 and 1 depend on the distance of\n"
174  "the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n"
175  "distance between a sensor and the light, and I is the reference intensity of the\n"
176  "perceived light. The reference intensity corresponds to the minimum distance at\n"
177  "which the light saturates a sensor. The reference intensity depends on the\n"
178  "individual light, and it is set with the \"intensity\" attribute of the light\n"
179  "entity. In case multiple lights are present in the environment, each sensor\n"
180  "reading is calculated as the sum of the individual readings due to each light.\n"
181  "In other words, light wave interference is not taken into account. In\n"
182  "controllers, you must include the ci_light_sensor.h header.\n\n"
183  "REQUIRED XML CONFIGURATION\n\n"
184  " <controllers>\n"
185  " ...\n"
186  " <my_controller ...>\n"
187  " ...\n"
188  " <sensors>\n"
189  " ...\n"
190  " <light implementation=\"default\" />\n"
191  " ...\n"
192  " </sensors>\n"
193  " ...\n"
194  " </my_controller>\n"
195  " ...\n"
196  " </controllers>\n\n"
197  "OPTIONAL XML CONFIGURATION\n\n"
198  "It is possible to draw the rays shot by the light sensor in the OpenGL\n"
199  "visualization. This can be useful for sensor debugging but also to understand\n"
200  "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
201  "they are not obstructed and in purple when they are. In case a ray is\n"
202  "obstructed, a black dot is drawn where the intersection occurred.\n"
203  "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
204  "example:\n\n"
205  " <controllers>\n"
206  " ...\n"
207  " <my_controller ...>\n"
208  " ...\n"
209  " <sensors>\n"
210  " ...\n"
211  " <light implementation=\"default\"\n"
212  " show_rays=\"true\" />\n"
213  " ...\n"
214  " </sensors>\n"
215  " ...\n"
216  " </my_controller>\n"
217  " ...\n"
218  " </controllers>\n\n"
219  "It is possible to add uniform noise to the sensors, thus matching the\n"
220  "characteristics of a real robot better. This can be done with the attribute\n"
221  "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
222  "reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
223  " <controllers>\n"
224  " ...\n"
225  " <my_controller ...>\n"
226  " ...\n"
227  " <sensors>\n"
228  " ...\n"
229  " <light implementation=\"default\"\n"
230  " noise_level=\"0.1\" />\n"
231  " ...\n"
232  " </sensors>\n"
233  " ...\n"
234  " </my_controller>\n"
235  " ...\n"
236  " </controllers>\n\n"
237  "OPTIONAL XML CONFIGURATION\n\n"
238  "None.\n",
239  "Usable"
240  );
241 
242 }
argos::CCI_LightSensor::m_tReadings
std::vector< Real > m_tReadings
Definition: ci_light_sensor.h:33
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::CRandom::CRNG::Uniform
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
Definition: rng.cpp:87
argos::SEmbodiedEntityIntersectionItem::TOnRay
Real TOnRay
Definition: physics_engine.h:34
argos::CLightDefaultSensor::m_cSpace
CSpace & m_cSpace
Reference to the space.
Definition: light_default_sensor.h:75
argos::CControllableEntity::AddIntersectionPoint
void AddIntersectionPoint(const CRay3 &c_ray, Real f_t_on_ray)
Adds an intersection point to the list.
Definition: controllable_entity.h:191
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::CLightDefaultSensor::m_pcLightEntity
CLightSensorEquippedEntity * m_pcLightEntity
Reference to light sensor equipped entity associated to this sensor.
Definition: light_default_sensor.h:57
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CLightDefaultSensor::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
Definition: light_default_sensor.cpp:48
argos::CLightSensorEquippedEntity
Definition: light_sensor_equipped_entity.h:21
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::CRay3
Definition: ray3.h:19
argos::SEmbodiedEntityIntersectionItem
Definition: physics_engine.h:32
light_default_sensor.h
argos::CSimulator
The core class of ARGOS.
Definition: simulator.h:62
argos::CLightSensorEquippedEntity::GetNumSensors
size_t GetNumSensors() const
Definition: light_sensor_equipped_entity.h:59
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CControllableEntity::AddCheckedRay
void AddCheckedRay(bool b_obstructed, const CRay3 &c_ray)
Adds a ray to the list of checked rays.
Definition: controllable_entity.h:178
argos::CLightDefaultSensor::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
Definition: light_default_sensor.cpp:34
argos::CPositionalEntity::GetPosition
const CVector3 & GetPosition() const
Definition: positional_entity.h:36
argos::CVector3::Length
Real Length() const
Returns the length of this vector.
Definition: vector3.h:205
argos::GetClosestEmbodiedEntityIntersectedByRay
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
Definition: physics_engine.cpp:41
argos::CLightDefaultSensor::Update
virtual void Update()
Updates the state of the entity associated to this sensor.
Definition: light_default_sensor.cpp:74
argos::CLightDefaultSensor::CLightDefaultSensor
CLightDefaultSensor()
Definition: light_default_sensor.cpp:25
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
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::CRay3::ToVector
CVector3 & ToVector(CVector3 &c_buffer) const
Definition: ray3.h:100
argos::CLightDefaultSensor::m_pcRNG
CRandom::CRNG * m_pcRNG
Random number generator.
Definition: light_default_sensor.h:66
argos::CLightSensorEquippedEntity::SSensor::Anchor
SAnchor & Anchor
Definition: light_sensor_equipped_entity.h:32
argos::REGISTER_SENSOR
REGISTER_SENSOR(CEyeBotLightRotZOnlySensor, "eyebot_light", "rot_z_only", "Carlo Pinciroli [ilpincy@gmail.com]", "1.0", "The eye-bot light sensor (optimized for 2D).", "This sensor accesses a set of light sensors. The sensors all return a value\n" "between 0 and 1, where 0 means nothing within range and 1 means the perceived\n" "light saturates the sensor. Values between 0 and 1 depend on the distance of\n" "the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n" "distance between a sensor and the light, and I is the reference intensity of the\n" "perceived light. The reference intensity corresponds to the minimum distance at\n" "which the light saturates a sensor. The reference intensity depends on the\n" "individual light, and it is set with the \"intensity\" attribute of the light\n" "entity. In case multiple lights are present in the environment, each sensor\n" "reading is calculated as the sum of the individual readings due to each light.\n" "In other words, light wave interference is not taken into account. In\n" "controllers, you must include the ci_light_sensor.h header.\n\n" "REQUIRED XML CONFIGURATION\n\n" " <controllers>\n" " ...\n" " <my_controller ...>\n" " ...\n" " <sensors>\n" " ...\n" " <eyebot_light implementation=\"rot_z_only\" />\n" " ...\n" " </sensors>\n" " ...\n" " </my_controller>\n" " ...\n" " </controllers>\n\n" "OPTIONAL XML CONFIGURATION\n\n" "It is possible to draw the rays shot by the light sensor in the OpenGL\n" "visualization. This can be useful for sensor debugging but also to understand\n" "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n" "they are not obstructed and in purple when they are. In case a ray is\n" "obstructed, a black dot is drawn where the intersection occurred.\n" "To turn this functionality on, add the attribute \"show_rays\" as in this\n" "example:\n\n" " <controllers>\n" " ...\n" " <my_controller ...>\n" " ...\n" " <sensors>\n" " ...\n" " <eyebot_light implementation=\"rot_z_only\"\n" " show_rays=\"true\" />\n" " ...\n" " </sensors>\n" " ...\n" " </my_controller>\n" " ...\n" " </controllers>\n\n" "It is possible to add uniform noise to the sensors, thus matching the\n" "characteristics of a real robot better. This can be done with the attribute\n" "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n" "reading. The final sensor reading is always normalized in the [0-1] range.\n\n" " <controllers>\n" " ...\n" " <my_controller ...>\n" " ...\n" " <sensors>\n" " ...\n" " <eyebot_light implementation=\"rot_z_only\"\n" " noise_level=\"0.1\" />\n" " ...\n" " </sensors>\n" " ...\n" " </my_controller>\n" " ...\n" " </controllers>\n\n" "OPTIONAL XML CONFIGURATION\n\n" "None.\n", "Usable")
argos::CSpace::TMapPerType
std::map< std::string, CAny, std::less< std::string > > TMapPerType
A map of entities indexed by type description.
Definition: space.h:54
argos::CRange::TruncValue
void TruncValue(T &t_value) const
Definition: range.h:97
argos::CLightDefaultSensor::CalculateReading
virtual Real CalculateReading(Real f_distance, Real f_intensity)
Calculates the light reading resulting from a light source at the given distance.
Definition: light_default_sensor.cpp:159
argos::CLightDefaultSensor::m_cNoiseRange
CRange< Real > m_cNoiseRange
Noise range.
Definition: light_default_sensor.h:72
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos::GetNodeAttributeOrDefault
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.
Definition: argos_configuration.h:318
argos::CRange::Set
void Set(const T &t_min, const T &t_max)
Definition: range.h:68
argos::CLightEntity::GetIntensity
Real GetIntensity() const
Definition: light_entity.h:37
argos::CLightSensorEquippedEntity::Enable
virtual void Enable()
Definition: light_sensor_equipped_entity.cpp:124
argos::CLightDefaultSensor
Definition: light_default_sensor.h:26
argos::CLightEntity
Definition: light_entity.h:20
argos::CLightSensorEquippedEntity::GetSensor
SSensor & GetSensor(size_t un_idx)
Definition: light_sensor_equipped_entity.h:63
argos::CRay3::Set
void Set(const CVector3 &c_start, const CVector3 &c_end)
Definition: ray3.h:67
argos::CRandom::CreateRNG
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
argos::CVector3::Rotate
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
argos::CSpace::GetEntityMapPerTypePerId
TMapPerTypePerId & GetEntityMapPerTypePerId()
Returns a nested map of entities, ordered by type and by id.
Definition: space.h:192
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CLightDefaultSensor::Reset
virtual void Reset()
Resets the sensor to the state it had just after Init().
Definition: light_default_sensor.cpp:150
argos::CCI_Sensor::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::CCI_LightSensor::GetReadings
const std::vector< Real > & GetReadings() const
Definition: ci_light_sensor.cpp:18
argos::CLightSensorEquippedEntity::SSensor::Position
CVector3 Position
Definition: light_sensor_equipped_entity.h:30
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CLightDefaultSensor::m_bShowRays
bool m_bShowRays
Flag to show rays in the simulator.
Definition: light_default_sensor.h:63
argos::CLightDefaultSensor::m_pcControllableEntity
CControllableEntity * m_pcControllableEntity
Reference to controllable entity associated to this sensor.
Definition: light_default_sensor.h:60
argos::CLightDefaultSensor::m_bAddNoise
bool m_bAddNoise
Whether to add noise or not.
Definition: light_default_sensor.h:69