ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
Go to the documentation of this file.
2 #include <argos3/core/simulator/simulator.h>
3 #include <argos3/core/simulator/space/positional_indices/positional_index.h>
4 #include <argos3/core/simulator/entity/composable_entity.h>
5 #include <argos3/core/simulator/entity/embodied_entity.h>
6 #include <argos3/plugins/simulator/entities/led_entity.h>
7 #include <argos3/plugins/simulator/entities/omnidirectional_camera_equipped_entity.h>
8 #include <argos3/plugins/simulator/media/led_medium.h>
25 Real f_noise_std_dev) :
27 m_cOmnicamEntity(c_omnicam_entity),
28 m_cEmbodiedEntity(c_embodied_entity),
29 m_cControllableEntity(c_controllable_entity),
30 m_bShowRays(b_show_rays),
31 m_fDistanceNoiseStdDev(f_noise_std_dev),
33 m_pcRootSensingEntity = &m_cEmbodiedEntity.
GetParent();
34 if(m_fDistanceNoiseStdDev > 0.0f) {
39 while(! m_tBlobs.empty()) {
40 delete m_tBlobs.back();
51 while(m_pcRootOfLEDEntity->
HasParent()) m_pcRootOfLEDEntity = &m_pcRootOfLEDEntity->
GetParent();
52 if(m_pcRootSensingEntity == m_pcRootOfLEDEntity) {
59 m_cLEDRelativePos -= m_cCameraPos;
60 m_cLEDRelativePosXY.
Set(m_cLEDRelativePos.
GetX(),
61 m_cLEDRelativePos.
GetY());
62 if(
Abs(m_cLEDRelativePos.
GetX()) < m_fGroundHalfRange &&
63 Abs(m_cLEDRelativePos.
GetY()) < m_fGroundHalfRange &&
64 m_cLEDRelativePos.
GetZ() < m_cCameraPos.
GetZ() &&
69 if(m_fDistanceNoiseStdDev > 0.0f) {
71 m_cLEDRelativePosXY.
Length() * m_pcRNG->
Gaussian(m_fDistanceNoiseStdDev),
77 m_cLEDRelativePosXY.
Length() * 100.0f));
87 while(! m_tBlobs.empty()) {
88 delete m_tBlobs.back();
91 m_fGroundHalfRange = f_ground_half_range;
94 m_cCameraPos += m_cOmnicamEntity.
GetOffset();
95 m_cOcclusionCheckRay.
SetStart(m_cCameraPos);
104 Real m_fGroundHalfRange;
106 CEntity* m_pcRootSensingEntity;
114 CRay3 m_cOcclusionCheckRay;
115 Real m_fDistanceNoiseStdDev;
124 m_pcOmnicamEntity(NULL),
125 m_pcControllableEntity(NULL),
126 m_pcEmbodiedEntity(NULL),
128 m_pcEmbodiedIndex(NULL),
160 Real fDistanceNoiseStdDev = 0;
163 std::string strMedium;
173 fDistanceNoiseStdDev);
197 cCameraPos.
GetZ() * 0.5f),
198 CVector3(fGroundHalfRange, fGroundHalfRange, cCameraPos.
GetZ() * 0.5f),
238 "colored_blob_omnidirectional_camera",
"rot_z_only",
239 "Carlo Pinciroli [ilpincy@gmail.com]",
241 "A generic omnidirectional camera sensor to detect colored blobs.",
242 "This sensor accesses an omnidirectional camera that detects colored blobs. The\n"
243 "sensor returns a list of blobs, each defined by a color and a position with\n"
244 "respect to the robot reference point on the ground. In controllers, you must\n"
245 "include the ci_colored_blob_omnidirectional_camera_sensor.h header.\n\n"
246 "REQUIRED XML CONFIGURATION\n\n"
249 " <my_controller ...>\n"
253 " <colored_blob_omnidirectional_camera implementation=\"rot_z_only\"\n"
254 " medium=\"leds\" />\n"
258 " </my_controller>\n"
260 " </controllers>\n\n"
261 "The 'medium' attribute must be set to the id of the leds medium declared in the\n"
262 "<media> section.\n\n"
263 "OPTIONAL XML CONFIGURATION\n\n"
264 "It is possible to draw the rays shot by the camera sensor in the OpenGL\n"
265 "visualization. This can be useful for sensor debugging but also to understand\n"
266 "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
267 "they are not obstructed and in purple when they are. In case a ray is\n"
268 "obstructed, a black dot is drawn where the intersection occurred.\n"
269 "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
273 " <my_controller ...>\n"
277 " <colored_blob_omnidirectional_camera implementation=\"rot_z_only\"\n"
278 " medium=\"leds\" />\n"
279 " show_rays=\"true\" />\n"
283 " </my_controller>\n"
285 " </controllers>\n\n"
286 "It is possible to add uniform noise to the blobs, thus matching the\n"
287 "characteristics of a real robot better. This can be done with the attribute\n"
288 "\"noise_std_dev\".\n\n"
291 " <my_controller ...>\n"
295 " <colored_blob_omnidirectional_camera implementation=\"rot_z_only\"\n"
296 " medium=\"leds\" />\n"
297 " noise_std_dev=\"0.1\" />\n"
301 " </my_controller>\n"
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
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.
Real GetZ() const
Returns the z coordinate of this vector.
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
const CColor & GetColor() const
Returns the current color of the LED.
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
virtual void Update()
Updates the state of the entity associated to this sensor.
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.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
COmnidirectionalCameraLEDCheckOperation * m_pcOperation
virtual void Disable()
Disables image acquisition and processing.
bool HasParent() const
Returns true if this entity has a parent.
void Enable()
Enables the entity.
This entity is a link to a body in the physics engine.
CRadians Angle() const
Returns the angle of this vector.
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual ~COmnidirectionalCameraLEDCheckOperation()
void AddCheckedRay(bool b_obstructed, const CRay3 &c_ray)
Adds a ray to the list of checked rays.
CPositionalIndex< CLEDEntity > * m_pcLEDIndex
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Real GetX() const
Returns the x coordinate of this vector.
COmnidirectionalCameraEquippedEntity * m_pcOmnicamEntity
const CVector3 & GetPosition() const
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
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
std::vector< SBlob * > TBlobList
Vector of pointers to colored blobs.
virtual bool operator()(CLEDEntity &c_led)
CEmbodiedEntity * m_pcEmbodiedEntity
virtual void Enable()
Enables image acquisition and processing.
virtual void Reset()
Resets the sensor to the state it had just after Init().
static const CRange< CRadians > UNSIGNED_RANGE
The unsigned normalization range [0:TWO_PI].
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
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")
void Setup(Real f_ground_half_range)
CControllableEntity * m_pcControllableEntity
virtual void Destroy()
Destroys the sensor.
CComposableEntity & GetParent()
Returns this entity's parent.
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 SetStart(const CVector3 &c_start)
void Disable()
Disables the entity.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
A data structure that contains positional entities.
Real Length() const
Returns the length of this vector.
const CVector3 & GetOffset() const
Returns the offset of the omnidirectional camera with respect to the reference point.
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
const CRadians & GetAperture() const
Returns the aperture of the visibility cone of the omnidirectional camera.
void SetEnd(const CVector3 &c_end)
Real GetY() const
Returns the y coordinate of this vector.
An SBlob represents a generic colored 2D segment in the image.
COmnidirectionalCameraLEDCheckOperation(CCI_ColoredBlobOmnidirectionalCameraSensor::TBlobList &t_blobs, COmnidirectionalCameraEquippedEntity &c_omnicam_entity, CEmbodiedEntity &c_embodied_entity, CControllableEntity &c_controllable_entity, bool b_show_rays, Real f_noise_std_dev)
An entity that contains a pointer to the user-defined controller.
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
Real Tan(const CRadians &c_radians)
Computes the tangent of the passed value in radians.
CVector3 Position
The position of the anchor wrt the global coordinate system.
float Real
Collects all ARGoS code.
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
CColoredBlobOmnidirectionalCameraRotZOnlySensor()
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
virtual ~CColoredBlobOmnidirectionalCameraRotZOnlySensor()