9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/core/simulator/entity/controllable_entity.h>
11 #include <argos3/core/simulator/entity/embodied_entity.h>
12 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
13 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
14 #include <argos3/plugins/simulator/entities/perspective_camera_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
16 #include <argos3/plugins/simulator/entities/quadrotor_entity.h>
17 #include <argos3/plugins/simulator/entities/rab_equipped_entity.h>
24 static const Real BODY_HEIGHT = 0.566f;
25 static const Real BODY_RADIUS = 0.25f;
26 static const Real LED_RING_RADIUS = BODY_RADIUS + 0.005;
27 static const Real LED_LOWER_RING_ELEVATION = 0.1535f;
28 static const Real LED_UPPER_RING_ELEVATION = 0.1635f;
29 static const CRadians LED_ANGLE_SLICE = CRadians(3.14159265358979323846264338327950288 / 8.0);
30 static const CRadians HALF_LED_ANGLE_SLICE = LED_ANGLE_SLICE * 0.5f;
31 static const Real PROXIMITY_SENSOR_RING_ELEVATION = LED_UPPER_RING_ELEVATION;
32 static const Real PROXIMITY_SENSOR_RING_RADIUS = LED_RING_RADIUS;
33 static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((
ARGOS_PI / 12.0f) * 0.5f);
34 static const Real PROXIMITY_SENSOR_RING_RANGE = 3.0f;
41 m_pcControllableEntity(NULL),
42 m_pcEmbodiedEntity(NULL),
43 m_pcLEDEquippedEntity(NULL),
44 m_pcLightSensorEquippedEntity(NULL),
45 m_pcPerspectiveCameraEquippedEntity(NULL),
46 m_pcProximitySensorEquippedEntity(NULL),
47 m_pcQuadRotorEntity(NULL),
48 m_pcRABEquippedEntity(NULL) {
55 const std::string& str_controller_id,
59 size_t un_rab_data_size,
61 Real f_perspcam_focal_length,
62 Real f_perspcam_range) :
64 m_pcControllableEntity(NULL),
65 m_pcEmbodiedEntity(NULL),
66 m_pcLEDEquippedEntity(NULL),
67 m_pcLightSensorEquippedEntity(NULL),
68 m_pcPerspectiveCameraEquippedEntity(NULL),
69 m_pcProximitySensorEquippedEntity(NULL),
70 m_pcQuadRotorEntity(NULL),
71 m_pcRABEquippedEntity(NULL) {
80 m_pcEmbodiedEntity =
new CEmbodiedEntity(
this,
"body_0", c_position, c_orientation);
88 CVector3(0.0f, 0.0f, LED_LOWER_RING_ELEVATION),
94 CVector3(0.0f, 0.0f, LED_UPPER_RING_ELEVATION),
99 CVector3 cLEDPos(LED_RING_RADIUS * 0.7f,
101 LED_LOWER_RING_ELEVATION - 0.01f);
103 m_pcLEDEquippedEntity->
AddLED(
108 m_pcProximitySensorEquippedEntity =
113 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
114 PROXIMITY_SENSOR_RING_RADIUS,
115 PROXIMITY_SENSOR_RING_START_ANGLE,
116 PROXIMITY_SENSOR_RING_RANGE,
120 m_pcLightSensorEquippedEntity =
125 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
126 PROXIMITY_SENSOR_RING_RADIUS,
127 PROXIMITY_SENSOR_RING_START_ANGLE,
128 PROXIMITY_SENSOR_RING_RANGE,
138 *m_pcEmbodiedEntity);
142 SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->
AddAnchor(
"perspective_camera",
145 m_pcPerspectiveCameraEquippedEntity =
147 "perspective_camera_0",
149 f_perspcam_focal_length,
192 CVector3(0.0f, 0.0f, LED_LOWER_RING_ELEVATION),
194 HALF_LED_ANGLE_SLICE,
198 CVector3(0.0f, 0.0f, LED_UPPER_RING_ELEVATION),
200 HALF_LED_ANGLE_SLICE,
203 CVector3 cLEDPos(LED_RING_RADIUS * 0.7f,
205 LED_LOWER_RING_ELEVATION - 0.01f);
207 m_pcLEDEquippedEntity->
AddLED(
212 m_pcProximitySensorEquippedEntity =
217 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
218 PROXIMITY_SENSOR_RING_RADIUS,
219 PROXIMITY_SENSOR_RING_START_ANGLE,
220 PROXIMITY_SENSOR_RING_RANGE,
224 m_pcLightSensorEquippedEntity =
229 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
230 PROXIMITY_SENSOR_RING_RADIUS,
231 PROXIMITY_SENSOR_RING_START_ANGLE,
232 PROXIMITY_SENSOR_RING_RANGE,
246 *m_pcEmbodiedEntity);
249 bool bPerspCamFront =
true;
251 Real fPerspCamFocalLength = 0.035;
253 Real fPerspCamRange = 2.0;
258 SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->
AddAnchor(
"perspective_camera",
261 m_pcPerspectiveCameraEquippedEntity =
263 "perspective_camera_0",
265 fPerspCamFocalLength,
274 m_pcControllableEntity->
Init(
GetNode(t_tree,
"controller"));
296 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
299 UPDATE(m_pcRABEquippedEntity);
300 UPDATE(m_pcLEDEquippedEntity);
308 "Carlo Pinciroli [ilpincy@gmail.com]",
310 "The eye-bot robot, developed in the Swarmanoid project.",
311 "The eye-bot is a quad-rotor developed in the Swarmanoid Project. It is a\n"
312 "fully autonomous robot with a rich set of sensors and actuators. For more\n"
313 "information, refer to the dedicated web page\n"
314 "(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
315 "REQUIRED XML CONFIGURATION\n\n"
318 " <eye-bot id=\"eb0\">\n"
319 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
320 " <controller config=\"mycntrl\" />\n"
324 "The 'id' attribute is necessary and must be unique among the entities. If two\n"
325 "entities share the same id, initialization aborts.\n"
326 "The 'body/position' attribute specifies the position of the bottom point of the\n"
327 "eye-bot in the arena. When the robot is untranslated and unrotated, the\n"
328 "bottom point is in the origin and it is defined as the middle point between\n"
329 "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
330 "axis, that is the point where the wheels touch the floor. The attribute values\n"
331 "are in the X,Y,Z order.\n"
332 "The 'body/orientation' attribute specifies the orientation of the eye-bot. All\n"
333 "rotations are performed with respect to the bottom point. The order of the\n"
334 "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
335 "around the Z axis, the second around Y and the last around X. This reflects\n"
336 "the internal convention used in ARGoS, in which rotations are performed in\n"
337 "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
338 "is oriented along the X axis.\n"
339 "The 'controller/config' attribute is used to assign a controller to the\n"
340 "eye-bot. The value of the attribute must be set to the id of a previously\n"
341 "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
342 "OPTIONAL XML CONFIGURATION\n\n"
343 "You can set the emission range of the range-and-bearing system. By default, a\n"
344 "message sent by an eye-bot can be received up to 3m. By using the 'rab_range'\n"
345 "attribute, you can change it to, i.e., 4m as follows:\n\n"
348 " <eye-bot id=\"eb0\" rab_range=\"4\">\n"
349 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
350 " <controller config=\"mycntrl\" />\n"
354 "You can also set the data sent at each time step through the range-and-bearing\n"
355 "system. By default, a message sent by a eye-bot is 10 bytes long. By using the\n"
356 "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
359 " <eye-bot id=\"eb0\" rab_data_size=\"20\">\n"
360 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
361 " <controller config=\"mycntrl\" />\n"
365 "Finally, you can change the parameters of the camera. You can set its aperture,\n"
366 "focal length, and range with the attributes 'camera_aperture',\n"
367 "'camera_focal_length', and 'camera_range', respectively. The default values are:\n"
368 "30 degrees for aperture, 0.035 for focal length, and 2 meters for range. Check\n"
369 "the following example:\n\n"
372 " <eye-bot id=\"eb0\"\n"
373 " camera_aperture=\"45\"\n"
374 " camera_focal_length=\"0.07\"\n"
375 " camera_range=\"10\">\n"
376 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
377 " <controller config=\"mycntrl\" />\n"