ARGoS  3
A parallel, multi-engine simulator for swarm robotics
range_and_bearing_medium_sensor.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 #include <argos3/core/simulator/entity/controllable_entity.h>
11 #include <argos3/plugins/simulator/entities/rab_equipped_entity.h>
12 #include <argos3/plugins/simulator/media/rab_medium.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
19  CRange<CRadians> INCLINATION_RANGE(CRadians(0), CRadians(ARGOS_PI));
20 
21  /****************************************/
22  /****************************************/
23 
25  m_pcRangeAndBearingEquippedEntity(NULL),
26  m_fDistanceNoiseStdDev(0.0f),
27  m_fPacketDropProb(0.0f),
28  m_pcRNG(NULL),
29  m_cSpace(CSimulator::GetInstance().GetSpace()),
30  m_bShowRays(false) {}
31 
32  /****************************************/
33  /****************************************/
34 
36  /* Assign RAB equipped entity to this sensor */
37  m_pcRangeAndBearingEquippedEntity = &c_entity.GetComponent<CRABEquippedEntity>("rab");
38  /* Enable the RAB equipped entity */
39  m_pcRangeAndBearingEquippedEntity->Enable();
40  /* Get reference to controllable entity */
41  m_pcControllableEntity = &c_entity.GetComponent<CControllableEntity>("controller");
42  }
43 
44  /****************************************/
45  /****************************************/
46 
48  try {
49  /* Parent class init */
51  /* Show rays? */
52  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
53  /* Parse noise */
54  GetNodeAttributeOrDefault(t_tree, "noise_std_dev", m_fDistanceNoiseStdDev, m_fDistanceNoiseStdDev);
55  GetNodeAttributeOrDefault(t_tree, "packet_drop_prob", m_fPacketDropProb, m_fPacketDropProb);
56  if((m_fPacketDropProb > 0.0f) ||
57  (m_fDistanceNoiseStdDev > 0.0f)) {
58  m_pcRNG = CRandom::CreateRNG("argos");
59  }
60  /* Get RAB medium from id specified in the XML */
61  std::string strMedium;
62  GetNodeAttribute(t_tree, "medium", strMedium);
63  m_pcRangeAndBearingMedium = &(CSimulator::GetInstance().GetMedium<CRABMedium>(strMedium));
64  /* Assign RAB entity to the medium */
65  m_pcRangeAndBearingMedium->AddEntity(*m_pcRangeAndBearingEquippedEntity);
66  }
67  catch(CARGoSException& ex) {
68  THROW_ARGOSEXCEPTION_NESTED("Error initializing the range and bearing medium sensor", ex);
69  }
70  }
71 
72  /****************************************/
73  /****************************************/
74 
77  /* Delete old readings */
78  m_tReadings.clear();
79  /* Get list of communicating RABs */
80  const CSet<CRABEquippedEntity*>& setRABs = m_pcRangeAndBearingMedium->GetRABsCommunicatingWith(*m_pcRangeAndBearingEquippedEntity);
81  /* Buffer for calculating the message--robot distance */
82  CVector3 cVectorRobotToMessage;
83  /* Buffer for the received packet */
85  /* Go through communicating RABs and create packets */
86  for(CSet<CRABEquippedEntity*>::iterator it = setRABs.begin();
87  it != setRABs.end(); ++it) {
88  /* Should we drop this packet? */
89  if(m_pcRNG == NULL || /* No noise to apply */
90  !(m_fPacketDropProb > 0.0f &&
91  m_pcRNG->Bernoulli(m_fPacketDropProb)) /* Packet is not dropped */
92  ) {
93  /* Create a reference to the RAB entity to process */
94  CRABEquippedEntity& cRABEntity = **it;
95  /* Add ray if requested */
96  if(m_bShowRays) {
97  m_pcControllableEntity->AddCheckedRay(false,
98  CRay3(cRABEntity.GetPosition(),
99  m_pcRangeAndBearingEquippedEntity->GetPosition()));
100  }
101  /* Calculate vector to entity */
102  cVectorRobotToMessage = cRABEntity.GetPosition();
103  cVectorRobotToMessage -= m_pcRangeAndBearingEquippedEntity->GetPosition();
104  /* If noise was setup, add it */
105  if(m_pcRNG && m_fDistanceNoiseStdDev > 0.0f) {
106  cVectorRobotToMessage += CVector3(
107  m_pcRNG->Gaussian(m_fDistanceNoiseStdDev),
108  m_pcRNG->Uniform(INCLINATION_RANGE),
110  }
111  /*
112  * Set range and bearing from cVectorRobotToMessage
113  * First, we must rotate the cVectorRobotToMessage so that
114  * it is local to the robot coordinate system. To do this,
115  * it enough to rotate cVectorRobotToMessage by the inverse
116  * of the robot orientation.
117  */
118  cVectorRobotToMessage.Rotate(m_pcRangeAndBearingEquippedEntity->GetOrientation().Inverse());
119  cVectorRobotToMessage.ToSphericalCoords(sPacket.Range,
120  sPacket.VerticalBearing,
121  sPacket.HorizontalBearing);
122  /* Convert range to cm */
123  sPacket.Range *= 100.0f;
124  /* Normalize horizontal bearing between [-pi,pi] */
126  /*
127  * The vertical bearing is defined as the angle between the local
128  * robot XY plane and the message source position, i.e., the elevation
129  * in math jargon. However, cVectorRobotToMessage.ToSphericalCoords()
130  * sets sPacket.VerticalBearing to the inclination, which is the angle
131  * between the azimuth vector (robot local Z axis) and
132  * cVectorRobotToMessage. Elevation = 90 degrees - Inclination.
133  */
134  sPacket.VerticalBearing.Negate();
137  /* Set message data */
138  sPacket.Data = cRABEntity.GetData();
139  /* Add message to the list */
140  m_tReadings.push_back(sPacket);
141  }
142  }
143  }
144 
145  /****************************************/
146  /****************************************/
147 
149  m_tReadings.clear();
150  }
151 
152  /****************************************/
153  /****************************************/
154 
156  m_pcRangeAndBearingMedium->RemoveEntity(*m_pcRangeAndBearingEquippedEntity);
157  }
158 
159  /****************************************/
160  /****************************************/
161 
163  "range_and_bearing", "medium",
164  "Carlo Pinciroli [ilpincy@gmail.com]",
165  "1.0",
166  "The range-and-bearing sensor.",
167  "This sensor allows robots to perform situated communication, i.e., a form of\n"
168  "wireless communication whereby the receiver also knows the location of the\n"
169  "sender with respect to its own frame of reference.\n"
170  "This implementation of the range-and-bearing sensor is associated to the\n"
171  "range-and-bearing medium. To be able to use this sensor, you must add a\n"
172  "range-and-bearing medium to the <media> section.\n"
173  "This sensor allows a robot to receive messages. To send messages, you need the\n"
174  "range-and-bearing actuator.\n"
175  "To use this sensor, in controllers you must include the\n"
176  "ci_range_and_bearing_sensor.h header.\n\n"
177  "REQUIRED XML CONFIGURATION\n\n"
178  " <controllers>\n"
179  " ...\n"
180  " <my_controller ...>\n"
181  " ...\n"
182  " <sensors>\n"
183  " ...\n"
184  " <range_and_bearing implementation=\"medium\"\n"
185  " medium=\"rab\" />\n"
186  " ...\n"
187  " </sensors>\n"
188  " ...\n"
189  " </my_controller>\n"
190  " ...\n"
191  " </controllers>\n\n"
192  "The 'medium' attribute must be set to the id of the range-and-bearing medium\n"
193  "declared in the <media> section.\n\n"
194  "OPTIONAL XML CONFIGURATION\n\n"
195  "It is possible to draw the rays shot by the range-and-bearing sensor in the\n"
196  "OpenGL visualization. This can be useful for sensor debugging but also to\n"
197  "understand what's wrong in your controller. In OpenGL, the rays are drawn in\n"
198  "cyan when two robots are communicating.\n"
199  "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
200  "example:\n\n"
201  " <controllers>\n"
202  " ...\n"
203  " <my_controller ...>\n"
204  " ...\n"
205  " <sensors>\n"
206  " ...\n"
207  " <range_and_bearing implementation=\"medium\"\n"
208  " medium=\"rab\"\n"
209  " show_rays=\"true\" />\n"
210  " ...\n"
211  " </sensors>\n"
212  " ...\n"
213  " </my_controller>\n"
214  " ...\n"
215  " </controllers>\n\n"
216  "It is possible to add noise to the readings, thus matching the characteristics\n"
217  "of a real robot better. Noise is implemented as a random vector added to the\n"
218  "vector joining two communicating robots. For the random vector, the inclination\n"
219  "and azimuth are chosen uniformly in the range [0:PI] and [0:2PI], respectively,\n"
220  "and the length is drawn from a Gaussian distribution. The standard deviation of\n"
221  "the Gaussian distribution is expressed in meters and set by the user through\n"
222  "the attribute 'noise_std_dev' as shown in this example:\n\n"
223  " <controllers>\n"
224  " ...\n"
225  " <my_controller ...>\n"
226  " ...\n"
227  " <sensors>\n"
228  " ...\n"
229  " <range_and_bearing implementation=\"medium\"\n"
230  " medium=\"rab\"\n"
231  " noise_std_dev=\"0.1\" />\n"
232  " ...\n"
233  " </sensors>\n"
234  " ...\n"
235  " </my_controller>\n"
236  " ...\n"
237  " </controllers>\n\n"
238  "In addition, it is possible to specify the probability that a packet gets lost\n"
239  "even though the robot should have received it (i.e., packet dropping). To set\n"
240  "this probability, use the attribute 'packet_drop_prob' as shown in the example:\n"
241  " <controllers>\n"
242  " ...\n"
243  " <my_controller ...>\n"
244  " ...\n"
245  " <sensors>\n"
246  " ...\n"
247  " <range_and_bearing implementation=\"medium\"\n"
248  " medium=\"rab\"\n"
249  " packet_drop_prob=\"0.1\" />\n"
250  " ...\n"
251  " </sensors>\n"
252  " ...\n"
253  " </my_controller>\n"
254  " ...\n"
255  " </controllers>\n" ,
256  "Usable");
257 
258 }
argos::CRangeAndBearingMediumSensor::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
Definition: range_and_bearing_medium_sensor.cpp:35
argos::CRandom::CRNG::Gaussian
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
Definition: rng.cpp:131
ARGOS_PI
#define ARGOS_PI
To be used when initializing static variables.
Definition: angles.h:32
argos::CRandom::CRNG::Uniform
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
Definition: rng.cpp:87
argos::CRABEquippedEntity::Enable
virtual void Enable()
Definition: rab_equipped_entity.cpp:129
argos::CSimulator::GetInstance
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:87
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::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
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::CCI_RangeAndBearingSensor::SPacket
Definition: ci_range_and_bearing_sensor.h:28
argos::CComposableEntity::GetComponent
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
Definition: composable_entity.cpp:109
argos::CQuaternion::Inverse
CQuaternion Inverse() const
Definition: quaternion.h:97
argos::CRay3
Definition: ray3.h:19
argos::CRangeAndBearingMediumSensor
Definition: range_and_bearing_medium_sensor.h:27
argos::CRadians::SignedNormalize
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
argos::CRangeAndBearingMediumSensor::Reset
virtual void Reset()
Resets the sensor to the state it had just after Init().
Definition: range_and_bearing_medium_sensor.cpp:148
argos::CVector3::ToSphericalCoords
void ToSphericalCoords(Real &f_radius, CRadians &c_inclination, CRadians &c_azimuth) const
Returns the vector contents as spherical coordinates.
Definition: vector3.h:185
argos::CSimulator
The core class of ARGOS.
Definition: simulator.h:62
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CRandom::CRNG::Bernoulli
bool Bernoulli(Real f_true=0.5)
Returns a random value from a Bernoulli distribution.
Definition: rng.cpp:80
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::CRABEquippedEntity
Definition: rab_equipped_entity.h:25
argos::CRadians::Negate
CRadians & Negate()
Definition: angles.h:153
argos::CSet::end
iterator end() const
Returns an invalid iterator.
Definition: set.h:396
argos::INCLINATION_RANGE
CRange< CRadians > INCLINATION_RANGE(CRadians(0), CRadians(ARGOS_PI))
argos::CPositionalEntity::GetPosition
const CVector3 & GetPosition() const
Definition: positional_entity.h:36
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
argos::CCI_RangeAndBearingSensor::m_tReadings
TReadings m_tReadings
Definition: ci_range_and_bearing_sensor.h:60
argos::CRABEquippedEntity::GetData
CByteArray & GetData()
Definition: rab_equipped_entity.h:67
argos::CSetIterator
The CSet iterator.
Definition: set.h:39
argos::CRangeAndBearingMediumSensor::CRangeAndBearingMediumSensor
CRangeAndBearingMediumSensor()
Definition: range_and_bearing_medium_sensor.cpp:24
argos::CRadians::UNSIGNED_RANGE
static const CRange< CRadians > UNSIGNED_RANGE
The unsigned normalization range [0:TWO_PI].
Definition: angles.h:274
argos::CRangeAndBearingMediumSensor::Destroy
virtual void Destroy()
Destroys the sensor.
Definition: range_and_bearing_medium_sensor.cpp:155
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")
range_and_bearing_medium_sensor.h
argos::CPositionalEntity::GetOrientation
const CQuaternion & GetOrientation() const
Definition: positional_entity.h:52
argos::CCI_RangeAndBearingSensor::SPacket::HorizontalBearing
CRadians HorizontalBearing
Definition: ci_range_and_bearing_sensor.h:30
argos::CRABMedium::GetRABsCommunicatingWith
const CSet< CRABEquippedEntity * > & GetRABsCommunicatingWith(CRABEquippedEntity &c_entity) const
Returns an immutable vector of RAB entities that can communicated with the given entity.
Definition: rab_medium.cpp:222
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::CRABMedium::AddEntity
void AddEntity(CRABEquippedEntity &c_entity)
Adds the specified entity to the list of managed entities.
Definition: rab_medium.cpp:198
argos::CRangeAndBearingMediumSensor::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
Definition: range_and_bearing_medium_sensor.cpp:47
argos::GetNodeAttribute
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
Definition: argos_configuration.h:208
argos::CCI_RangeAndBearingSensor::SPacket::VerticalBearing
CRadians VerticalBearing
The vertical bearing is defined as the angle between the local robot XY plane and the message source ...
Definition: ci_range_and_bearing_sensor.h:38
argos::CRABMedium
Definition: rab_medium.h:16
argos::CSet::begin
iterator begin() const
Returns an iterator to the first element.
Definition: set.h:388
argos::CRandom::CreateRNG
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:326
argos::CVector3::Rotate
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
argos::CCI_RangeAndBearingSensor::SPacket::Data
CByteArray Data
Definition: ci_range_and_bearing_sensor.h:39
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CRangeAndBearingMediumSensor::Update
virtual void Update()
Updates the state of the entity associated to this sensor.
Definition: range_and_bearing_medium_sensor.cpp:75
argos::CCI_Sensor::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
argos::CRABMedium::RemoveEntity
void RemoveEntity(CRABEquippedEntity &c_entity)
Removes the specified entity from the list of managed entities.
Definition: rab_medium.cpp:208
argos::CSet
Defines a very simple double-linked list that stores unique elements.
Definition: set.h:100
argos::CSimulator::GetMedium
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
argos::CCI_RangeAndBearingSensor::SPacket::Range
Real Range
Definition: ci_range_and_bearing_sensor.h:29