ARGoS  3
A parallel, multi-engine simulator for swarm robotics
eyebot_light_rotzonly_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 
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  static CRange<Real> SENSOR_RANGE(0.0f, 1.0f);
21  static CRadians SENSOR_SPACING = CRadians(ARGOS_PI / 12.0f);
22  static CRadians SENSOR_HALF_SPACING = SENSOR_SPACING * 0.5;
23 
24  /****************************************/
25  /****************************************/
26 
27  static SInt32 Modulo(SInt32 n_value, SInt32 un_modulo) {
28  while(n_value < 0) n_value += un_modulo;
29  while(n_value >= un_modulo) n_value -= un_modulo;
30  return n_value;
31  }
32 
33  static Real ComputeReading(Real f_distance) {
34  if(f_distance > 2.5f) {
35  return 0.0f;
36  }
37  else {
38  return ::exp(-f_distance * 2.0f);
39  }
40  }
41 
42  static Real ScaleReading(const CRadians& c_angular_distance) {
43  if(c_angular_distance > CRadians::PI_OVER_TWO) {
44  return 0.0f;
45  }
46  else {
47  return (1.0f - 2.0f * c_angular_distance / CRadians::PI);
48  }
49  }
50 
51  /****************************************/
52  /****************************************/
53 
55  m_pcEmbodiedEntity(NULL),
56  m_bShowRays(false),
57  m_pcRNG(NULL),
58  m_bAddNoise(false),
59  m_cSpace(CSimulator::GetInstance().GetSpace()) {}
60 
61  /****************************************/
62  /****************************************/
63 
65  try {
66  m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
67  m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
68  m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors"));
70  }
71  catch(CARGoSException& ex) {
72  THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the eye-bot light default sensor", ex);
73  }
74  }
75 
76  /****************************************/
77  /****************************************/
78 
80  try {
81  /* Show rays? */
82  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
83  /* Parse noise level */
84  Real fNoiseLevel = 0.0f;
85  GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
86  if(fNoiseLevel < 0.0f) {
87  THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor");
88  }
89  else if(fNoiseLevel > 0.0f) {
90  m_bAddNoise = true;
91  m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
92  m_pcRNG = CRandom::CreateRNG("argos");
93  }
95  }
96  catch(CARGoSException& ex) {
97  THROW_ARGOSEXCEPTION_NESTED("Initialization error in rot_z_only light sensor", ex);
98  }
99  }
100 
101  /****************************************/
102  /****************************************/
103 
105  /* Erase readings */
106  for(size_t i = 0; i < m_tReadings.size(); ++i) {
107  m_tReadings[i].Value = 0.0f;
108  }
109  /* Get eye-bot orientation */
110  CRadians cTmp1, cTmp2, cOrientationZ;
111  m_pcEmbodiedEntity->GetOriginAnchor().Orientation.ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
112  /* Ray used for scanning the environment for obstacles */
113  CRay3 cOcclusionCheckRay;
114  cOcclusionCheckRay.SetStart(m_pcEmbodiedEntity->GetOriginAnchor().Position);
115  CVector3 cRobotToLight;
116  /* Buffer for the angle of the light wrt to the eye-bot */
117  CRadians cAngleLightWrtEyebot;
118  /* Buffers to contain data about the intersection */
119  SEmbodiedEntityIntersectionItem sIntersection;
120  /* List of light entities */
121  CSpace::TMapPerType& mapLights = m_cSpace.GetEntitiesByType("light");
122  /*
123  * 1. go through the list of light entities in the scene
124  * 2. check if a light is occluded
125  * 3. if it isn't, distribute the reading across the sensors
126  * NOTE: the readings are additive
127  * 4. go through the sensors and clamp their values
128  */
129  for(CSpace::TMapPerType::iterator it = mapLights.begin();
130  it != mapLights.end();
131  ++it) {
132  /* Get a reference to the light */
133  CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second));
134  /* Consider the light only if it has non zero intensity */
135  if(cLight.GetIntensity() > 0.0f) {
136  /* Set the ray end */
137  cOcclusionCheckRay.SetEnd(cLight.GetPosition());
138  /* Check occlusion between the eye-bot and the light */
139  if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
140  cOcclusionCheckRay,
141  *m_pcEmbodiedEntity)) {
142  /* The light is not occluded */
143  if(m_bShowRays) {
144  m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay);
145  }
146  /* Get the distance between the light and the eye-bot */
147  cOcclusionCheckRay.ToVector(cRobotToLight);
148  /*
149  * Linearly scale the distance with the light intensity
150  * The greater the intensity, the smaller the distance
151  */
152  cRobotToLight /= cLight.GetIntensity();
153  /* Get the angle wrt to eye-bot rotation */
154  cAngleLightWrtEyebot = cRobotToLight.GetZAngle();
155  cAngleLightWrtEyebot -= cOrientationZ;
156  /*
157  * Find closest sensor index to point at which ray hits eyebot body
158  * Rotate whole body by half a sensor spacing (corresponding to placement of first sensor)
159  * Division says how many sensor spacings there are between first sensor and point at which ray hits eyebot body
160  * Increase magnitude of result of division to ensure correct rounding
161  */
162  Real fIdx = (cAngleLightWrtEyebot - SENSOR_HALF_SPACING) / SENSOR_SPACING;
163  SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f;
164  /* Set the actual readings */
165  Real fReading = cRobotToLight.Length();
166  /*
167  * Take 6 readings before closest sensor and 6 readings after - thus we
168  * process sensors that are with 180 degrees of intersection of light
169  * ray with robot body
170  */
171  for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) {
172  UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 24);
173  CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtEyebot - m_tReadings[unIdx].Angle).SignedNormalize());
174  /*
175  * ComputeReading gives value as if sensor was perfectly in line with
176  * light ray. We then linearly decrease actual reading from 1 (dist
177  * 0) to 0 (dist PI/2)
178  */
179  m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint);
180  }
181  }
182  else {
183  /* The ray is occluded */
184  if(m_bShowRays) {
185  m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay);
186  m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersection.TOnRay);
187  }
188  }
189  }
190  }
191  /* Apply noise to the sensors */
192  if(m_bAddNoise) {
193  for(size_t i = 0; i < 24; ++i) {
195  }
196  }
197  /* Trunc the reading between 0 and 1 */
198  for(size_t i = 0; i < 24; ++i) {
199  SENSOR_RANGE.TruncValue(m_tReadings[i].Value);
200  }
201  }
202 
203  /****************************************/
204  /****************************************/
205 
207  for(UInt32 i = 0; i < GetReadings().size(); ++i) {
208  m_tReadings[i].Value = 0.0f;
209  }
210  }
211 
212  /****************************************/
213  /****************************************/
214 
216  "eyebot_light", "rot_z_only",
217  "Carlo Pinciroli [ilpincy@gmail.com]",
218  "1.0",
219  "The eye-bot light sensor (optimized for 2D).",
220  "This sensor accesses a set of light sensors. The sensors all return a value\n"
221  "between 0 and 1, where 0 means nothing within range and 1 means the perceived\n"
222  "light saturates the sensor. Values between 0 and 1 depend on the distance of\n"
223  "the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n"
224  "distance between a sensor and the light, and I is the reference intensity of the\n"
225  "perceived light. The reference intensity corresponds to the minimum distance at\n"
226  "which the light saturates a sensor. The reference intensity depends on the\n"
227  "individual light, and it is set with the \"intensity\" attribute of the light\n"
228  "entity. In case multiple lights are present in the environment, each sensor\n"
229  "reading is calculated as the sum of the individual readings due to each light.\n"
230  "In other words, light wave interference is not taken into account. In\n"
231  "controllers, you must include the ci_light_sensor.h header.\n\n"
232  "REQUIRED XML CONFIGURATION\n\n"
233  " <controllers>\n"
234  " ...\n"
235  " <my_controller ...>\n"
236  " ...\n"
237  " <sensors>\n"
238  " ...\n"
239  " <eyebot_light implementation=\"rot_z_only\" />\n"
240  " ...\n"
241  " </sensors>\n"
242  " ...\n"
243  " </my_controller>\n"
244  " ...\n"
245  " </controllers>\n\n"
246  "OPTIONAL XML CONFIGURATION\n\n"
247  "It is possible to draw the rays shot by the light sensor in the OpenGL\n"
248  "visualization. This can be useful for sensor debugging but also to understand\n"
249  "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
250  "they are not obstructed and in purple when they are. In case a ray is\n"
251  "obstructed, a black dot is drawn where the intersection occurred.\n"
252  "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
253  "example:\n\n"
254  " <controllers>\n"
255  " ...\n"
256  " <my_controller ...>\n"
257  " ...\n"
258  " <sensors>\n"
259  " ...\n"
260  " <eyebot_light implementation=\"rot_z_only\"\n"
261  " show_rays=\"true\" />\n"
262  " ...\n"
263  " </sensors>\n"
264  " ...\n"
265  " </my_controller>\n"
266  " ...\n"
267  " </controllers>\n\n"
268  "It is possible to add uniform noise to the sensors, thus matching the\n"
269  "characteristics of a real robot better. This can be done with the attribute\n"
270  "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
271  "reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
272  " <controllers>\n"
273  " ...\n"
274  " <my_controller ...>\n"
275  " ...\n"
276  " <sensors>\n"
277  " ...\n"
278  " <eyebot_light implementation=\"rot_z_only\"\n"
279  " noise_level=\"0.1\" />\n"
280  " ...\n"
281  " </sensors>\n"
282  " ...\n"
283  " </my_controller>\n"
284  " ...\n"
285  " </controllers>\n\n"
286  "OPTIONAL XML CONFIGURATION\n\n"
287  "None.\n",
288  "Usable"
289  );
290 
291 }
ARGOS_PI
#define ARGOS_PI
To be used when initializing static variables.
Definition: angles.h:32
argos::CEmbodiedEntity::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition: embodied_entity.h:119
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::CSpace::GetEntitiesByType
TMapPerType & GetEntitiesByType(const std::string &str_type)
Returns a map containing all the objects of a given type.
Definition: space.cpp:108
argos::SEmbodiedEntityIntersectionItem::TOnRay
Real TOnRay
Definition: physics_engine.h:34
eyebot_light_rotzonly_sensor.h
argos::CEyeBotLightRotZOnlySensor::m_pcRNG
CRandom::CRNG * m_pcRNG
Random number generator.
Definition: eyebot_light_rotzonly_sensor.h:58
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::CEyeBotLightRotZOnlySensor
Definition: eyebot_light_rotzonly_sensor.h:26
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::CLightSensorEquippedEntity
Definition: light_sensor_equipped_entity.h:21
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CRadians::PI_OVER_TWO
static const CRadians PI_OVER_TWO
Set to PI / 2.
Definition: angles.h:59
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::CCI_EyeBotLightSensor::m_tReadings
TReadings m_tReadings
Definition: ci_eyebot_light_sensor.h:86
argos::SEmbodiedEntityIntersectionItem
Definition: physics_engine.h:32
argos::CSimulator
The core class of ARGOS.
Definition: simulator.h:62
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
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::CEyeBotLightRotZOnlySensor::m_cSpace
CSpace & m_cSpace
Reference to the space.
Definition: eyebot_light_rotzonly_sensor.h:67
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::CEyeBotLightRotZOnlySensor::m_pcEmbodiedEntity
CEmbodiedEntity * m_pcEmbodiedEntity
Reference to embodied entity associated to this sensor.
Definition: eyebot_light_rotzonly_sensor.h:46
argos::CRadians::PI
static const CRadians PI
The PI constant.
Definition: angles.h:49
argos::CEyeBotLightRotZOnlySensor::Reset
virtual void Reset()
Resets the sensor to the state it had just after Init().
Definition: eyebot_light_rotzonly_sensor.cpp:206
argos::CEyeBotLightRotZOnlySensor::m_pcControllableEntity
CControllableEntity * m_pcControllableEntity
Reference to controllable entity associated to this sensor.
Definition: eyebot_light_rotzonly_sensor.h:52
argos::Abs
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
argos::CEyeBotLightRotZOnlySensor::m_bAddNoise
bool m_bAddNoise
Whether to add noise or not.
Definition: eyebot_light_rotzonly_sensor.h:61
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::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:171
argos::CEyeBotLightRotZOnlySensor::m_pcLightEntity
CLightSensorEquippedEntity * m_pcLightEntity
Reference to light sensor equipped entity associated to this sensor.
Definition: eyebot_light_rotzonly_sensor.h:49
argos::CEyeBotLightRotZOnlySensor::Update
virtual void Update()
Updates the state of the entity associated to this sensor.
Definition: eyebot_light_rotzonly_sensor.cpp:104
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::CEyeBotLightRotZOnlySensor::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
Definition: eyebot_light_rotzonly_sensor.cpp:79
argos::CVector3::GetZAngle
CRadians GetZAngle() const
Returns the angle between this vector and the z axis.
Definition: vector3.h:339
SInt32
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
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
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::CRay3::SetStart
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
argos::CCI_EyeBotLightSensor::GetReadings
const TReadings & GetReadings() const
Returns the readings of this sensor.
Definition: ci_eyebot_light_sensor.cpp:35
argos::CLightSensorEquippedEntity::Enable
virtual void Enable()
Definition: light_sensor_equipped_entity.cpp:124
argos::CLightEntity
Definition: light_entity.h:20
argos::CRandom::CreateRNG
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
argos::CEyeBotLightRotZOnlySensor::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
Definition: eyebot_light_rotzonly_sensor.cpp:64
argos::CRay3::SetEnd
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CEyeBotLightRotZOnlySensor::m_bShowRays
bool m_bShowRays
Flag to show rays in the simulator.
Definition: eyebot_light_rotzonly_sensor.h:55
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CEyeBotLightRotZOnlySensor::CEyeBotLightRotZOnlySensor
CEyeBotLightRotZOnlySensor()
Definition: eyebot_light_rotzonly_sensor.cpp:54
argos::CEyeBotLightRotZOnlySensor::m_cNoiseRange
CRange< Real > m_cNoiseRange
Noise range.
Definition: eyebot_light_rotzonly_sensor.h:64