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/rab_equipped_entity.h>
14 #include <argos3/plugins/simulator/entities/gripper_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
16 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
17 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
18 #include <argos3/plugins/simulator/entities/omnidirectional_camera_equipped_entity.h>
19 #include <argos3/plugins/simulator/entities/perspective_camera_equipped_entity.h>
20 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
21 #include <argos3/plugins/simulator/entities/wifi_equipped_entity.h>
30 static const Real BODY_RADIUS = 0.085036758f;
31 static const Real BODY_HEIGHT = 0.146899733f;
33 static const Real LED_RING_RADIUS = BODY_RADIUS + 0.005;
35 static const Real INTERWHEEL_DISTANCE = 0.14f;
36 static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
37 static const Real WHEEL_RADIUS = 0.029112741f;
39 static const Real PROXIMITY_SENSOR_RING_ELEVATION = 0.06f;
40 static const Real PROXIMITY_SENSOR_RING_RADIUS = BODY_RADIUS;
41 static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((
ARGOS_PI / 12.0f) * 0.5f);
42 static const Real PROXIMITY_SENSOR_RING_RANGE = 0.1f;
44 static const Real LED_RING_ELEVATION = 0.085f;
45 static const Real RAB_ELEVATION = 0.1f;
46 static const Real BEACON_ELEVATION = 0.174249733f;
48 static const Real GRIPPER_ELEVATION = LED_RING_ELEVATION;
50 static const CRadians LED_ANGLE_SLICE = CRadians(
ARGOS_PI / 6.0);
51 static const CRadians HALF_LED_ANGLE_SLICE = LED_ANGLE_SLICE * 0.5f;
53 static const Real OMNIDIRECTIONAL_CAMERA_ELEVATION = 0.288699733f;
60 m_pcControllableEntity(NULL),
61 m_pcDistanceScannerEquippedEntity(NULL),
62 m_pcTurretEntity(NULL),
63 m_pcEmbodiedEntity(NULL),
64 m_pcGripperEquippedEntity(NULL),
65 m_pcGroundSensorEquippedEntity(NULL),
66 m_pcLEDEquippedEntity(NULL),
67 m_pcLightSensorEquippedEntity(NULL),
68 m_pcOmnidirectionalCameraEquippedEntity(NULL),
69 m_pcPerspectiveCameraEquippedEntity(NULL),
70 m_pcProximitySensorEquippedEntity(NULL),
71 m_pcRABEquippedEntity(NULL),
72 m_pcWheeledEntity(NULL),
73 m_pcWiFiEquippedEntity(NULL) {
80 const std::string& str_controller_id,
84 size_t un_rab_data_size,
86 bool b_perspcam_front,
88 Real f_perspcam_focal_length,
89 Real f_perspcam_range) :
91 m_pcControllableEntity(NULL),
92 m_pcDistanceScannerEquippedEntity(NULL),
93 m_pcTurretEntity(NULL),
94 m_pcEmbodiedEntity(NULL),
95 m_pcGripperEquippedEntity(NULL),
96 m_pcGroundSensorEquippedEntity(NULL),
97 m_pcLEDEquippedEntity(NULL),
98 m_pcLightSensorEquippedEntity(NULL),
99 m_pcOmnidirectionalCameraEquippedEntity(NULL),
100 m_pcPerspectiveCameraEquippedEntity(NULL),
101 m_pcProximitySensorEquippedEntity(NULL),
102 m_pcRABEquippedEntity(NULL),
103 m_pcWheeledEntity(NULL),
104 m_pcWiFiEquippedEntity(NULL) {
113 m_pcEmbodiedEntity =
new CEmbodiedEntity(
this,
"body_0", c_position, c_orientation);
118 SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->
AddAnchor(
"perspective_camera",
119 CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
124 m_pcWheeledEntity->
SetWheel(0,
CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
125 m_pcWheeledEntity->
SetWheel(1,
CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
130 CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
132 HALF_LED_ANGLE_SLICE,
135 m_pcLEDEquippedEntity->
AddLED(
136 CVector3(0.0f, 0.0f, BEACON_ELEVATION),
139 m_pcProximitySensorEquippedEntity =
143 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
144 PROXIMITY_SENSOR_RING_RADIUS,
145 PROXIMITY_SENSOR_RING_START_ANGLE,
146 PROXIMITY_SENSOR_RING_RANGE,
150 m_pcLightSensorEquippedEntity =
154 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
155 PROXIMITY_SENSOR_RING_RADIUS,
156 PROXIMITY_SENSOR_RING_START_ANGLE,
157 PROXIMITY_SENSOR_RING_RANGE,
161 m_pcGripperEquippedEntity =
164 CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
168 m_pcGroundSensorEquippedEntity =
208 m_pcDistanceScannerEquippedEntity =
212 m_pcRABEquippedEntity =
219 CVector3(0.0f, 0.0f, RAB_ELEVATION));
222 m_pcOmnidirectionalCameraEquippedEntity =
224 "omnidirectional_camera_0",
228 OMNIDIRECTIONAL_CAMERA_ELEVATION));
231 m_pcPerspectiveCameraEquippedEntity =
233 "perspective_camera_0",
235 f_perspcam_focal_length,
282 m_pcWheeledEntity->
SetWheel(0,
CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
283 m_pcWheeledEntity->
SetWheel(1,
CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
288 CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
290 HALF_LED_ANGLE_SLICE,
293 m_pcLEDEquippedEntity->
AddLED(
294 CVector3(0.0f, 0.0f, BEACON_ELEVATION),
297 m_pcProximitySensorEquippedEntity =
301 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
302 PROXIMITY_SENSOR_RING_RADIUS,
303 PROXIMITY_SENSOR_RING_START_ANGLE,
304 PROXIMITY_SENSOR_RING_RANGE,
308 m_pcLightSensorEquippedEntity =
312 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
313 PROXIMITY_SENSOR_RING_RADIUS,
314 PROXIMITY_SENSOR_RING_START_ANGLE,
315 PROXIMITY_SENSOR_RING_RANGE,
319 m_pcGripperEquippedEntity =
322 CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
326 m_pcGroundSensorEquippedEntity =
366 m_pcDistanceScannerEquippedEntity =
374 m_pcRABEquippedEntity =
381 CVector3(0.0f, 0.0f, RAB_ELEVATION));
386 m_pcOmnidirectionalCameraEquippedEntity =
388 "omnidirectional_camera_0",
392 OMNIDIRECTIONAL_CAMERA_ELEVATION));
395 bool bPerspCamFront =
true;
397 Real fPerspCamFocalLength = 0.035;
399 Real fPerspCamRange = 2.0;
405 SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->
AddAnchor(
"perspective_camera",
406 CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
408 m_pcPerspectiveCameraEquippedEntity =
410 "perspective_camera_0",
412 fPerspCamFocalLength,
427 m_pcControllableEntity->
Init(
GetNode(t_tree,
"controller"));
449 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
453 UPDATE(m_pcDistanceScannerEquippedEntity);
455 UPDATE(m_pcGripperEquippedEntity);
456 UPDATE(m_pcRABEquippedEntity);
457 UPDATE(m_pcLEDEquippedEntity);
465 "Carlo Pinciroli [ilpincy@gmail.com]",
467 "The foot-bot robot, developed in the Swarmanoid project.",
468 "The foot-bot is a wheeled robot developed in the Swarmanoid Project. It is a\n"
469 "modular robot with a rich set of sensors and actuators. For more information,\n"
470 "refer to the dedicated web page\n"
471 "(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
472 "REQUIRED XML CONFIGURATION\n\n"
475 " <foot-bot id=\"fb0\">\n"
476 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
477 " <controller config=\"mycntrl\" />\n"
481 "The 'id' attribute is necessary and must be unique among the entities. If two\n"
482 "entities share the same id, initialization aborts.\n"
483 "The 'body/position' attribute specifies the position of the bottom point of the\n"
484 "foot-bot in the arena. When the robot is untranslated and unrotated, the\n"
485 "bottom point is in the origin and it is defined as the middle point between\n"
486 "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
487 "axis, that is the point where the wheels touch the floor. The attribute values\n"
488 "are in the X,Y,Z order.\n"
489 "The 'body/orientation' attribute specifies the orientation of the foot-bot. All\n"
490 "rotations are performed with respect to the bottom point. The order of the\n"
491 "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
492 "around the Z axis, the second around Y and the last around X. This reflects\n"
493 "the internal convention used in ARGoS, in which rotations are performed in\n"
494 "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
495 "is oriented along the X axis.\n"
496 "The 'controller/config' attribute is used to assign a controller to the\n"
497 "foot-bot. The value of the attribute must be set to the id of a previously\n"
498 "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
499 "OPTIONAL XML CONFIGURATION\n\n"
500 "You can set the emission range of the range-and-bearing system. By default, a\n"
501 "message sent by a foot-bot can be received up to 3m. By using the 'rab_range'\n"
502 "attribute, you can change it to, i.e., 4m as follows:\n\n"
505 " <foot-bot id=\"fb0\" rab_range=\"4\">\n"
506 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
507 " <controller config=\"mycntrl\" />\n"
511 "You can also set the data sent at each time step through the range-and-bearing"
512 "system. By default, a message sent by a foot-bot is 10 bytes long. By using the"
513 "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
516 " <foot-bot id=\"fb0\" rab_data_size=\"20\">\n"
517 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
518 " <controller config=\"mycntrl\" />\n"
522 "You can also change the aperture of the omnidirectional camera. The aperture is\n"
523 "set to 70 degrees by default. The tip of the omnidirectional camera is placed on\n"
524 "top of the robot (h=0.289), and with an aperture of 70 degrees the range on the\n"
525 "ground is r=h*tan(aperture)=0.289*tan(70)=0.794m. To change the aperture to 80\n"
526 "degrees, use the 'omnidirectional_camera_aperture' as follows:\n\n"
529 " <foot-bot id=\"fb0\" omnidirectional_camera_aperture=\"80\">\n"
530 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
531 " <controller config=\"mycntrl\" />\n"
535 "Finally, you can change the parameters of the perspective camera. You can set\n"
536 "its direction, aperture, focal length, and range with the attributes\n"
537 "'perspective_camera_front', 'perspective_camera_aperture',\n"
538 "'perspective_camera_focal_length', and 'perspective_camera_range', respectively.\n"
539 "The default values are: 'true' for front direction, 30 degrees for aperture,\n"
540 "0.035 for focal length, and 2 meters for range. When the direction is set to\n"
541 "'false', the camera looks up. This can be useful to see the eye-bot LEDs. Check\n"
542 "the following example:\n\n"
545 " <foot-bot id=\"fb0\"\n"
546 " perspective_camera_front=\"false\"\n"
547 " perspective_camera_aperture=\"45\"\n"
548 " perspective_camera_focal_length=\"0.07\"\n"
549 " perspective_camera_range=\"10\">\n"
550 " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
551 " <controller config=\"mycntrl\" />\n"