proximity_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/proximity_sensor_equipped_entity.h>
11 
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
19  static CRange<Real> UNIT(0.0f, 1.0f);
20 
21  /****************************************/
22  /****************************************/
23 
25  m_pcEmbodiedEntity(NULL),
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_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
37  m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
38  m_pcProximityEntity = &(c_entity.GetComponent<CProximitySensorEquippedEntity>("proximity_sensors"));
40  }
41  catch(CARGoSException& ex) {
42  THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the proximity default sensor", ex);
43  }
44  }
45 
46  /****************************************/
47  /****************************************/
48 
50  try {
52  /* Show rays? */
53  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
54  /* Parse noise level */
55  Real fNoiseLevel = 0.0f;
56  GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
57  if(fNoiseLevel < 0.0f) {
58  THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the proximity sensor");
59  }
60  else if(fNoiseLevel > 0.0f) {
61  m_bAddNoise = true;
62  m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
63  m_pcRNG = CRandom::CreateRNG("argos");
64  }
66  }
67  catch(CARGoSException& ex) {
68  THROW_ARGOSEXCEPTION_NESTED("Initialization error in default proximity sensor", ex);
69  }
70  }
71 
72  /****************************************/
73  /****************************************/
74 
76  /* Ray used for scanning the environment for obstacles */
77  CRay3 cScanningRay;
78  CVector3 cRayStart, cRayEnd;
79  /* Buffers to contain data about the intersection */
80  SEmbodiedEntityIntersectionItem sIntersection;
81  /* Go through the sensors */
82  for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
83  /* Compute ray for sensor i */
84  cRayStart = m_pcProximityEntity->GetSensor(i).Offset;
87  cRayEnd = m_pcProximityEntity->GetSensor(i).Offset;
91  cScanningRay.Set(cRayStart,cRayEnd);
92  /* Compute reading */
93  /* Get the closest intersection */
95  cScanningRay,
97  /* There is an intersection */
98  if(m_bShowRays) {
100  sIntersection.TOnRay);
101  m_pcControllableEntity->AddCheckedRay(true, cScanningRay);
102  }
103  m_tReadings[i] = CalculateReading(cScanningRay.GetDistance(sIntersection.TOnRay));
104  }
105  else {
106  /* No intersection */
107  m_tReadings[i] = 0.0f;
108  if(m_bShowRays) {
109  m_pcControllableEntity->AddCheckedRay(false, cScanningRay);
110  }
111  }
112  /* Apply noise to the sensor */
113  if(m_bAddNoise) {
115  }
116  /* Trunc the reading between 0 and 1 */
117  UNIT.TruncValue(m_tReadings[i]);
118  }
119  }
120 
121  /****************************************/
122  /****************************************/
123 
125  for(UInt32 i = 0; i < GetReadings().size(); ++i) {
126  m_tReadings[i] = 0.0f;
127  }
128  }
129 
130  /****************************************/
131  /****************************************/
132 
134  return Exp(-f_distance);
135  }
136 
137  /****************************************/
138  /****************************************/
139 
141  "proximity", "default",
142  "Carlo Pinciroli [ilpincy@gmail.com]",
143  "1.0",
144  "A generic proximity sensor.",
145  "This sensor accesses a set of proximity sensors. The sensors all return a value\n"
146  "between 0 and 1, where 0 means nothing within range and 1 means an external\n"
147  "object is touching the sensor. Values between 0 and 1 depend on the distance of\n"
148  "the occluding object, and are calculated as value=exp(-distance). In\n"
149  "controllers, you must include the ci_proximity_sensor.h header.\n\n"
150  "REQUIRED XML CONFIGURATION\n\n"
151  " <controllers>\n"
152  " ...\n"
153  " <my_controller ...>\n"
154  " ...\n"
155  " <sensors>\n"
156  " ...\n"
157  " <proximity implementation=\"default\" />\n"
158  " ...\n"
159  " </sensors>\n"
160  " ...\n"
161  " </my_controller>\n"
162  " ...\n"
163  " </controllers>\n\n"
164  "OPTIONAL XML CONFIGURATION\n\n"
165  "It is possible to draw the rays shot by the proximity sensor in the OpenGL\n"
166  "visualization. This can be useful for sensor debugging but also to understand\n"
167  "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
168  "they are not obstructed and in purple when they are. In case a ray is\n"
169  "obstructed, a black dot is drawn where the intersection occurred.\n"
170  "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
171  "example:\n\n"
172  " <controllers>\n"
173  " ...\n"
174  " <my_controller ...>\n"
175  " ...\n"
176  " <sensors>\n"
177  " ...\n"
178  " <proximity implementation=\"default\"\n"
179  " show_rays=\"true\" />\n"
180  " ...\n"
181  " </sensors>\n"
182  " ...\n"
183  " </my_controller>\n"
184  " ...\n"
185  " </controllers>\n\n"
186  "It is possible to add uniform noise to the sensors, thus matching the\n"
187  "characteristics of a real robot better. This can be done with the attribute\n"
188  "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
189  "reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
190  " <controllers>\n"
191  " ...\n"
192  " <my_controller ...>\n"
193  " ...\n"
194  " <sensors>\n"
195  " ...\n"
196  " <proximity implementation=\"default\"\n"
197  " noise_level=\"0.1\" />\n"
198  " ...\n"
199  " </sensors>\n"
200  " ...\n"
201  " </my_controller>\n"
202  " ...\n"
203  " </controllers>\n\n",
204  "Usable"
205  );
206 
207 }
CControllableEntity * m_pcControllableEntity
Reference to controllable entity associated to this sensor.
Real GetDistance(Real f_t) const
Definition: ray3.h:117
An entity that contains a pointer to the user-defined controller.
A 3D vector class.
Definition: vector3.h:29
void Set(const CVector3 &c_start, const CVector3 &c_end)
Definition: ray3.h:67
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
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.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CProximitySensorEquippedEntity * m_pcProximityEntity
Reference to proximity sensor equipped entity associated to this sensor.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
virtual Real CalculateReading(Real f_distance)
Calculates the proximity reading when the closest occluding object is located as the given distance...
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
void TruncValue(T &t_value) const
Definition: range.h:97
void Set(const T &t_min, const T &t_max)
Definition: range.h:68
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
CRange< Real > m_cNoiseRange
Noise range.
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...
virtual void Reset()
Resets the sensor to the state it had just after Init().
bool m_bShowRays
Flag to show rays in the simulator.
const std::vector< Real > & GetReadings() const
#define Exp
Definition: general.h:65
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
Definition: rng.cpp:87
Basic class for an entity that contains other entities.
bool m_bAddNoise
Whether to add noise or not.
void AddIntersectionPoint(const CRay3 &c_ray, Real f_t_on_ray)
Adds an intersection point to the list.
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
The exception that wraps all errors in ARGoS.
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
CRandom::CRNG * m_pcRNG
Random number generator.
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
void AddCheckedRay(bool b_obstructed, const CRay3 &c_ray)
Adds a ray to the list of checked rays.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
virtual void Update()
Updates the state of the entity associated to this sensor.
std::vector< Real > m_tReadings
The core class of ARGOS.
Definition: simulator.h:62
CEmbodiedEntity * m_pcEmbodiedEntity
Reference to embodied entity associated to this sensor.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
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")