footbot_entity.cpp
Go to the documentation of this file.
1 
7 #include "footbot_entity.h"
8 
9 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
10 #include <argos3/core/simulator/space/space.h>
11 #include <argos3/core/simulator/entity/controllable_entity.h>
12 #include <argos3/core/simulator/entity/embodied_entity.h>
13 #include <argos3/plugins/simulator/entities/battery_equipped_entity.h>
14 #include <argos3/plugins/simulator/entities/rab_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/gripper_equipped_entity.h>
16 #include <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
17 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
18 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
19 #include <argos3/plugins/simulator/entities/omnidirectional_camera_equipped_entity.h>
20 #include <argos3/plugins/simulator/entities/perspective_camera_equipped_entity.h>
21 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
22 #include <argos3/plugins/simulator/entities/wifi_equipped_entity.h>
23 #include "footbot_distance_scanner_equipped_entity.h"
24 #include "footbot_turret_entity.h"
25 
26 namespace argos {
27 
28  /****************************************/
29  /****************************************/
30 
31  static const Real BODY_RADIUS = 0.085036758f;
32  static const Real BODY_HEIGHT = 0.146899733f;
33 
34  static const Real LED_RING_RADIUS = BODY_RADIUS + 0.005;
35 
36  static const Real INTERWHEEL_DISTANCE = 0.14f;
37  static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
38  static const Real WHEEL_RADIUS = 0.029112741f;
39 
40  static const Real PROXIMITY_SENSOR_RING_ELEVATION = 0.06f;
41  static const Real PROXIMITY_SENSOR_RING_RADIUS = BODY_RADIUS;
42  static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((ARGOS_PI / 12.0f) * 0.5f);
43  static const Real PROXIMITY_SENSOR_RING_RANGE = 0.1f;
44 
45  static const Real LED_RING_ELEVATION = 0.085f;
46  static const Real RAB_ELEVATION = 0.1f;
47  static const Real BEACON_ELEVATION = 0.174249733f;
48 
49  static const Real GRIPPER_ELEVATION = LED_RING_ELEVATION;
50 
51  static const CRadians LED_ANGLE_SLICE = CRadians(ARGOS_PI / 6.0);
52  static const CRadians HALF_LED_ANGLE_SLICE = LED_ANGLE_SLICE * 0.5f;
53 
54  static const Real OMNIDIRECTIONAL_CAMERA_ELEVATION = 0.288699733f;
55 
56  /****************************************/
57  /****************************************/
58 
59  CFootBotEntity::CFootBotEntity() :
60  CComposableEntity(nullptr),
61  m_pcControllableEntity(nullptr),
62  m_pcDistanceScannerEquippedEntity(nullptr),
63  m_pcTurretEntity(nullptr),
64  m_pcEmbodiedEntity(nullptr),
65  m_pcGripperEquippedEntity(nullptr),
66  m_pcGroundSensorEquippedEntity(nullptr),
67  m_pcLEDEquippedEntity(nullptr),
68  m_pcLightSensorEquippedEntity(nullptr),
69  m_pcOmnidirectionalCameraEquippedEntity(nullptr),
70  m_pcPerspectiveCameraEquippedEntity(nullptr),
71  m_pcProximitySensorEquippedEntity(nullptr),
72  m_pcRABEquippedEntity(nullptr),
73  m_pcWheeledEntity(nullptr),
74  m_pcWiFiEquippedEntity(nullptr),
75  m_pcBatteryEquippedEntity(nullptr) {
76  }
77 
78  /****************************************/
79  /****************************************/
80 
81  CFootBotEntity::CFootBotEntity(const std::string& str_id,
82  const std::string& str_controller_id,
83  const CVector3& c_position,
84  const CQuaternion& c_orientation,
85  Real f_rab_range,
86  size_t un_rab_data_size,
87  const std::string& str_bat_model,
88  const CRadians& c_omnicam_aperture,
89  bool b_perspcam_front,
90  const CRadians& c_perspcam_aperture,
91  Real f_perspcam_focal_length,
92  Real f_perspcam_range) :
93  CComposableEntity(nullptr, str_id),
94  m_pcControllableEntity(nullptr),
95  m_pcDistanceScannerEquippedEntity(nullptr),
96  m_pcTurretEntity(nullptr),
97  m_pcEmbodiedEntity(nullptr),
98  m_pcGripperEquippedEntity(nullptr),
99  m_pcGroundSensorEquippedEntity(nullptr),
100  m_pcLEDEquippedEntity(nullptr),
101  m_pcLightSensorEquippedEntity(nullptr),
102  m_pcOmnidirectionalCameraEquippedEntity(nullptr),
103  m_pcPerspectiveCameraEquippedEntity(nullptr),
104  m_pcProximitySensorEquippedEntity(nullptr),
105  m_pcRABEquippedEntity(nullptr),
106  m_pcWheeledEntity(nullptr),
107  m_pcWiFiEquippedEntity(nullptr),
108  m_pcBatteryEquippedEntity(nullptr) {
109  try {
110  /*
111  * Create and init components
112  */
113  /*
114  * Embodied entity
115  * Better to put this first, because many other entities need this one
116  */
117  m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
118  AddComponent(*m_pcEmbodiedEntity);
119  SAnchor& cTurretAnchor = m_pcEmbodiedEntity->AddAnchor("turret");
120  CQuaternion cPerspCamOrient(b_perspcam_front ? CRadians::ZERO : -CRadians::PI_OVER_TWO,
121  CVector3::Y);
122  SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->AddAnchor("perspective_camera",
123  CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
124  cPerspCamOrient);
125  /* Wheeled entity and wheel positions (left, right) */
126  m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
127  AddComponent(*m_pcWheeledEntity);
128  m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
129  m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
130  /* LED equipped entity, with LEDs [0-11] and beacon [12] */
131  m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
132  AddComponent(*m_pcLEDEquippedEntity);
133  m_pcLEDEquippedEntity->AddLEDRing(
134  CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
135  LED_RING_RADIUS,
136  HALF_LED_ANGLE_SLICE,
137  12,
138  cTurretAnchor);
139  m_pcLEDEquippedEntity->AddLED(
140  CVector3(0.0f, 0.0f, BEACON_ELEVATION),
141  cTurretAnchor);
142  /* Proximity sensor equipped entity */
143  m_pcProximitySensorEquippedEntity =
144  new CProximitySensorEquippedEntity(this, "proximity_0");
145  AddComponent(*m_pcProximitySensorEquippedEntity);
146  m_pcProximitySensorEquippedEntity->AddSensorRing(
147  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
148  PROXIMITY_SENSOR_RING_RADIUS,
149  PROXIMITY_SENSOR_RING_START_ANGLE,
150  PROXIMITY_SENSOR_RING_RANGE,
151  24,
152  m_pcEmbodiedEntity->GetOriginAnchor());
153  /* Light sensor equipped entity */
154  m_pcLightSensorEquippedEntity =
155  new CLightSensorEquippedEntity(this, "light_0");
156  AddComponent(*m_pcLightSensorEquippedEntity);
157  m_pcLightSensorEquippedEntity->AddSensorRing(
158  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
159  PROXIMITY_SENSOR_RING_RADIUS,
160  PROXIMITY_SENSOR_RING_START_ANGLE,
161  PROXIMITY_SENSOR_RING_RANGE,
162  24,
163  m_pcEmbodiedEntity->GetOriginAnchor());
164  /* Gripper equipped entity */
165  m_pcGripperEquippedEntity =
166  new CGripperEquippedEntity(this,
167  "gripper_0",
168  CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
169  CVector3::X);
170  AddComponent(*m_pcGripperEquippedEntity);
171  /* Ground sensor equipped entity */
172  m_pcGroundSensorEquippedEntity =
173  new CGroundSensorEquippedEntity(this, "ground_0");
174  AddComponent(*m_pcGroundSensorEquippedEntity);
175  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, 0.0116),
176  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
177  m_pcEmbodiedEntity->GetOriginAnchor());
178  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, 0.0116),
179  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
180  m_pcEmbodiedEntity->GetOriginAnchor());
181  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, -0.0116),
182  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
183  m_pcEmbodiedEntity->GetOriginAnchor());
184  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, -0.0116),
185  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
186  m_pcEmbodiedEntity->GetOriginAnchor());
187  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.08, 0.0),
188  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
189  m_pcEmbodiedEntity->GetOriginAnchor());
190  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, 0.065),
191  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
192  m_pcEmbodiedEntity->GetOriginAnchor());
193  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, 0.08),
194  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
195  m_pcEmbodiedEntity->GetOriginAnchor());
196  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, 0.065),
197  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
198  m_pcEmbodiedEntity->GetOriginAnchor());
199  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.08, 0.0),
200  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
201  m_pcEmbodiedEntity->GetOriginAnchor());
202  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, -0.065),
203  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
204  m_pcEmbodiedEntity->GetOriginAnchor());
205  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, -0.08),
206  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
207  m_pcEmbodiedEntity->GetOriginAnchor());
208  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, -0.065),
209  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
210  m_pcEmbodiedEntity->GetOriginAnchor());
211  /* Distance scanner */
212  m_pcDistanceScannerEquippedEntity =
213  new CFootBotDistanceScannerEquippedEntity(this, "distance_scanner_0");
214  AddComponent(*m_pcDistanceScannerEquippedEntity);
215  /* RAB equipped entity */
216  m_pcRABEquippedEntity =
217  new CRABEquippedEntity(this,
218  "rab_0",
219  un_rab_data_size,
220  f_rab_range,
221  m_pcEmbodiedEntity->GetOriginAnchor(),
222  *m_pcEmbodiedEntity,
223  CVector3(0.0f, 0.0f, RAB_ELEVATION));
224  AddComponent(*m_pcRABEquippedEntity);
225  /* Omnidirectional camera equipped entity */
226  m_pcOmnidirectionalCameraEquippedEntity =
227  new COmnidirectionalCameraEquippedEntity(this,
228  "omnidirectional_camera_0",
229  c_omnicam_aperture,
230  CVector3(0.0f,
231  0.0f,
232  OMNIDIRECTIONAL_CAMERA_ELEVATION));
233  AddComponent(*m_pcOmnidirectionalCameraEquippedEntity);
234  /* Perspective camera equipped entity */
235  m_pcPerspectiveCameraEquippedEntity =
236  new CPerspectiveCameraEquippedEntity(this,
237  "perspective_camera_0",
238  c_perspcam_aperture,
239  f_perspcam_focal_length,
240  f_perspcam_range,
241  640, 480,
242  cPerspCamAnchor);
243  AddComponent(*m_pcPerspectiveCameraEquippedEntity);
244  /* Turret equipped entity */
245  m_pcTurretEntity = new CFootBotTurretEntity(this, "turret_0", cTurretAnchor);
246  AddComponent(*m_pcTurretEntity);
247  /* WiFi equipped entity */
248  m_pcWiFiEquippedEntity = new CWiFiEquippedEntity(this, "wifi_0");
249  AddComponent(*m_pcWiFiEquippedEntity);
250  /* Battery equipped entity */
251  m_pcBatteryEquippedEntity = new CBatteryEquippedEntity(this, "battery_0", str_bat_model);
252  AddComponent(*m_pcBatteryEquippedEntity);
253  /* Controllable entity
254  It must be the last one, for actuators/sensors to link to composing entities correctly */
255  m_pcControllableEntity = new CControllableEntity(this, "controller_0");
256  AddComponent(*m_pcControllableEntity);
257  m_pcControllableEntity->SetController(str_controller_id);
258  /* Update components */
259  UpdateComponents();
260  }
261  catch(CARGoSException& ex) {
262  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
263  }
264  }
265 
266  /****************************************/
267  /****************************************/
268 
269  void CFootBotEntity::Init(TConfigurationNode& t_tree) {
270  try {
271  /*
272  * Init parent
273  */
274  CComposableEntity::Init(t_tree);
275  /*
276  * Create and init components
277  */
278  /*
279  * Embodied entity
280  * Better to put this first, because many other entities need this one
281  */
282  m_pcEmbodiedEntity = new CEmbodiedEntity(this);
283  AddComponent(*m_pcEmbodiedEntity);
284  m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
285  SAnchor& cTurretAnchor = m_pcEmbodiedEntity->AddAnchor("turret");
286  /* Wheeled entity and wheel positions (left, right) */
287  m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
288  AddComponent(*m_pcWheeledEntity);
289  m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
290  m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
291  /* LED equipped entity, with LEDs [0-11] and beacon [12] */
292  m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
293  AddComponent(*m_pcLEDEquippedEntity);
294  m_pcLEDEquippedEntity->AddLEDRing(
295  CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
296  LED_RING_RADIUS,
297  HALF_LED_ANGLE_SLICE,
298  12,
299  cTurretAnchor);
300  m_pcLEDEquippedEntity->AddLED(
301  CVector3(0.0f, 0.0f, BEACON_ELEVATION),
302  cTurretAnchor);
303  /* Proximity sensor equipped entity */
304  m_pcProximitySensorEquippedEntity =
305  new CProximitySensorEquippedEntity(this, "proximity_0");
306  AddComponent(*m_pcProximitySensorEquippedEntity);
307  m_pcProximitySensorEquippedEntity->AddSensorRing(
308  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
309  PROXIMITY_SENSOR_RING_RADIUS,
310  PROXIMITY_SENSOR_RING_START_ANGLE,
311  PROXIMITY_SENSOR_RING_RANGE,
312  24,
313  m_pcEmbodiedEntity->GetOriginAnchor());
314  /* Light sensor equipped entity */
315  m_pcLightSensorEquippedEntity =
316  new CLightSensorEquippedEntity(this, "light_0");
317  AddComponent(*m_pcLightSensorEquippedEntity);
318  m_pcLightSensorEquippedEntity->AddSensorRing(
319  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
320  PROXIMITY_SENSOR_RING_RADIUS,
321  PROXIMITY_SENSOR_RING_START_ANGLE,
322  PROXIMITY_SENSOR_RING_RANGE,
323  24,
324  m_pcEmbodiedEntity->GetOriginAnchor());
325  /* Gripper equipped entity */
326  m_pcGripperEquippedEntity =
327  new CGripperEquippedEntity(this,
328  "gripper_0",
329  CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
330  CVector3::X);
331  AddComponent(*m_pcGripperEquippedEntity);
332  /* Ground sensor equipped entity */
333  m_pcGroundSensorEquippedEntity =
334  new CGroundSensorEquippedEntity(this, "ground_0");
335  AddComponent(*m_pcGroundSensorEquippedEntity);
336  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, 0.0116),
337  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
338  m_pcEmbodiedEntity->GetOriginAnchor());
339  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, 0.0116),
340  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
341  m_pcEmbodiedEntity->GetOriginAnchor());
342  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, -0.0116),
343  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
344  m_pcEmbodiedEntity->GetOriginAnchor());
345  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, -0.0116),
346  CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
347  m_pcEmbodiedEntity->GetOriginAnchor());
348  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.08, 0.0),
349  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
350  m_pcEmbodiedEntity->GetOriginAnchor());
351  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, 0.065),
352  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
353  m_pcEmbodiedEntity->GetOriginAnchor());
354  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, 0.08),
355  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
356  m_pcEmbodiedEntity->GetOriginAnchor());
357  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, 0.065),
358  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
359  m_pcEmbodiedEntity->GetOriginAnchor());
360  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.08, 0.0),
361  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
362  m_pcEmbodiedEntity->GetOriginAnchor());
363  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, -0.065),
364  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
365  m_pcEmbodiedEntity->GetOriginAnchor());
366  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, -0.08),
367  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
368  m_pcEmbodiedEntity->GetOriginAnchor());
369  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, -0.065),
370  CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
371  m_pcEmbodiedEntity->GetOriginAnchor());
372  /* Distance scanner */
373  m_pcDistanceScannerEquippedEntity =
374  new CFootBotDistanceScannerEquippedEntity(this, "distance_scanner_0");
375  AddComponent(*m_pcDistanceScannerEquippedEntity);
376  /* RAB equipped entity */
377  Real fRange = 3.0f;
378  GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
379  UInt32 unDataSize = 10;
380  GetNodeAttributeOrDefault(t_tree, "rab_data_size", unDataSize, unDataSize);
381  m_pcRABEquippedEntity =
382  new CRABEquippedEntity(this,
383  "rab_0",
384  unDataSize,
385  fRange,
386  m_pcEmbodiedEntity->GetOriginAnchor(),
387  *m_pcEmbodiedEntity,
388  CVector3(0.0f, 0.0f, RAB_ELEVATION));
389  AddComponent(*m_pcRABEquippedEntity);
390  /* Omnidirectional camera equipped entity */
391  CDegrees cAperture(70.0f);
392  GetNodeAttributeOrDefault(t_tree, "omnidirectional_camera_aperture", cAperture, cAperture);
393  m_pcOmnidirectionalCameraEquippedEntity =
394  new COmnidirectionalCameraEquippedEntity(this,
395  "omnidirectional_camera_0",
396  ToRadians(cAperture),
397  CVector3(0.0f,
398  0.0f,
399  OMNIDIRECTIONAL_CAMERA_ELEVATION));
400  AddComponent(*m_pcOmnidirectionalCameraEquippedEntity);
401  /* Perspective camera equipped entity */
402  bool bPerspCamFront = true;
403  GetNodeAttributeOrDefault(t_tree, "perspective_camera_front", bPerspCamFront, bPerspCamFront);
404  Real fPerspCamFocalLength = 0.035;
405  GetNodeAttributeOrDefault(t_tree, "perspective_camera_focal_length", fPerspCamFocalLength, fPerspCamFocalLength);
406  Real fPerspCamRange = 2.0;
407  GetNodeAttributeOrDefault(t_tree, "perspective_camera_range", fPerspCamRange, fPerspCamRange);
408  cAperture.SetValue(30.0f);
409  GetNodeAttributeOrDefault(t_tree, "perspective_camera_aperture", cAperture, cAperture);
410  CQuaternion cPerspCamOrient(bPerspCamFront ? CRadians::ZERO : -CRadians::PI_OVER_TWO,
411  CVector3::Y);
412  SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->AddAnchor("perspective_camera",
413  CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
414  cPerspCamOrient);
415  m_pcPerspectiveCameraEquippedEntity =
416  new CPerspectiveCameraEquippedEntity(this,
417  "perspective_camera_0",
418  ToRadians(cAperture),
419  fPerspCamFocalLength,
420  fPerspCamRange,
421  640, 480,
422  cPerspCamAnchor);
423  AddComponent(*m_pcPerspectiveCameraEquippedEntity);
424  /* Turret equipped entity */
425  m_pcTurretEntity = new CFootBotTurretEntity(this, "turret_0", cTurretAnchor);
426  AddComponent(*m_pcTurretEntity);
427  /* WiFi equipped entity */
428  m_pcWiFiEquippedEntity = new CWiFiEquippedEntity(this, "wifi_0");
429  AddComponent(*m_pcWiFiEquippedEntity);
430  /* Battery equipped entity */
431  m_pcBatteryEquippedEntity = new CBatteryEquippedEntity(this, "battery_0");
432  if(NodeExists(t_tree, "battery"))
433  m_pcBatteryEquippedEntity->Init(GetNode(t_tree, "battery"));
434  AddComponent(*m_pcBatteryEquippedEntity);
435  /* Controllable entity
436  It must be the last one, for actuators/sensors to link to composing entities correctly */
437  m_pcControllableEntity = new CControllableEntity(this);
438  AddComponent(*m_pcControllableEntity);
439  m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
440  /* Update components */
441  UpdateComponents();
442  }
443  catch(CARGoSException& ex) {
444  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
445  }
446  }
447 
448  /****************************************/
449  /****************************************/
450 
451  void CFootBotEntity::Reset() {
452  /* Reset all components */
453  CComposableEntity::Reset();
454  /* Update components */
455  UpdateComponents();
456  }
457 
458  /****************************************/
459  /****************************************/
460 
461 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
462 
463  void CFootBotEntity::UpdateComponents() {
464  /* Update only the components that might change */
465  UPDATE(m_pcDistanceScannerEquippedEntity);
466  UPDATE(m_pcTurretEntity);
467  UPDATE(m_pcGripperEquippedEntity);
468  UPDATE(m_pcRABEquippedEntity);
469  UPDATE(m_pcLEDEquippedEntity);
470  UPDATE(m_pcBatteryEquippedEntity);
471  }
472 
473  /****************************************/
474  /****************************************/
475 
476  REGISTER_ENTITY(CFootBotEntity,
477  "foot-bot",
478  "Carlo Pinciroli [ilpincy@gmail.com]",
479  "1.0",
480  "The foot-bot robot, developed in the Swarmanoid project.",
481  "The foot-bot is a wheeled robot developed in the Swarmanoid Project. It is a\n"
482  "modular robot with a rich set of sensors and actuators. For more information,\n"
483  "refer to the dedicated web page\n"
484  "(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
485  "REQUIRED XML CONFIGURATION\n\n"
486  " <arena ...>\n"
487  " ...\n"
488  " <foot-bot id=\"fb0\">\n"
489  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
490  " <controller config=\"mycntrl\" />\n"
491  " </foot-bot>\n"
492  " ...\n"
493  " </arena>\n\n"
494  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
495  "entities share the same id, initialization aborts.\n"
496  "The 'body/position' attribute specifies the position of the bottom point of the\n"
497  "foot-bot in the arena. When the robot is untranslated and unrotated, the\n"
498  "bottom point is in the origin and it is defined as the middle point between\n"
499  "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
500  "axis, that is the point where the wheels touch the floor. The attribute values\n"
501  "are in the X,Y,Z order.\n"
502  "The 'body/orientation' attribute specifies the orientation of the foot-bot. All\n"
503  "rotations are performed with respect to the bottom point. The order of the\n"
504  "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
505  "around the Z axis, the second around Y and the last around X. This reflects\n"
506  "the internal convention used in ARGoS, in which rotations are performed in\n"
507  "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
508  "is oriented along the X axis.\n"
509  "The 'controller/config' attribute is used to assign a controller to the\n"
510  "foot-bot. The value of the attribute must be set to the id of a previously\n"
511  "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
512  "OPTIONAL XML CONFIGURATION\n\n"
513  "You can set the emission range of the range-and-bearing system. By default, a\n"
514  "message sent by a foot-bot can be received up to 3m. By using the 'rab_range'\n"
515  "attribute, you can change it to, i.e., 4m as follows:\n\n"
516  " <arena ...>\n"
517  " ...\n"
518  " <foot-bot id=\"fb0\" rab_range=\"4\">\n"
519  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
520  " <controller config=\"mycntrl\" />\n"
521  " </foot-bot>\n"
522  " ...\n"
523  " </arena>\n\n"
524  "You can also set the data sent at each time step through the range-and-bearing\n"
525  "system. By default, a message sent by a foot-bot is 10 bytes long. By using the\n"
526  "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
527  " <arena ...>\n"
528  " ...\n"
529  " <foot-bot id=\"fb0\" rab_data_size=\"20\">\n"
530  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
531  " <controller config=\"mycntrl\" />\n"
532  " </foot-bot>\n"
533  " ...\n"
534  " </arena>\n\n"
535  "You can also configure the battery of the robot. By default, the battery never\n"
536  "depletes. You can choose among several battery discharge models, such as\n"
537  "- time: the battery depletes by a fixed amount at each time step\n"
538  "- motion: the battery depletes according to how the robot moves\n"
539  "- time_motion: a combination of the above models.\n"
540  "You can define your own models too. Follow the examples in the file\n"
541  "argos3/src/plugins/simulator/entities/battery_equipped_entity.cpp.\n\n"
542  " <arena ...>\n"
543  " ...\n"
544  " <foot-bot id=\"fb0\"\n"
545  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
546  " <controller config=\"mycntrl\" />\n"
547  " <battery model=\"time\" factor=\"1e-5\"/>\n"
548  " </foot-bot>\n"
549  " ...\n"
550  " </arena>\n\n"
551  " <arena ...>\n"
552  " ...\n"
553  " <foot-bot id=\"fb0\"\n"
554  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
555  " <controller config=\"mycntrl\" />\n"
556  " <battery model=\"motion\" pos_factor=\"1e-3\"\n"
557  " orient_factor=\"1e-3\"/>\n"
558  " </foot-bot>\n"
559  " ...\n"
560  " </arena>\n\n"
561  " <arena ...>\n"
562  " ...\n"
563  " <foot-bot id=\"fb0\"\n"
564  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
565  " <controller config=\"mycntrl\" />\n"
566  " <battery model=\"time_motion\" time_factor=\"1e-5\"\n"
567  " pos_factor=\"1e-3\"\n"
568  " orient_factor=\"1e-3\"/>\n"
569  " </foot-bot>\n"
570  " ...\n"
571  " </arena>\n\n"
572  "You can also change the aperture of the omnidirectional camera. The aperture is\n"
573  "set to 70 degrees by default. The tip of the omnidirectional camera is placed on\n"
574  "top of the robot (h=0.289), and with an aperture of 70 degrees the range on the\n"
575  "ground is r=h*tan(aperture)=0.289*tan(70)=0.794m. To change the aperture to 80\n"
576  "degrees, use the 'omnidirectional_camera_aperture' as follows:\n\n"
577  " <arena ...>\n"
578  " ...\n"
579  " <foot-bot id=\"fb0\" omnidirectional_camera_aperture=\"80\">\n"
580  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
581  " <controller config=\"mycntrl\" />\n"
582  " </foot-bot>\n"
583  " ...\n"
584  " </arena>\n\n"
585  "Finally, you can change the parameters of the perspective camera. You can set\n"
586  "its direction, aperture, focal length, and range with the attributes\n"
587  "'perspective_camera_front', 'perspective_camera_aperture',\n"
588  "'perspective_camera_focal_length', and 'perspective_camera_range', respectively.\n"
589  "The default values are: 'true' for front direction, 30 degrees for aperture,\n"
590  "0.035 for focal length, and 2 meters for range. When the direction is set to\n"
591  "'false', the camera looks up. This can be useful to see the eye-bot LEDs. Check\n"
592  "the following example:\n\n"
593  " <arena ...>\n"
594  " ...\n"
595  " <foot-bot id=\"fb0\"\n"
596  " perspective_camera_front=\"false\"\n"
597  " perspective_camera_aperture=\"45\"\n"
598  " perspective_camera_focal_length=\"0.07\"\n"
599  " perspective_camera_range=\"10\">\n"
600  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
601  " <controller config=\"mycntrl\" />\n"
602  " </foot-bot>\n"
603  " ...\n"
604  " </arena>\n\n"
605  ,
606  "Under development"
607  );
608 
609  /****************************************/
610  /****************************************/
611 
612  REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CFootBotEntity);
613 
614  /****************************************/
615  /****************************************/
616 
617 }
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
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
ARGOS_PI
#define ARGOS_PI
To be used when initializing static variables.
Definition: angles.h:32
UPDATE
#define UPDATE(COMPONENT)
Definition: footbot_entity.cpp:461
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::GetNode
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
Definition: argos_configuration.h:63
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::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::NodeExists
bool NodeExists(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns true if one of its child nodes has the wanted name.
Definition: argos_configuration.h:44
argos::ToRadians
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
argos::REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
argos::REGISTER_ENTITY
REGISTER_ENTITY(CFloorEntity, "floor", "Carlo Pinciroli [ilpincy@gmail.com]", "1.0", "It contains the properties of the arena floor.", "The floor entity contains the properties of the arena floor. In the current\n" "implementation, it contains only the color of the floor. The floor color is\n" "detected by the robots' ground sensors.\n\n" "REQUIRED XML CONFIGURATION\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"SOURCE\" />\n" " ...\n" " </arena>\n\n" "The 'id' attribute is necessary and must be unique among the entities. If two\n" "entities share the same id, initialization aborts.\n" "The 'source' attribute specifies where to get the color of the floor from. Its\n" "value, here denoted as SOURCE, can assume the following values:\n\n" " image The color is calculated from the passed image file\n" " loop_functions The color is calculated calling the loop functions\n\n" "When 'source' is set to 'image', as showed in the following example, you have\n" "to specify the image path in the additional attribute 'path':\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"image\"\n" " path=\"/path/to/imagefile.ext\" />\n" " ...\n" " </arena>\n\n" "Many image formats are available, such as PNG, JPG, BMP, GIF and many more.\n" "Refer to the FreeImage webpage for a complete list of supported image formats\n" "(http://freeimage.sourceforge.net/features.html).\n\n" "When 'source' is set to 'loop_functions', as showed in the following example,\n" "an image is implicitly created to be used as texture for graphical\n" "visualizations. The algorithm that creates the texture needs to convert from\n" "meters (in the arena) to pixels (of the texture). You control how many pixels\n" "per meter are used with the attribute 'pixels_per_meter'. Clearly, the higher\n" "value, the higher the quality, but also the slower the algorithm and the bigger\n" "the texture. The algorithm is called only once at init time, so the fact that\n" "it is slow is not so important. However, the image size is limited by OpenGL.\n" "Every implementation has its own limit, and you should check yours if any\n" "texture-related problem arises. Now for the example:\n\n" " <arena ...>\n" " ...\n" " <floor id=\"floor\"\n" " source=\"loop_functions\"\n" " pixels_per_meter=\"100\" />\n" " ...\n" " </arena>\n\n" "OPTIONAL XML CONFIGURATION\n\n" "None for the time being.\n", "Usable")
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CComposableEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
Definition: composable_entity.cpp:29
argos::CComposableEntity::AddComponent
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
Definition: composable_entity.cpp:72
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CControllableEntity::SetController
void SetController(const std::string &str_controller_id)
Creates and assigns a controller with the given id.
Definition: controllable_entity.cpp:139
argos::CControllableEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: controllable_entity.cpp:44
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::CEmbodiedEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: embodied_entity.cpp:68
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::CEmbodiedEntity::AddAnchor
SAnchor & AddAnchor(const std::string &str_id, const CVector3 &c_rel_position=CVector3(), const CQuaternion &c_rel_orientation=CQuaternion())
Adds an anchor to the embodied entity.
Definition: embodied_entity.cpp:121
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CRadians::PI_OVER_TWO
static const CRadians PI_OVER_TWO
Set to PI / 2.
Definition: angles.h:59
argos::CRadians::ZERO
static const CRadians ZERO
Set to zero radians.
Definition: angles.h:79
argos::CDegrees
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
argos::CDegrees::SetValue
void SetValue(Real f_value)
Sets the value in degrees.
Definition: angles.h:338
argos::CVector2
A 2D vector class.
Definition: vector2.h:27
argos::CVector3
A 3D vector class.
Definition: vector3.h:31
argos::CVector3::Y
static const CVector3 Y
The y axis.
Definition: vector3.h:39
argos::CVector3::X
static const CVector3 X
The x axis.
Definition: vector3.h:36
argos::CFootBotEntity::UpdateComponents
virtual void UpdateComponents()
Calls the Update() method on all the components.
Definition: footbot_entity.cpp:463
argos::CFootBotEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: footbot_entity.cpp:269
argos::CFootBotEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
Definition: footbot_entity.cpp:451
argos::CBatteryEquippedEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: battery_equipped_entity.cpp:68
argos::CGripperEquippedEntity
An entity that stores the state of a robot gripper.
Definition: gripper_equipped_entity.h:37
argos::CGroundSensorEquippedEntity::AddSensor
void AddSensor(const CVector2 &c_offset, ESensorType e_type, SAnchor &s_anchor)
Definition: ground_sensor_equipped_entity.cpp:130
argos::CLEDEquippedEntity
A container of CLEDEntity.
Definition: led_equipped_entity.h:36
argos::CLEDEquippedEntity::AddLEDRing
void AddLEDRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, UInt32 un_num_leds, SAnchor &s_anchor, const CColor &c_color=CColor::BLACK)
Adds a ring of LEDs to this entity.
Definition: led_equipped_entity.cpp:148
argos::CLEDEquippedEntity::AddLED
void AddLED(const CVector3 &c_offset, SAnchor &s_anchor, const CColor &c_color=CColor::BLACK)
Adds an LED to this entity.
Definition: led_equipped_entity.cpp:132
argos::CLightSensorEquippedEntity::AddSensorRing
void AddSensorRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, Real f_range, UInt32 un_num_sensors, SAnchor &s_anchor)
Definition: light_sensor_equipped_entity.cpp:154
argos::CProximitySensorEquippedEntity::AddSensorRing
void AddSensorRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, Real f_range, UInt32 un_num_sensors, SAnchor &s_anchor)
Definition: proximity_sensor_equipped_entity.cpp:154
argos::CWheeledEntity::SetWheel
void SetWheel(UInt32 un_index, const CVector3 &c_position, Real f_radius)
Definition: wheeled_entity.cpp:62