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/ground_sensor_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
16 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
17 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
24 static const Real BODY_RADIUS = 0.035f;
25 static const Real BODY_HEIGHT = 0.086f;
27 static const Real INTERWHEEL_DISTANCE = 0.053f;
28 static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
29 static const Real WHEEL_RADIUS = 0.0205f;
31 static const Real PROXIMITY_SENSOR_RING_ELEVATION = 0.06f;
32 static const Real PROXIMITY_SENSOR_RING_RADIUS = BODY_RADIUS;
33 static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((2 *
ARGOS_PI / 8.0f) * 0.5f);
34 static const Real PROXIMITY_SENSOR_RING_RANGE = 0.1f;
36 static const CRadians LED_RING_START_ANGLE = CRadians((
ARGOS_PI / 8.0f) * 0.5f);
37 static const Real LED_RING_RADIUS = BODY_RADIUS + 0.007;
38 static const Real LED_RING_ELEVATION = 0.086f;
39 static const Real RAB_ELEVATION = LED_RING_ELEVATION;
46 m_pcControllableEntity(NULL),
47 m_pcEmbodiedEntity(NULL),
48 m_pcGroundSensorEquippedEntity(NULL),
49 m_pcLEDEquippedEntity(NULL),
50 m_pcLightSensorEquippedEntity(NULL),
51 m_pcProximitySensorEquippedEntity(NULL),
52 m_pcRABEquippedEntity(NULL),
53 m_pcWheeledEntity(NULL) {
60 const std::string& str_controller_id,
64 size_t un_rab_data_size) :
66 m_pcControllableEntity(NULL),
67 m_pcEmbodiedEntity(NULL),
68 m_pcGroundSensorEquippedEntity(NULL),
69 m_pcLEDEquippedEntity(NULL),
70 m_pcLightSensorEquippedEntity(NULL),
71 m_pcProximitySensorEquippedEntity(NULL),
72 m_pcRABEquippedEntity(NULL),
73 m_pcWheeledEntity(NULL) {
79 m_pcEmbodiedEntity =
new CEmbodiedEntity(
this,
"body_0", c_position, c_orientation);
84 m_pcWheeledEntity->
SetWheel(0,
CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
85 m_pcWheeledEntity->
SetWheel(1,
CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
90 CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
96 m_pcProximitySensorEquippedEntity =
101 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
102 PROXIMITY_SENSOR_RING_RADIUS,
103 PROXIMITY_SENSOR_RING_START_ANGLE,
104 PROXIMITY_SENSOR_RING_RANGE,
108 m_pcLightSensorEquippedEntity =
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_pcGroundSensorEquippedEntity =
140 CVector3(0.0f, 0.0f, RAB_ELEVATION));
174 m_pcWheeledEntity->
SetWheel(0,
CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
175 m_pcWheeledEntity->
SetWheel(1,
CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
180 CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
182 LED_RING_START_ANGLE,
186 m_pcProximitySensorEquippedEntity =
191 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
192 PROXIMITY_SENSOR_RING_RADIUS,
193 PROXIMITY_SENSOR_RING_START_ANGLE,
194 PROXIMITY_SENSOR_RING_RANGE,
198 m_pcLightSensorEquippedEntity =
203 CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
204 PROXIMITY_SENSOR_RING_RADIUS,
205 PROXIMITY_SENSOR_RING_START_ANGLE,
206 PROXIMITY_SENSOR_RING_RANGE,
210 m_pcGroundSensorEquippedEntity =
234 CVector3(0.0f, 0.0f, RAB_ELEVATION));
240 m_pcControllableEntity->
Init(
GetNode(t_tree,
"controller"));
269 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
272 UPDATE(m_pcRABEquippedEntity);
273 UPDATE(m_pcLEDEquippedEntity);
281 "Carlo Pinciroli [ilpincy@gmail.com]",
284 "The e-puck is a open-hardware, extensible robot intended for education. In its\n"
285 "simplest form, it is a two-wheeled robot equipped with proximity sensors,\n"
286 "ground sensors, light sensors, a microphone, a frontal camera, and a ring of\n"
287 "red LEDs. More information is available at http://www.epuck.org\n\n"
288 "REQUIRED XML CONFIGURATION\n\n"
291 " <e-puck id=\"eb0\">\n"
292 " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
293 " <controller config=\"mycntrl\" />\n"
297 "The 'id' attribute is necessary and must be unique among the entities. If two\n"
298 "entities share the same id, initialization aborts.\n"
299 "The 'body/position' attribute specifies the position of the pucktom point of the\n"
300 "e-puck in the arena. When the robot is untranslated and unrotated, the\n"
301 "pucktom point is in the origin and it is defined as the middle point between\n"
302 "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
303 "axis, that is the point where the wheels touch the floor. The attribute values\n"
304 "are in the X,Y,Z order.\n"
305 "The 'body/orientation' attribute specifies the orientation of the e-puck. All\n"
306 "rotations are performed with respect to the pucktom point. The order of the\n"
307 "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
308 "around the Z axis, the second around Y and the last around X. This reflects\n"
309 "the internal convention used in ARGoS, in which rotations are performed in\n"
310 "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
311 "is oriented along the X axis.\n"
312 "The 'controller/config' attribute is used to assign a controller to the\n"
313 "e-puck. The value of the attribute must be set to the id of a previously\n"
314 "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
315 "OPTIONAL XML CONFIGURATION\n\n"
316 "You can set the emission range of the range-and-bearing system. By default, a\n"
317 "message sent by an e-puck can be received up to 80cm. By using the 'rab_range'\n"
318 "attribute, you can change it to, i.e., 4m as follows:\n\n"
321 " <e-puck id=\"eb0\" rab_range=\"4\">\n"
322 " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
323 " <controller config=\"mycntrl\" />\n"
327 "You can also set the data sent at each time step through the range-and-bearing"
328 "system. By default, a message sent by an e-puck is 2 bytes long. By using the"
329 "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
332 " <e-puck id=\"eb0\" rab_data_size=\"20\">\n"
333 " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
334 " <controller config=\"mycntrl\" />\n"