epuck_entity.cpp
Go to the documentation of this file.
1 
7 #include "epuck_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/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>
18 #include <argos3/plugins/simulator/entities/battery_equipped_entity.h>
19 
20 namespace argos {
21 
22  /****************************************/
23  /****************************************/
24 
25  static const Real BODY_RADIUS = 0.035f;
26  static const Real BODY_HEIGHT = 0.086f;
27 
28  static const Real INTERWHEEL_DISTANCE = 0.053f;
29  static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
30  static const Real WHEEL_RADIUS = 0.0205f;
31 
32  static const Real PROXIMITY_SENSOR_RING_ELEVATION = 0.06f;
33  static const Real PROXIMITY_SENSOR_RING_RADIUS = BODY_RADIUS;
34  static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((2 * ARGOS_PI / 8.0f) * 0.5f);
35  static const Real PROXIMITY_SENSOR_RING_RANGE = 0.1f;
36 
37  static const CRadians LED_RING_START_ANGLE = CRadians((ARGOS_PI / 8.0f) * 0.5f);
38  static const Real LED_RING_RADIUS = BODY_RADIUS + 0.007;
39  static const Real LED_RING_ELEVATION = 0.086f;
40  static const Real RAB_ELEVATION = LED_RING_ELEVATION;
41 
42  /****************************************/
43  /****************************************/
44 
46  CComposableEntity(NULL),
47  m_pcControllableEntity(NULL),
48  m_pcEmbodiedEntity(NULL),
49  m_pcGroundSensorEquippedEntity(NULL),
50  m_pcLEDEquippedEntity(NULL),
51  m_pcLightSensorEquippedEntity(NULL),
52  m_pcProximitySensorEquippedEntity(NULL),
53  m_pcRABEquippedEntity(NULL),
54  m_pcWheeledEntity(NULL),
55  m_pcBatteryEquippedEntity(NULL) {
56  }
57 
58  /****************************************/
59  /****************************************/
60 
61  CEPuckEntity::CEPuckEntity(const std::string& str_id,
62  const std::string& str_controller_id,
63  const CVector3& c_position,
64  const CQuaternion& c_orientation,
65  Real f_rab_range,
66  size_t un_rab_data_size,
67  const std::string& str_bat_model) :
68  CComposableEntity(NULL, str_id),
69  m_pcControllableEntity(NULL),
70  m_pcEmbodiedEntity(NULL),
71  m_pcGroundSensorEquippedEntity(NULL),
72  m_pcLEDEquippedEntity(NULL),
73  m_pcLightSensorEquippedEntity(NULL),
74  m_pcProximitySensorEquippedEntity(NULL),
75  m_pcRABEquippedEntity(NULL),
76  m_pcWheeledEntity(NULL),
77  m_pcBatteryEquippedEntity(NULL) {
78  try {
79  /*
80  * Create and init components
81  */
82  /* Embodied entity */
83  m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
84  AddComponent(*m_pcEmbodiedEntity);
85  /* Wheeled entity and wheel positions (left, right) */
86  m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
87  AddComponent(*m_pcWheeledEntity);
88  m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
89  m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
90  /* LED equipped entity */
91  m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
92  AddComponent(*m_pcLEDEquippedEntity);
93  m_pcLEDEquippedEntity->AddLEDRing(
94  CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
95  LED_RING_RADIUS,
96  LED_RING_START_ANGLE,
97  8,
98  m_pcEmbodiedEntity->GetOriginAnchor());
99  /* Proximity sensor equipped entity */
100  m_pcProximitySensorEquippedEntity =
102  "proximity_0");
103  AddComponent(*m_pcProximitySensorEquippedEntity);
104  m_pcProximitySensorEquippedEntity->AddSensorRing(
105  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
106  PROXIMITY_SENSOR_RING_RADIUS,
107  PROXIMITY_SENSOR_RING_START_ANGLE,
108  PROXIMITY_SENSOR_RING_RANGE,
109  8,
110  m_pcEmbodiedEntity->GetOriginAnchor());
111  /* Light sensor equipped entity */
112  m_pcLightSensorEquippedEntity =
114  "light_0");
115  AddComponent(*m_pcLightSensorEquippedEntity);
116  m_pcLightSensorEquippedEntity->AddSensorRing(
117  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
118  PROXIMITY_SENSOR_RING_RADIUS,
119  PROXIMITY_SENSOR_RING_START_ANGLE,
120  PROXIMITY_SENSOR_RING_RANGE,
121  8,
122  m_pcEmbodiedEntity->GetOriginAnchor());
123  /* Ground sensor equipped entity */
124  m_pcGroundSensorEquippedEntity =
126  "ground_0");
127  AddComponent(*m_pcGroundSensorEquippedEntity);
128  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, -0.009f),
130  m_pcEmbodiedEntity->GetOriginAnchor());
131  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, 0.0f),
133  m_pcEmbodiedEntity->GetOriginAnchor());
134  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, 0.009f),
136  m_pcEmbodiedEntity->GetOriginAnchor());
137  /* RAB equipped entity */
138  m_pcRABEquippedEntity = new CRABEquippedEntity(this,
139  "rab_0",
140  un_rab_data_size,
141  f_rab_range,
142  m_pcEmbodiedEntity->GetOriginAnchor(),
143  *m_pcEmbodiedEntity,
144  CVector3(0.0f, 0.0f, RAB_ELEVATION));
145  AddComponent(*m_pcRABEquippedEntity);
146  /* Battery equipped entity */
147  m_pcBatteryEquippedEntity = new CBatteryEquippedEntity(this, "battery_0", str_bat_model);
148  AddComponent(*m_pcBatteryEquippedEntity);
149  /* Controllable entity
150  It must be the last one, for actuators/sensors to link to composing entities correctly */
151  m_pcControllableEntity = new CControllableEntity(this, "controller_0");
152  AddComponent(*m_pcControllableEntity);
153  m_pcControllableEntity->SetController(str_controller_id);
154  /* Update components */
156  }
157  catch(CARGoSException& ex) {
158  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
159  }
160  }
161 
162  /****************************************/
163  /****************************************/
164 
166  try {
167  /*
168  * Init parent
169  */
170  CComposableEntity::Init(t_tree);
171  /*
172  * Create and init components
173  */
174  /* Embodied entity */
175  m_pcEmbodiedEntity = new CEmbodiedEntity(this);
176  AddComponent(*m_pcEmbodiedEntity);
177  m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
178  /* Wheeled entity and wheel positions (left, right) */
179  m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
180  AddComponent(*m_pcWheeledEntity);
181  m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
182  m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
183  /* LED equipped entity, with LEDs [0-11] and beacon [12] */
184  m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
185  AddComponent(*m_pcLEDEquippedEntity);
186  m_pcLEDEquippedEntity->AddLEDRing(
187  CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
188  LED_RING_RADIUS,
189  LED_RING_START_ANGLE,
190  8,
191  m_pcEmbodiedEntity->GetOriginAnchor());
192  /* Proximity sensor equipped entity */
193  m_pcProximitySensorEquippedEntity =
195  "proximity_0");
196  AddComponent(*m_pcProximitySensorEquippedEntity);
197  m_pcProximitySensorEquippedEntity->AddSensorRing(
198  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
199  PROXIMITY_SENSOR_RING_RADIUS,
200  PROXIMITY_SENSOR_RING_START_ANGLE,
201  PROXIMITY_SENSOR_RING_RANGE,
202  8,
203  m_pcEmbodiedEntity->GetOriginAnchor());
204  /* Light sensor equipped entity */
205  m_pcLightSensorEquippedEntity =
207  "light_0");
208  AddComponent(*m_pcLightSensorEquippedEntity);
209  m_pcLightSensorEquippedEntity->AddSensorRing(
210  CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
211  PROXIMITY_SENSOR_RING_RADIUS,
212  PROXIMITY_SENSOR_RING_START_ANGLE,
213  PROXIMITY_SENSOR_RING_RANGE,
214  8,
215  m_pcEmbodiedEntity->GetOriginAnchor());
216  /* Ground sensor equipped entity */
217  m_pcGroundSensorEquippedEntity =
219  "ground_0");
220  AddComponent(*m_pcGroundSensorEquippedEntity);
221  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, -0.009f),
223  m_pcEmbodiedEntity->GetOriginAnchor());
224  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, 0.0f),
226  m_pcEmbodiedEntity->GetOriginAnchor());
227  m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, 0.009f),
229  m_pcEmbodiedEntity->GetOriginAnchor());
230  /* RAB equipped entity */
231  Real fRange = 0.8f;
232  GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
233  UInt32 unDataSize = 2;
234  GetNodeAttributeOrDefault(t_tree, "rab_data_size", unDataSize, unDataSize);
235  m_pcRABEquippedEntity = new CRABEquippedEntity(this,
236  "rab_0",
237  unDataSize,
238  fRange,
239  m_pcEmbodiedEntity->GetOriginAnchor(),
240  *m_pcEmbodiedEntity,
241  CVector3(0.0f, 0.0f, RAB_ELEVATION));
242  AddComponent(*m_pcRABEquippedEntity);
243  /* Battery equipped entity */
244  m_pcBatteryEquippedEntity = new CBatteryEquippedEntity(this, "battery_0");
245  if(NodeExists(t_tree, "battery"))
246  m_pcBatteryEquippedEntity->Init(GetNode(t_tree, "battery"));
247  AddComponent(*m_pcBatteryEquippedEntity);
248  /* Controllable entity
249  It must be the last one, for actuators/sensors to link to composing entities correctly */
250  m_pcControllableEntity = new CControllableEntity(this);
251  AddComponent(*m_pcControllableEntity);
252  m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
253  /* Update components */
255  }
256  catch(CARGoSException& ex) {
257  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
258  }
259  }
260 
261  /****************************************/
262  /****************************************/
263 
265  /* Reset all components */
267  /* Update components */
269  }
270 
271  /****************************************/
272  /****************************************/
273 
276  }
277 
278  /****************************************/
279  /****************************************/
280 
281 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
282 
284  UPDATE(m_pcRABEquippedEntity);
285  UPDATE(m_pcLEDEquippedEntity);
286  UPDATE(m_pcBatteryEquippedEntity);
287  }
288 
289  /****************************************/
290  /****************************************/
291 
293  "e-puck",
294  "Carlo Pinciroli [ilpincy@gmail.com]",
295  "1.0",
296  "The e-puck robot.",
297  "The e-puck is a open-hardware, extensible robot intended for education. In its\n"
298  "simplest form, it is a two-wheeled robot equipped with proximity sensors,\n"
299  "ground sensors, light sensors, a microphone, a frontal camera, and a ring of\n"
300  "red LEDs. More information is available at http://www.epuck.org\n\n"
301  "REQUIRED XML CONFIGURATION\n\n"
302  " <arena ...>\n"
303  " ...\n"
304  " <e-puck id=\"eb0\">\n"
305  " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
306  " <controller config=\"mycntrl\" />\n"
307  " </e-puck>\n"
308  " ...\n"
309  " </arena>\n\n"
310  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
311  "entities share the same id, initialization aborts.\n"
312  "The 'body/position' attribute specifies the position of the pucktom point of the\n"
313  "e-puck in the arena. When the robot is untranslated and unrotated, the\n"
314  "pucktom point is in the origin and it is defined as the middle point between\n"
315  "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
316  "axis, that is the point where the wheels touch the floor. The attribute values\n"
317  "are in the X,Y,Z order.\n"
318  "The 'body/orientation' attribute specifies the orientation of the e-puck. All\n"
319  "rotations are performed with respect to the pucktom point. The order of the\n"
320  "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
321  "around the Z axis, the second around Y and the last around X. This reflects\n"
322  "the internal convention used in ARGoS, in which rotations are performed in\n"
323  "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
324  "is oriented along the X axis.\n"
325  "The 'controller/config' attribute is used to assign a controller to the\n"
326  "e-puck. The value of the attribute must be set to the id of a previously\n"
327  "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
328  "OPTIONAL XML CONFIGURATION\n\n"
329  "You can set the emission range of the range-and-bearing system. By default, a\n"
330  "message sent by an e-puck can be received up to 80cm. By using the 'rab_range'\n"
331  "attribute, you can change it to, i.e., 4m as follows:\n\n"
332  " <arena ...>\n"
333  " ...\n"
334  " <e-puck id=\"eb0\" rab_range=\"4\">\n"
335  " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
336  " <controller config=\"mycntrl\" />\n"
337  " </e-puck>\n"
338  " ...\n"
339  " </arena>\n\n"
340  "You can also set the data sent at each time step through the range-and-bearing\n"
341  "system. By default, a message sent by an e-puck is 2 bytes long. By using the\n"
342  "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
343  " <arena ...>\n"
344  " ...\n"
345  " <e-puck id=\"eb0\" rab_data_size=\"20\">\n"
346  " <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
347  " <controller config=\"mycntrl\" />\n"
348  " </e-puck>\n"
349  " ...\n"
350  " </arena>\n\n"
351  "You can also configure the battery of the robot. By default, the battery never\n"
352  "depletes. You can choose among several battery discharge models, such as\n"
353  "- time: the battery depletes by a fixed amount at each time step\n"
354  "- motion: the battery depletes according to how the robot moves\n"
355  "- time_motion: a combination of the above models.\n"
356  "You can define your own models too. Follow the examples in the file\n"
357  "argos3/src/plugins/simulator/entities/battery_equipped_entity.cpp.\n\n"
358  " <arena ...>\n"
359  " ...\n"
360  " <e-puck id=\"eb0\"\n"
361  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
362  " <controller config=\"mycntrl\" />\n"
363  " <battery model=\"time\" factor=\"1e-5\"/>\n"
364  " </e-puck>\n"
365  " ...\n"
366  " </arena>\n\n"
367  " <arena ...>\n"
368  " ...\n"
369  " <e-puck id=\"eb0\"\n"
370  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
371  " <controller config=\"mycntrl\" />\n"
372  " <battery model=\"motion\" pos_factor=\"1e-3\"\n"
373  " orient_factor=\"1e-3\"/>\n"
374  " </e-puck>\n"
375  " ...\n"
376  " </arena>\n\n"
377  " <arena ...>\n"
378  " ...\n"
379  " <e-puck id=\"eb0\"\n"
380  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
381  " <controller config=\"mycntrl\" />\n"
382  " <battery model=\"time_motion\" time_factor=\"1e-5\"\n"
383  " pos_factor=\"1e-3\"\n"
384  " orient_factor=\"1e-3\"/>\n"
385  " </e-puck>\n"
386  " ...\n"
387  " </arena>\n\n",
388  "Under development"
389  );
390 
391  /****************************************/
392  /****************************************/
393 
395 
396  /****************************************/
397  /****************************************/
398 
399 }
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
An entity that contains a pointer to the user-defined controller.
A 3D vector class.
Definition: vector3.h:29
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.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
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.
void SetWheel(UInt32 un_index, const CVector3 &c_position, Real f_radius)
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.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
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")
This entity is a link to a body in the physics engine.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
void SetController(const std::string &str_controller_id)
Creates and assigns a controller with the given id.
virtual void Destroy()
Destroys the entity, undoing whatever was done by Init() or by the standalone constructor.
A container of CLEDEntity.
Basic class for an entity that contains other entities.
A 2D vector class.
Definition: vector2.h:25
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
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.
virtual void UpdateComponents()
Calls the Update() method on all the components.
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
void AddSensor(const CVector2 &c_offset, ESensorType e_type, SAnchor &s_anchor)
void AddSensorRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, Real f_range, UInt32 un_num_sensors, SAnchor &s_anchor)
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
#define ARGOS_PI
To be used when initializing static variables.
Definition: angles.h:32
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
#define UPDATE(COMPONENT)
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void AddSensorRing(const CVector3 &c_center, Real f_radius, const CRadians &c_start_angle, Real f_range, UInt32 un_num_sensors, SAnchor &s_anchor)
virtual void Destroy()
Destroys the entity, undoing whatever was done by Init() or by the standalone constructor.
Definition: entity.h:151
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.