ARGoS  3
A parallel, multi-engine simulator for swarm robotics
spiri_entity.cpp
Go to the documentation of this file.
1 
7 #include "spiri_entity.h"
8 
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>
18 
19 namespace argos {
20 
21  /****************************************/
22  /****************************************/
23 
24  static const Real HEIGHT = 0.090f;
25  static const Real BODY_HEIGHT = HEIGHT * 3.0f / 4.0f;
26  static const Real BODY_SIDE = 0.470f;
27  static const Real BODY_DIAGONAL = BODY_SIDE * ::sqrt(2);
28  static const Real BODY_RADIUS = BODY_DIAGONAL / 2.0f;
29  static const Real BODY_ELEVATION = HEIGHT - BODY_HEIGHT;
30  static const Real RAB_ELEVATION = (BODY_ELEVATION + BODY_HEIGHT) / 2.0f;
31 
32  /****************************************/
33  /****************************************/
34 
36  CComposableEntity(NULL),
37  m_pcControllableEntity(NULL),
38  m_pcEmbodiedEntity(NULL),
39  m_pcQuadRotorEntity(NULL),
40  m_pcRABEquippedEntity(NULL),
41  m_pcPerspectiveCameraEquippedEntity(NULL) {
42  }
43 
44  /****************************************/
45  /****************************************/
46 
47  CSpiriEntity::CSpiriEntity(const std::string& str_id,
48  const std::string& str_controller_id,
49  const CVector3& c_position,
50  const CQuaternion& c_orientation,
51  Real f_rab_range,
52  size_t un_rab_data_size,
53  const CRadians& c_cam_aperture,
54  Real f_cam_range) :
55  CComposableEntity(NULL, str_id),
56  m_pcControllableEntity(NULL),
57  m_pcEmbodiedEntity(NULL),
58  m_pcQuadRotorEntity(NULL),
59  m_pcRABEquippedEntity(NULL),
60  m_pcPerspectiveCameraEquippedEntity(NULL) {
61  try {
62  /*
63  * Create and init components
64  */
65  /*
66  * Embodied entity
67  * Better to put this first, because many other entities need this one
68  */
69  m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
70  AddComponent(*m_pcEmbodiedEntity);
71  /* Quadrotor entity */
72  m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
73  AddComponent(*m_pcQuadRotorEntity);
74  /* RAB equipped entity */
75  SAnchor& cRABAnchor = m_pcEmbodiedEntity->AddAnchor(
76  "rab",
77  CVector3(0.0f, 0.0f, RAB_ELEVATION));
78  m_pcRABEquippedEntity = new CRABEquippedEntity(
79  this,
80  "rab_0",
81  un_rab_data_size,
82  f_rab_range,
83  cRABAnchor,
84  *m_pcEmbodiedEntity);
85  AddComponent(*m_pcRABEquippedEntity);
86  m_pcEmbodiedEntity->EnableAnchor("rab");
87  /* Perspective camera */
88  SAnchor& cCameraAnchor =
89  m_pcEmbodiedEntity->AddAnchor(
90  "camera",
91  CVector3(BODY_RADIUS, 0.0f, 0.0f));
92  m_pcPerspectiveCameraEquippedEntity = new CPerspectiveCameraEquippedEntity(
93  this,
94  "perspective_camera_0",
95  c_cam_aperture,
96  0.035f,
97  f_cam_range,
98  640, 480,
99  cCameraAnchor);
100  AddComponent(*m_pcPerspectiveCameraEquippedEntity);
101  m_pcEmbodiedEntity->EnableAnchor("camera");
102  /* Controllable entity
103  It must be the last one, for actuators/sensors to link to composing entities correctly */
104  m_pcControllableEntity = new CControllableEntity(this, "controller_0");
105  AddComponent(*m_pcControllableEntity);
106  m_pcControllableEntity->SetController(str_controller_id);
107  /* Update components */
109  }
110  catch(CARGoSException& ex) {
111  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
112  }
113  }
114 
115  /****************************************/
116  /****************************************/
117 
119  try {
120  /*
121  * Init parent
122  */
123  CComposableEntity::Init(t_tree);
124  /*
125  * Create and init components
126  */
127  /*
128  * Embodied entity
129  * Better to put this first, because many other entities need this one
130  */
131  m_pcEmbodiedEntity = new CEmbodiedEntity(this);
132  AddComponent(*m_pcEmbodiedEntity);
133  m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
134  /* Quadrotor entity */
135  m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
136  AddComponent(*m_pcQuadRotorEntity);
137  /* RAB equipped entity */
138  Real fRABRange = 3.0f;
139  GetNodeAttributeOrDefault(t_tree, "rab_range", fRABRange, fRABRange);
140  UInt32 unRABDataSize = 10;
141  GetNodeAttributeOrDefault(t_tree, "rab_data_size", unRABDataSize, unRABDataSize);
142  SAnchor& cRABAnchor = m_pcEmbodiedEntity->AddAnchor("rab", CVector3(0.0f, 0.0f, RAB_ELEVATION));
143  m_pcRABEquippedEntity = new CRABEquippedEntity(
144  this,
145  "rab_0",
146  unRABDataSize,
147  fRABRange,
148  cRABAnchor,
149  *m_pcEmbodiedEntity);
150  AddComponent(*m_pcRABEquippedEntity);
151  m_pcEmbodiedEntity->EnableAnchor("rab");
152  /* Perspective camera */
153  CDegrees cCameraAperture(30.0f);
154  GetNodeAttributeOrDefault(t_tree, "camera_aperture", cCameraAperture, cCameraAperture);
155  Real fCameraRange = 10.0f;
156  GetNodeAttributeOrDefault(t_tree, "camera_range", fCameraRange, fCameraRange);
157  SAnchor& cCameraAnchor =
158  m_pcEmbodiedEntity->AddAnchor(
159  "camera",
160  CVector3(BODY_RADIUS, 0.0f, 0.0f));
161  m_pcPerspectiveCameraEquippedEntity = new CPerspectiveCameraEquippedEntity(
162  this,
163  "perspective_camera_0",
164  ToRadians(cCameraAperture),
165  0.035f,
166  fCameraRange,
167  640, 480,
168  cCameraAnchor);
169  AddComponent(*m_pcPerspectiveCameraEquippedEntity);
170  m_pcEmbodiedEntity->EnableAnchor("camera");
171  /* Controllable entity
172  It must be the last one, for actuators/sensors to link to composing entities correctly */
173  m_pcControllableEntity = new CControllableEntity(this);
174  AddComponent(*m_pcControllableEntity);
175  m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
176  /* Update components */
178  }
179  catch(CARGoSException& ex) {
180  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
181  }
182  }
183 
184  /****************************************/
185  /****************************************/
186 
188  /* Reset all components */
190  /* Update components */
192  }
193 
194  /****************************************/
195  /****************************************/
196 
198  "spiri",
199  "Carlo Pinciroli [ilpincy@gmail.com]",
200  "1.0",
201  "The spiri robot, developed by Pleiades Robotics.",
202  "The spiri is a quad-rotor developed by Pleiades Robotics. It is a\n"
203  "fully autonomous robot with a rich set of sensors and actuators. For more\n"
204  "information, refer to the dedicated web page (http://www.pleaides.ca/).\n\n"
205  "REQUIRED XML CONFIGURATION\n\n"
206  " <arena ...>\n"
207  " ...\n"
208  " <spiri id=\"eb0\">\n"
209  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
210  " <controller config=\"mycntrl\" />\n"
211  " </spiri>\n"
212  " ...\n"
213  " </arena>\n\n"
214  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
215  "entities share the same id, initialization aborts.\n"
216  "The 'body/position' attribute specifies the position of the bottom point of the\n"
217  "spiri in the arena. When the robot is untranslated and unrotated, the\n"
218  "bottom point is in the origin and it is defined as the middle point between\n"
219  "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
220  "axis, that is the point where the wheels touch the floor. The attribute values\n"
221  "are in the X,Y,Z order.\n"
222  "The 'body/orientation' attribute specifies the orientation of the spiri. All\n"
223  "rotations are performed with respect to the bottom point. The order of the\n"
224  "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
225  "around the Z axis, the second around Y and the last around X. This reflects\n"
226  "the internal convention used in ARGoS, in which rotations are performed in\n"
227  "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
228  "is oriented along the X axis.\n"
229  "The 'controller/config' attribute is used to assign a controller to the\n"
230  "spiri. The value of the attribute must be set to the id of a previously\n"
231  "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
232  "OPTIONAL XML CONFIGURATION\n\n"
233  "You can set the emission range of the range-and-bearing system. By default, a\n"
234  "message sent by an spiri can be received up to 3m. By using the 'rab_range'\n"
235  "attribute, you can change it to, i.e., 4m as follows:\n\n"
236  " <arena ...>\n"
237  " ...\n"
238  " <spiri id=\"eb0\" rab_range=\"4\">\n"
239  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
240  " <controller config=\"mycntrl\" />\n"
241  " </spiri>\n"
242  " ...\n"
243  " </arena>\n\n"
244  "You can also set the data sent at each time step through the range-and-bearing"
245  "system. By default, a message sent by a spiri is 10 bytes long. By using the"
246  "'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
247  " <arena ...>\n"
248  " ...\n"
249  " <spiri id=\"eb0\" rab_data_size=\"20\">\n"
250  " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
251  " <controller config=\"mycntrl\" />\n"
252  " </spiri>\n"
253  " ...\n"
254  " </arena>\n\n"
255  ,
256  "Under development"
257  );
258 
259  /****************************************/
260  /****************************************/
261 
263 
264  /****************************************/
265  /****************************************/
266 
267 }
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CVector3
A 3D vector class.
Definition: vector3.h:29
argos::CSpiriEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
Definition: spiri_entity.cpp:187
argos::CPerspectiveCameraEquippedEntity
Definition: perspective_camera_equipped_entity.h:23
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
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::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CEmbodiedEntity::EnableAnchor
void EnableAnchor(const std::string &str_id)
Enables an anchor.
Definition: embodied_entity.cpp:150
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::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CRABEquippedEntity
Definition: rab_equipped_entity.h:25
argos::CComposableEntity::AddComponent
void AddComponent(CEntity &c_component)
Adds a component to this composable entity.
Definition: composable_entity.cpp:72
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::CQuaternion
Definition: quaternion.h:14
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
argos::REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CComposableEntity::UpdateComponents
virtual void UpdateComponents()
Calls the Update() method on all the components.
Definition: composable_entity.cpp:59
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::REGISTER_ENTITY
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")
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
argos::CSpiriEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: spiri_entity.cpp:118
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::CSpiriEntity
Definition: spiri_entity.h:23
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::CQuadRotorEntity
Definition: quadrotor_entity.h:15
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::CDegrees
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::ToRadians
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
argos::CSpiriEntity::CSpiriEntity
CSpiriEntity()
Definition: spiri_entity.cpp:35
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:36
spiri_entity.h