ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
Go to the documentation of this file.
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>
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;
28 while(n_value < 0) n_value += un_modulo;
29 while(n_value >= un_modulo) n_value -= un_modulo;
33 static Real ComputeReading(
Real f_distance) {
34 if(f_distance > 2.5f) {
38 return ::exp(-f_distance * 2.0f);
42 static Real ScaleReading(
const CRadians& c_angular_distance) {
47 return (1.0f - 2.0f * c_angular_distance /
CRadians::PI);
55 m_pcEmbodiedEntity(NULL),
59 m_cSpace(
CSimulator::GetInstance().GetSpace()) {}
84 Real fNoiseLevel = 0.0f;
86 if(fNoiseLevel < 0.0f) {
89 else if(fNoiseLevel > 0.0f) {
110 CRadians cTmp1, cTmp2, cOrientationZ;
113 CRay3 cOcclusionCheckRay;
129 for(CSpace::TMapPerType::iterator it = mapLights.begin();
130 it != mapLights.end();
133 CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second));
147 cOcclusionCheckRay.
ToVector(cRobotToLight);
154 cAngleLightWrtEyebot = cRobotToLight.
GetZAngle();
155 cAngleLightWrtEyebot -= cOrientationZ;
162 Real fIdx = (cAngleLightWrtEyebot - SENSOR_HALF_SPACING) / SENSOR_SPACING;
163 SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f;
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());
179 m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint);
193 for(
size_t i = 0; i < 24; ++i) {
198 for(
size_t i = 0; i < 24; ++i) {
216 "eyebot_light",
"rot_z_only",
217 "Carlo Pinciroli [ilpincy@gmail.com]",
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"
235 " <my_controller ...>\n"
239 " <eyebot_light implementation=\"rot_z_only\" />\n"
243 " </my_controller>\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"
256 " <my_controller ...>\n"
260 " <eyebot_light implementation=\"rot_z_only\"\n"
261 " show_rays=\"true\" />\n"
265 " </my_controller>\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"
274 " <my_controller ...>\n"
278 " <eyebot_light implementation=\"rot_z_only\"\n"
279 " noise_level=\"0.1\" />\n"
283 " </my_controller>\n"
285 " </controllers>\n\n"
286 "OPTIONAL XML CONFIGURATION\n\n"
#define ARGOS_PI
To be used when initializing static variables.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
TMapPerType & GetEntitiesByType(const std::string &str_type)
Returns a map containing all the objects of a given type.
CRandom::CRNG * m_pcRNG
Random number generator.
void AddIntersectionPoint(const CRay3 &c_ray, Real f_t_on_ray)
Adds an intersection point to the list.
The namespace containing all the ARGoS related code.
It defines the basic type CRadians, used to store an angle value in radians.
Basic class for an entity that contains other entities.
The exception that wraps all errors in ARGoS.
static const CRadians PI_OVER_TWO
Set to PI / 2.
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.
size_t GetNumSensors() const
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
CSpace & m_cSpace
Reference to the space.
void AddCheckedRay(bool b_obstructed, const CRay3 &c_ray)
Adds a ray to the list of checked rays.
CEmbodiedEntity * m_pcEmbodiedEntity
Reference to embodied entity associated to this sensor.
static const CRadians PI
The PI constant.
virtual void Reset()
Resets the sensor to the state it had just after Init().
CControllableEntity * m_pcControllableEntity
Reference to controllable entity associated to this sensor.
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
bool m_bAddNoise
Whether to add noise or not.
const CVector3 & GetPosition() const
Real Length() const
Returns the length of this vector.
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
CLightSensorEquippedEntity * m_pcLightEntity
Reference to light sensor equipped entity associated to this sensor.
virtual void Update()
Updates the state of the entity associated to this sensor.
#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.
CVector3 & ToVector(CVector3 &c_buffer) const
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
CRadians GetZAngle() const
Returns the angle between this vector and the z axis.
signed int SInt32
32-bit signed integer.
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")
std::map< std::string, CAny, std::less< std::string > > TMapPerType
A map of entities indexed by type description.
void TruncValue(T &t_value) const
unsigned int UInt32
32-bit unsigned integer.
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.
void Set(const T &t_min, const T &t_max)
Real GetIntensity() const
void SetStart(const CVector3 &c_start)
const TReadings & GetReadings() const
Returns the readings of this sensor.
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
void SetEnd(const CVector3 &c_end)
An entity that contains a pointer to the user-defined controller.
bool m_bShowRays
Flag to show rays in the simulator.
CVector3 Position
The position of the anchor wrt the global coordinate system.
float Real
Collects all ARGoS code.
CEyeBotLightRotZOnlySensor()
CRange< Real > m_cNoiseRange
Noise range.