ARGoS  3
A parallel, multi-engine simulator for swarm robotics
footbot_motor_ground_rotzonly_sensor.cpp
Go to the documentation of this file.
1 
7 #include <argos3/core/simulator/simulator.h>
8 #include <argos3/core/simulator/entity/composable_entity.h>
9 #include <argos3/core/simulator/entity/embodied_entity.h>
10 #include <argos3/core/simulator/entity/floor_entity.h>
11 #include <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
12 
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  static CRange<Real> UNIT(0.0f, 1.0f);
21 
22  /****************************************/
23  /****************************************/
24 
26  m_pcEmbodiedEntity(NULL),
27  m_pcFloorEntity(NULL),
28  m_pcGroundSensorEntity(NULL),
29  m_pcRNG(NULL),
30  m_bAddNoise(false),
31  m_cSpace(CSimulator::GetInstance().GetSpace()) {}
32 
33  /****************************************/
34  /****************************************/
35 
37  m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
38  m_pcGroundSensorEntity = &(c_entity.GetComponent<CGroundSensorEquippedEntity>("ground_sensors"));
41  }
42 
43  /****************************************/
44  /****************************************/
45 
47  try {
49  /* Parse noise level */
50  Real fNoiseLevel = 0.0f;
51  GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
52  if(fNoiseLevel < 0.0f) {
53  THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the foot-bot ground sensor");
54  }
55  else if(fNoiseLevel > 0.0f) {
56  m_bAddNoise = true;
57  m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
58  m_pcRNG = CRandom::CreateRNG("argos");
59  }
60  m_tReadings.resize(4);
61  }
62  catch(CARGoSException& ex) {
63  THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot rotzonly ground sensor", ex);
64  }
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  /*
72  * We make the assumption that the robot is rotated only wrt to Z
73  */
74  /* Get robot position and orientation */
75  const CVector3& cEntityPos = m_pcEmbodiedEntity->GetOriginAnchor().Position;
77  CRadians cRotZ, cRotY, cRotX;
78  cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX);
79  /* Set robot center */
80  CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
81  /* Position of sensor on the ground after rototranslation */
82  CVector2 cSensorPos;
83  /* Go through the sensors */
84  for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
85  /* Calculate sensor position on the ground */
86  cSensorPos = m_pcGroundSensorEntity->GetSensor(i).Offset;
87  cSensorPos.Rotate(cRotZ);
88  cSensorPos += cCenterPos;
89  /* Get the color */
90  const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(),
91  cSensorPos.GetY());
92  /* Set the reading */
93  m_tReadings[i].Value = cColor.ToGrayScale() / 255.0f;
94  /* Apply noise to the sensor */
95  if(m_bAddNoise) {
97  }
98  /* Clamp the reading between 0 and 1 */
99  UNIT.TruncValue(m_tReadings[i].Value);
100  }
101  }
102 
103  /****************************************/
104  /****************************************/
105 
107  for(UInt32 i = 0; i < GetReadings().size(); ++i) {
108  m_tReadings[i].Value = 0.0f;
109  }
110  }
111 
112  /****************************************/
113  /****************************************/
114 
116  "footbot_motor_ground", "rot_z_only",
117  "Carlo Pinciroli [ilpincy@gmail.com]",
118  "1.0",
119  "The foot-bot motor ground sensor.",
120  "This sensor accesses the foot-bot motor ground sensor. For a complete description\n"
121  "of its usage, refer to the ci_footbot_motor_ground_sensor.h interface. For the XML\n"
122  "configuration, refer to the default ground sensor.\n",
123  "Usable"
124  );
125 
126 }
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::CColor::ToGrayScale
Real ToGrayScale() const
Returns the color in grayscale.
Definition: color.h:68
argos::CFootBotMotorGroundRotZOnlySensor::CFootBotMotorGroundRotZOnlySensor
CFootBotMotorGroundRotZOnlySensor()
Definition: footbot_motor_ground_rotzonly_sensor.cpp:25
argos::CFootBotMotorGroundRotZOnlySensor::m_cNoiseRange
CRange< Real > m_cNoiseRange
Noise range.
Definition: footbot_motor_ground_rotzonly_sensor.h:62
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::CFootBotMotorGroundRotZOnlySensor::m_pcFloorEntity
CFloorEntity * m_pcFloorEntity
Reference to floor entity.
Definition: footbot_motor_ground_rotzonly_sensor.h:50
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::CFootBotMotorGroundRotZOnlySensor
Definition: footbot_motor_ground_rotzonly_sensor.h:27
argos::CFootBotMotorGroundRotZOnlySensor::Update
virtual void Update()
Updates the state of the entity associated to this sensor.
Definition: footbot_motor_ground_rotzonly_sensor.cpp:70
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::CGroundSensorEquippedEntity::Enable
virtual void Enable()
Definition: ground_sensor_equipped_entity.cpp:110
argos::CFootBotMotorGroundRotZOnlySensor::m_pcRNG
CRandom::CRNG * m_pcRNG
Random number generator.
Definition: footbot_motor_ground_rotzonly_sensor.h:56
argos::CFootBotMotorGroundRotZOnlySensor::m_cSpace
CSpace & m_cSpace
Reference to the space.
Definition: footbot_motor_ground_rotzonly_sensor.h:65
argos::CFloorEntity::GetColorAtPoint
CColor GetColorAtPoint(Real f_x, Real f_y)
Returns the color at the given point.
Definition: floor_entity.h:99
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::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CVector2::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
argos::CGroundSensorEquippedEntity::SSensor::Offset
CVector2 Offset
Definition: ground_sensor_equipped_entity.h:35
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
argos::CGroundSensorEquippedEntity::GetSensor
SSensor & GetSensor(size_t un_idx)
Definition: ground_sensor_equipped_entity.h:70
argos::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:171
argos::CQuaternion
Definition: quaternion.h:14
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
footbot_motor_ground_rotzonly_sensor.h
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::CFootBotMotorGroundRotZOnlySensor::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
Definition: footbot_motor_ground_rotzonly_sensor.cpp:46
argos::CVector2::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
argos::CFootBotMotorGroundRotZOnlySensor::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
Definition: footbot_motor_ground_rotzonly_sensor.cpp:36
argos::CVector2
A 2D vector class.
Definition: vector2.h:25
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::GetFloorEntity
CFloorEntity & GetFloorEntity()
Returns the floor entity.
Definition: space.h:219
argos::CRange::TruncValue
void TruncValue(T &t_value) const
Definition: range.h:97
argos::CFootBotMotorGroundRotZOnlySensor::m_pcEmbodiedEntity
CEmbodiedEntity * m_pcEmbodiedEntity
Reference to embodied entity associated to this sensor.
Definition: footbot_motor_ground_rotzonly_sensor.h:47
argos::CCI_FootBotMotorGroundSensor::m_tReadings
TReadings m_tReadings
Definition: ci_footbot_motor_ground_sensor.h:81
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::CFootBotMotorGroundRotZOnlySensor::Reset
virtual void Reset()
Resets the sensor to the state it had just after Init().
Definition: footbot_motor_ground_rotzonly_sensor.cpp:106
argos::CRange::Set
void Set(const T &t_min, const T &t_max)
Definition: range.h:68
argos::CColor
The basic color type.
Definition: color.h:25
argos::CGroundSensorEquippedEntity
Definition: ground_sensor_equipped_entity.h:21
argos::CRandom::CreateRNG
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
argos::CFootBotMotorGroundRotZOnlySensor::m_bAddNoise
bool m_bAddNoise
Whether to add noise or not.
Definition: footbot_motor_ground_rotzonly_sensor.h:59
argos::CFootBotMotorGroundRotZOnlySensor::m_pcGroundSensorEntity
CGroundSensorEquippedEntity * m_pcGroundSensorEntity
Reference to ground sensor equipped entity associated to this sensor.
Definition: footbot_motor_ground_rotzonly_sensor.h:53
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
argos::CCI_FootBotMotorGroundSensor::GetReadings
const TReadings & GetReadings() const
Definition: ci_footbot_motor_ground_sensor.cpp:31
argos::CCI_Sensor::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
argos::CVector2::Rotate
CVector2 & Rotate(const CRadians &c_angle)
Rotates this vector by the wanted angle.
Definition: vector2.h:169
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