ARGoS 3
A parallel, multi-engine simulator for swarm robotics
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
26namespace 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}
UPDATE
#define UPDATE(COMPONENT)
Definition epuck_entity.cpp:336
ARGOS_PI
#define ARGOS_PI
To be used when initializing static variables.
Definition angles.h:32
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition datatypes.h:97
Real
float Real
Collects all ARGoS code.
Definition datatypes.h:39
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
REGISTER_ENTITY
#define REGISTER_ENTITY(CLASSNAME, LABEL, AUTHOR, VERSION, BRIEF_DESCRIPTION, LONG_DESCRIPTION, STATUS)
Definition entity.h:432
argos
The namespace containing all the ARGoS related code.
Definition ci_actuator.h:12
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::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::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::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::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition embodied_entity.h:119
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::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