colored_blob_perspective_camera_default_sensor.cpp
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/perspective_camera_equipped_entity.h>
8 #include <argos3/plugins/simulator/media/led_medium.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
15  class CPerspectiveCameraLEDCheckOperation : public CPositionalIndex<CLEDEntity>::COperation {
16 
17  public:
18 
22  CEmbodiedEntity& c_embodied_entity,
23  CControllableEntity& c_controllable_entity,
24  bool b_show_rays,
25  Real f_noise_std_dev) :
26  m_tBlobs(t_blobs),
27  m_cCamEntity(c_cam_entity),
28  m_cEmbodiedEntity(c_embodied_entity),
29  m_cControllableEntity(c_controllable_entity),
30  m_bShowRays(b_show_rays),
31  m_fNoiseStdDev(f_noise_std_dev),
32  m_pcRNG(nullptr) {
33  m_pcRootSensingEntity = &m_cEmbodiedEntity.GetRootEntity();
34  if(m_fNoiseStdDev > 0.0f) {
35  m_pcRNG = CRandom::CreateRNG("argos");
36  }
37  }
39  while(! m_tBlobs.empty()) {
40  delete m_tBlobs.back();
41  m_tBlobs.pop_back();
42  }
43  }
44 
45  virtual bool operator()(CLEDEntity& c_led) {
46  /* Process this LED only if it's lit */
47  if(c_led.GetColor() != CColor::BLACK) {
48  /* Filter out the LEDs belonging to the sensing entity by checking if they share the same parent entity */
49  if(m_pcRootSensingEntity == &c_led.GetRootEntity()) return true;
50  /* If we are here, it's because the LED must be processed */
51  /* Set the end of the ray for occlusion checking */
52  m_cOcclusionCheckRay.SetEnd(c_led.GetPosition());
53  /* Calculate the vector to LED in the camera-anchor frame of reference */
54  m_cLEDRelative = c_led.GetPosition();
55  m_cLEDRelative -= m_cCamEntity.GetAnchor().Position;
56  m_cLEDRelative.Rotate(m_cInvCameraOrient);
57  /* Calculate the projection of the LED vector into the camera direction */
58  Real fDotProd = m_cLEDRelative.GetX();
59  /* The blob is visible if
60  * 1. It is within the distance range AND
61  * 2. It is within the aperture range AND
62  * 3. There are no occlusions
63  */
64  if(fDotProd < m_cCamEntity.GetRange() &&
65  ACos(fDotProd / m_cLEDRelative.Length()) < m_cCamEntity.GetAperture() &&
66  !GetClosestEmbodiedEntityIntersectedByRay(m_sIntersectionItem,
67  m_cOcclusionCheckRay,
68  m_cEmbodiedEntity)) {
69  /* The LED is visibile */
70  /* Calculate the intersection point between the LED ray and the image plane */
71  m_cLEDRelative.Normalize();
72  m_cLEDRelative *= m_cCamEntity.GetFocalLength() / m_cLEDRelative.GetX();
73  /*
74  * The image plane is perpendicular to the local X axis
75  * Y points to the left, Z up, the origin is in the image center
76  * To find the pixel (i,j), we need to flip both Y and Z, and translate the origin
77  * So that the origin is up-left, the i axis goes to the right, and the j axis goes down
78  */
79  SInt32 nI =
80  static_cast<SInt32>(- m_cCamEntity.GetImagePxWidth() /
81  m_cCamEntity.GetImageMtWidth() *
82  (m_cLEDRelative.GetY() -
83  m_cCamEntity.GetImageMtWidth() * 0.5f));
84  SInt32 nJ =
85  static_cast<SInt32>(- m_cCamEntity.GetImagePxHeight() /
86  m_cCamEntity.GetImageMtHeight() *
87  (m_cLEDRelative.GetZ() -
88  m_cCamEntity.GetImageMtHeight() * 0.5f));
89  /* Make sure (i,j) is within the limits */
90  if((nI >= m_cCamEntity.GetImagePxWidth() || nI < 0) ||
91  (nJ >= m_cCamEntity.GetImagePxHeight() || nJ < 0))
92  return true;
93  /* Add new blob */
94  m_tBlobs.push_back(
96  c_led.GetColor(), nI, nJ));
97  /* Draw ray */
98  if(m_bShowRays) {
99  m_cControllableEntity.AddCheckedRay(
100  false,
101  CRay3(m_cCamEntity.GetAnchor().Position,
102  c_led.GetPosition()));
103  }
104  }
105  }
106  return true;
107  }
108 
109  void Setup() {
110  /* Erase blobs */
111  while(! m_tBlobs.empty()) {
112  delete m_tBlobs.back();
113  m_tBlobs.pop_back();
114  }
115  /* Reset ray start */
116  m_cOcclusionCheckRay.SetStart(m_cCamEntity.GetAnchor().Position);
117  /* Calculate inverse of camera orientation */
118  m_cInvCameraOrient = m_cCamEntity.GetAnchor().Orientation.Inverse();
119  }
120 
121  private:
122 
124  CPerspectiveCameraEquippedEntity& m_cCamEntity;
125  CEmbodiedEntity& m_cEmbodiedEntity;
126  CControllableEntity& m_cControllableEntity;
127  CQuaternion m_cInvCameraOrient;
128  bool m_bShowRays;
129  CEntity* m_pcRootSensingEntity;
130  CRadians m_cTmp1, m_cTmp2;
131  CVector3 m_cLEDRelative;
132  SEmbodiedEntityIntersectionItem m_sIntersectionItem;
133  CRay3 m_cOcclusionCheckRay;
134  Real m_fNoiseStdDev;
135  CRandom::CRNG* m_pcRNG;
136  };
137 
138  /****************************************/
139  /****************************************/
140 
142  m_pcCamEntity(nullptr),
143  m_pcControllableEntity(nullptr),
144  m_pcEmbodiedEntity(nullptr),
145  m_pcLEDIndex(nullptr),
146  m_pcEmbodiedIndex(nullptr),
147  m_bShowRays(false) {
148  }
149 
150  /****************************************/
151  /****************************************/
152 
154  }
155 
156  /****************************************/
157  /****************************************/
158 
160  /* Get omndirectional camera equipped entity */
161  m_pcCamEntity = &(c_entity.GetComponent<CPerspectiveCameraEquippedEntity>("perspective_camera"));
162  /* Get controllable entity */
163  m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
164  /* Get embodied entity */
165  m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
166  }
167 
168  /****************************************/
169  /****************************************/
170 
172  try {
173  /* Parent class init */
175  /* Show rays? */
176  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
177  /* Parse noise */
178  Real fNoiseStdDev = 0.0f;
179  GetNodeAttributeOrDefault(t_tree, "noise_std_dev", fNoiseStdDev, fNoiseStdDev);
180  /* Get LED medium from id specified in the XML */
181  std::string strMedium;
182  GetNodeAttribute(t_tree, "medium", strMedium);
183  m_pcLEDIndex = &(CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium).GetIndex());
184  /* Create check operation */
187  *m_pcCamEntity,
190  m_bShowRays,
191  fNoiseStdDev);
192  }
193  catch(CARGoSException& ex) {
194  THROW_ARGOSEXCEPTION_NESTED("Error initializing the colored blob perspective camera default sensor", ex);
195  }
196  /* sensor is disabled by default */
197  Disable();
198  }
199 
200  /****************************************/
201  /****************************************/
202 
204  /* sensor is disabled--nothing to do */
205  if (IsDisabled()) {
206  return;
207  }
208  /* Increase data counter */
210  /* Prepare the operation */
211  m_pcOperation->Setup();
212  /* Calculate the sensing box */
213  Real fHalfRange = m_pcCamEntity->GetRange() * 0.5f;
214  Real fHalfSide = fHalfRange * Tan(m_pcCamEntity->GetAperture());
215  /* Box center */
216  CVector3 cCenter(fHalfRange, 0.0f, 0.0f);
218  cCenter += m_pcCamEntity->GetAnchor().Position;
219  /* Box half size */
220  CVector3 cCorner(fHalfRange, fHalfSide, fHalfSide);
222  CVector3 cHalfSize(
223  Abs(cCorner.GetX()),
224  Abs(cCorner.GetY()),
225  Abs(cCorner.GetZ()));
226  /* Go through LED entities in box range */
227  m_pcLEDIndex->ForEntitiesInBoxRange(
228  cCenter, cHalfSize, *m_pcOperation);
229  }
230 
231  /****************************************/
232  /****************************************/
233 
235  m_sReadings.Counter = 0;
236  m_sReadings.BlobList.clear();
237  }
238 
239  /****************************************/
240  /****************************************/
241 
243  delete m_pcOperation;
244  }
245 
246  /****************************************/
247  /****************************************/
248 
252 
253  }
254 
255  /****************************************/
256  /****************************************/
257 
261 
262  }
263 
264  /****************************************/
265  /****************************************/
266 
268  "colored_blob_perspective_camera", "default",
269  "Carlo Pinciroli [ilpincy@gmail.com]",
270  "1.0",
271 
272  "A generic perspective camera sensor to detect colored blobs.",
273  "This sensor accesses an perspective camera that detects colored blobs. The\n"
274  "sensor returns a list of blobs, each defined by a color and a position with\n"
275  "respect to the robot reference point on the ground. In controllers, you must\n"
276  "include the ci_colored_blob_perspective_camera_sensor.h header.\n\n"
277 
278  "This sensor is disabled by default, and must be enabled before it can be\n"
279  "used.\n\n"
280 
281  "REQUIRED XML CONFIGURATION\n\n"
282 
283  " <controllers>\n"
284  " ...\n"
285  " <my_controller ...>\n"
286  " ...\n"
287  " <sensors>\n"
288  " ...\n"
289  " <colored_blob_perspective_camera implementation=\"default\"\n"
290  " medium=\"leds\" />\n"
291  " ...\n"
292  " </sensors>\n"
293  " ...\n"
294  " </my_controller>\n"
295  " ...\n"
296  " </controllers>\n\n"
297 
298  "The 'medium' attribute must be set to the id of the leds medium declared in the\n"
299  "<media> section.\n\n"
300 
301  "OPTIONAL XML CONFIGURATION\n\n"
302 
303  "It is possible to draw the rays shot by the camera sensor in the OpenGL\n"
304  "visualization. This can be useful for sensor debugging but also to understand\n"
305  "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
306  "they are not obstructed and in purple when they are. In case a ray is\n"
307  "obstructed, a black dot is drawn where the intersection occurred.\n"
308  "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
309  "example:\n\n"
310 
311  " <controllers>\n"
312  " ...\n"
313  " <my_controller ...>\n"
314  " ...\n"
315  " <sensors>\n"
316  " ...\n"
317  " <colored_blob_perspective_camera implementation=\"default\"\n"
318  " medium=\"leds\" />\n"
319  " show_rays=\"true\" />\n"
320  " ...\n"
321  " </sensors>\n"
322  " ...\n"
323  " </my_controller>\n"
324  " ...\n"
325  " </controllers>\n\n"
326 
327  "It is possible to add uniform noise to the blobs, thus matching the\n"
328  "characteristics of a real robot better. This can be done with the attribute\n"
329  "\"noise_std_dev\".\n\n"
330 
331  " <controllers>\n"
332  " ...\n"
333  " <my_controller ...>\n"
334  " ...\n"
335  " <sensors>\n"
336  " ...\n"
337  " <colored_blob_perspective_camera implementation=\"default\"\n"
338  " medium=\"leds\" />\n"
339  " noise_std_dev=\"0.1\" />\n"
340  " ...\n"
341  " </sensors>\n"
342  " ...\n"
343  " </my_controller>\n"
344  " ...\n"
345  " </controllers>\n",
346 
347  "Usable"
348  );
349 
350 }
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
Definition: angles.h:622
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
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.
REGISTER_SENSOR(CEPuckProximityDefaultSensor, "epuck_proximity", "default", "Danesh Tarapore [daneshtarapore@gmail.com]", "1.0", "The E-Puck proximity sensor.", "This sensor accesses the epuck proximity sensor. For a complete description\n" "of its usage, refer to the ci_epuck_proximity_sensor.h interface. For the XML\n" "configuration, refer to the default proximity sensor.\n", "Usable")
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real Tan(const CRadians &c_radians)
Computes the tangent of the passed value in radians.
Definition: angles.h:604
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
virtual void Enable()
Enables updating of sensor information in the event loop.
Definition: ci_sensor.h:78
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
bool IsDisabled() const
Definition: ci_sensor.h:86
virtual void Disable()
Disables updating of sensor information in the event loop.
Definition: ci_sensor.h:83
Basic class for an entity that contains other entities.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
An entity that contains a pointer to the user-defined controller.
void AddCheckedRay(bool b_obstructed, const CRay3 &c_ray)
Adds a ray to the list of checked rays.
This entity is a link to a body in the physics engine.
The basic entity type.
Definition: entity.h:90
CEntity & GetRootEntity()
Returns the root entity containing this entity.
Definition: entity.cpp:115
const CVector3 & GetPosition() const
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
A data structure that contains positional entities.
The exception that wraps all errors in ARGoS.
static CColor BLACK
Definition: color.h:29
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CQuaternion Inverse() const
Definition: quaternion.h:98
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:347
The RNG.
Definition: rng.h:90
A 3D vector class.
Definition: vector3.h:31
Real Length() const
Returns the length of this vector.
Definition: vector3.h:227
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:23
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
CVector3 & Normalize()
Normalizes this vector.
Definition: vector3.h:237
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:137
std::vector< SBlob * > TBlobList
Vector of pointers to colored blobs.
An SBlob represents a generic colored 2D segment in the image.
CPerspectiveCameraLEDCheckOperation(CCI_ColoredBlobPerspectiveCameraSensor::TBlobList &t_blobs, CPerspectiveCameraEquippedEntity &c_cam_entity, CEmbodiedEntity &c_embodied_entity, CControllableEntity &c_controllable_entity, bool b_show_rays, Real f_noise_std_dev)
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
virtual void Disable()
Disables updating of sensor information in the event loop.
virtual void Enable()
Enables updating of sensor information in the event loop.
virtual void Reset()
Resets the sensor to the state it had just after Init().
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
virtual void Update()
Updates the state of the entity associated to this sensor, if the sensor is currently enabled.
const CColor & GetColor() const
Returns the current color of the LED.
Definition: led_entity.h:58
Real GetRange() const
Returns the range of the camera.
const SAnchor & GetAnchor() const
Returns the offset of the camera with respect to the reference point.
SInt32 GetImagePxWidth() const
Returns the image width in pixel.
const CRadians & GetAperture() const
Returns the aperture of the visibility cone of the camera.
Real GetFocalLength() const
Returns the focal length of the camera.
Real GetImageMtWidth() const
Returns the image width in meters.
SInt32 GetImagePxHeight() const
Returns the image height in pixel.
Real GetImageMtHeight() const
Returns the image height in meters.