prototype_entity.cpp
Go to the documentation of this file.
1 
7 #include "prototype_entity.h"
8 #include <argos3/core/simulator/space/space.h>
9 #include <argos3/core/simulator/entity/embodied_entity.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/plugins/robots/prototype/simulator/prototype_link_equipped_entity.h>
12 #include <argos3/plugins/robots/prototype/simulator/prototype_joint_equipped_entity.h>
13 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
14 #include <argos3/plugins/simulator/entities/directional_led_equipped_entity.h>
15 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
16 #include <argos3/plugins/simulator/entities/magnet_equipped_entity.h>
17 #include <argos3/plugins/simulator/entities/radio_equipped_entity.h>
18 #include <argos3/plugins/simulator/entities/tag_equipped_entity.h>
19 #include <argos3/plugins/simulator/media/directional_led_medium.h>
20 #include <argos3/plugins/simulator/media/led_medium.h>
21 #include <argos3/plugins/simulator/media/radio_medium.h>
22 #include <argos3/plugins/simulator/media/tag_medium.h>
23 
24 namespace argos {
25 
26  /****************************************/
27  /****************************************/
28 
30  CComposableEntity(nullptr),
31  m_pcEmbodiedEntity(nullptr),
32  m_pcControllableEntity(nullptr),
33  m_pcLinkEquippedEntity(nullptr),
34  m_pcJointEquippedEntity(nullptr),
35  m_pcDirectionalLEDEquippedEntity(nullptr),
36  m_pcLEDEquippedEntity(nullptr),
37  m_pcTagEquippedEntity(nullptr),
38  m_pcRadioEquippedEntity(nullptr),
39  m_pcMagnetEquippedEntity(nullptr),
40  m_pcProximitySensorEquippedEntity(nullptr) {}
41 
42  /****************************************/
43  /****************************************/
44 
46  try {
47  /* Initialize the parent */
49  /* Check if the model is movable */
50  bool bMovable = true;
51  GetNodeAttributeOrDefault(t_tree, "movable", bMovable, bMovable);
52  /* Initialize the body */
53  m_pcEmbodiedEntity = new CEmbodiedEntity(this);
54  AddComponent(*m_pcEmbodiedEntity);
55  m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
56  m_pcEmbodiedEntity->SetMovable(bMovable);
57  /* Initialize the links */
58  m_pcLinkEquippedEntity = new CPrototypeLinkEquippedEntity(this);
59  m_pcLinkEquippedEntity->Init(GetNode(t_tree, "links"));
60  AddComponent(*m_pcLinkEquippedEntity);
61  /* Initialize the joints */
62  m_pcJointEquippedEntity = new CPrototypeJointEquippedEntity(this);
63  AddComponent(*m_pcJointEquippedEntity);
64  /* create an empty joints configuration node if one isn't provided */
65  if(!NodeExists(t_tree, "joints")) {
66  TConfigurationNode tJointsNode("joints");
67  AddChildNode(t_tree, tJointsNode);
68  }
69  m_pcJointEquippedEntity->Init(GetNode(t_tree, "joints"));
70  /* Initialize the devices */
71  if(NodeExists(t_tree, "devices")) {
73  for(itDevice = itDevice.begin(&GetNode(t_tree, "devices"));
74  itDevice != itDevice.end();
75  ++itDevice) {
76  if(itDevice->Value() == "directional_leds" ) {
77  m_pcDirectionalLEDEquippedEntity =
79  m_pcDirectionalLEDEquippedEntity->Init(*itDevice);
80  AddComponent(*m_pcDirectionalLEDEquippedEntity);
81  /* Add the directional LEDs to the medium */
82  std::string strMedium;
83  GetNodeAttribute(*itDevice, "medium", strMedium);
84  CDirectionalLEDMedium& cDirectionalLEDMedium =
86  m_pcDirectionalLEDEquippedEntity->SetMedium(cDirectionalLEDMedium);
87  m_pcDirectionalLEDEquippedEntity->Enable();
88  }
89  else if(itDevice->Value() == "leds" ) {
90  m_pcLEDEquippedEntity = new CLEDEquippedEntity(this);
91  m_pcLEDEquippedEntity->Init(*itDevice);
92  AddComponent(*m_pcLEDEquippedEntity);
93  /* Add the LEDs to the medium */
94  std::string strMedium;
95  GetNodeAttribute(*itDevice, "medium", strMedium);
96  CLEDMedium& cLEDMedium =
98  m_pcLEDEquippedEntity->SetMedium(cLEDMedium);
99  m_pcLEDEquippedEntity->Enable();
100  }
101  else if(itDevice->Value() == "proximity_sensors") {
102  m_pcProximitySensorEquippedEntity = new CProximitySensorEquippedEntity(this);
103  m_pcProximitySensorEquippedEntity->Init(*itDevice);
104  AddComponent(*m_pcProximitySensorEquippedEntity);
105  m_pcProximitySensorEquippedEntity->Enable();
106  }
107  else if(itDevice->Value() == "tags" ) {
108  m_pcTagEquippedEntity = new CTagEquippedEntity(this);
109  m_pcTagEquippedEntity->Init(*itDevice);
110  AddComponent(*m_pcTagEquippedEntity);
111  /* Add the tags to the medium */
112  std::string strMedium;
113  GetNodeAttribute(*itDevice, "medium", strMedium);
114  CTagMedium& cTagMedium =
116  m_pcTagEquippedEntity->SetMedium(cTagMedium);
117  m_pcTagEquippedEntity->Enable();
118  }
119  else if(itDevice->Value() == "radios" ) {
120  CRadioEquippedEntity* m_pcRadioEquippedEntity =
121  new CRadioEquippedEntity(this);
122  m_pcRadioEquippedEntity->Init(*itDevice);
123  AddComponent(*m_pcRadioEquippedEntity);
124  /* Add the radios to the medium */
125  std::string strMedium;
126  GetNodeAttribute(*itDevice, "medium", strMedium);
127  CRadioMedium& cRadioMedium =
129  m_pcRadioEquippedEntity->SetMedium(cRadioMedium);
130  m_pcRadioEquippedEntity->Enable();
131  }
132  else if(itDevice->Value() == "magnets" ) {
133  m_pcMagnetEquippedEntity = new CMagnetEquippedEntity(this);
134  m_pcMagnetEquippedEntity->Init(*itDevice);
135  AddComponent(*m_pcMagnetEquippedEntity);
136  m_pcMagnetEquippedEntity->Enable();
137  }
138  else {
139  THROW_ARGOSEXCEPTION("Device type \"" << itDevice->Value() << "\" not implemented");
140  }
141  }
142  }
143  /* Initialize the controllable entity, this is done last so that the sensors and
144  actuators correctly link to the entity */
145  if(NodeExists(t_tree, "controller")) {
146  m_pcControllableEntity = new CControllableEntity(this);
147  AddComponent(*m_pcControllableEntity);
148  m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
149  }
150  /* Update components */
152  }
153  catch(CARGoSException& ex) {
154  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
155  }
156  }
157 
158  /****************************************/
159  /****************************************/
160 
162  "prototype",
163  "1.0",
164  "Michael Allwright [allsey87@gmail.com]",
165  "A generic and configurable entity",
166  "The prototyping entity allows users to quickly implement new entities using an\n"
167  "experiment's configuration file. A prototype entity is predominantly described\n"
168  "as a set of links connected through joints. The joints can be fixed, prismatic,\n"
169  "revolute, spherical, or planar. Sensors and actuators for prismatic and revolute\n"
170  "joints are provided. Since each link defines an anchor in the embodied entity,\n"
171  "it is also possible to attach the following entities to any link in the model:\n"
172  "the directional LED entity, the LED entity, the magnet entity, the tag entity,\n"
173  "the radio entity, and the proximity sensor equipped entity.\n\n"
174  "REQUIRED XML CONFIGURATION\n\n"
175  " <arena ...>\n"
176  " ...\n"
177  " <prototype id=\"pt0\" movable=\"true\">\n"
178  " <body position=\"0.1,0.1,0.0\" orientation=\"45,0,0\" />\n"
179  " <links ref=\"base\">\n"
180  " <link id=\"base\" geometry=\"box\" size=\"0.1,0.1,0.1\"\n"
181  " mass=\"1\" position=\"0,0,0\" orientation=\"0,0,0\" />\n"
182  " </prototype>\n"
183  " ...\n"
184  " </arena>\n\n"
185  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
186  "entities share the same id the initialization aborts and the simulator will\n"
187  "terminate. The movable attribute defines whether the reference link of a robot\n"
188  "is fixed or not. This attribute is useful, for example, if you want to model a\n"
189  "robotic arm whose links are movable, but where there is one link (i.e. the\n"
190  "reference link) that is fixed to the ground, a wall, or the ceiling.\n"
191  "The body tag defines the position and orientation of the prototype entity in the\n"
192  "global coordinate system. The most simple entity contains a single link and no\n"
193  "joints. An important detail to note is the 'ref' attribute of the links node.\n"
194  "This attribute specifies which link is to be used to refer to the prototype\n"
195  "entity's location. The example in the required configuration defines a single\n"
196  "link with a box geometry. Three geometries for links are currently supported:\n"
197  "boxes, cylinders, and spheres. The size of a box is defined using the size\n"
198  "attribute while cylinders and spheres are defined using a radius attribute and a\n"
199  "height attribute in the case of the cylinder. The mass of each link must be\n"
200  "defined and specified in kilograms. The position and orientation of each link\n"
201  "refer to the offset of the link in the entity's local coordinate system.\n\n"
202  "OPTIONAL XML CONFIGURATION\n\n"
203  "In the following example, additional links and joints are added to a prototype\n"
204  "entity to form a small four-wheeled vehicle.\n\n"
205  " <arena ...>\n"
206  " ...\n"
207  " <prototype id=\"vehicle\" movable=\"true\">\n"
208  " <body position=\"0,0,0\" orientation=\"0,0,0\" />\n"
209  " <links ref=\"base\">\n"
210  " <link id=\"base\" geometry=\"box\" size=\"0.1,0.05,0.02\" mass=\"1\"\n"
211  " position=\"0,0,0.01\" orientation=\"0,0,0\" />\n"
212  " <link id=\"wheel_fl\" geometry=\"cylinder\" radius=\"0.02\" height=\".005\"\n"
213  " mass=\".05\" position=\".025,.025,0.02\" orientation=\"0,0,-90\" />\n"
214  " <link id=\"wheel_fr\" geometry=\"cylinder\" radius=\"0.02\" height=\".005\"\n"
215  " mass=\".05\" position=\".025,-.025,0.02\" orientation=\"0,0,90\" />\n"
216  " <link id=\"wheel_br\" geometry=\"cylinder\" radius=\"0.02\" height=\".005\"\n"
217  " mass=\".05\" position=\"-.025,-.025,0.02\" orientation=\"0,0,90\" />\n"
218  " <link id=\"wheel_bl\" geometry=\"cylinder\" radius=\"0.02\" height=\".005\"\n"
219  " mass=\".05\" position=\"-.025,.025,0.02\" orientation=\"0,0,-90\" />\n"
220  " </links>\n"
221  " <joints>\n"
222  " <joint id=\"base_wheel_fr\" type=\"revolute\" axis=\"0,0,1\">\n"
223  " <parent link=\"base\" position=\".025,-.025,.01\" orientation=\"0,0,90\"/>\n"
224  " <child link=\"wheel_fr\" position=\"0,0,0\" orientation=\"0,0,0\"/>\n"
225  " </joint>\n"
226  " <joint id=\"base_wheel_fl\" type=\"revolute\" axis=\"0,0,1\">\n"
227  " <parent link=\"base\" position=\".025,.025,.01\" orientation=\"0,0,-90\"/>\n"
228  " <child link=\"wheel_fl\" position=\"0,0,0\" orientation=\"0,0,0\"/>\n"
229  " </joint>\n"
230  " <joint id=\"base_wheel_br\" type=\"revolute\" axis=\"0,0,1\">\n"
231  " <parent link=\"base\" position=\"-.025,-.025,.01\" orientation=\"0,0,90\"/>\n"
232  " <child link=\"wheel_br\" position=\"0,0,0\" orientation=\"0,0,0\"/>\n"
233  " </joint>\n"
234  " <joint id=\"base_wheel_bl\" type=\"revolute\" axis=\"0,0,1\">\n"
235  " <parent link=\"base\" position=\"-.025,.025,.01\" orientation=\"0,0,-90\"/>\n"
236  " <child link=\"wheel_bl\" position=\"0,0,0\" orientation=\"0,0,0\"/>\n"
237  " </joint>\n"
238  " </joints>\n"
239  " </prototype>\n"
240  " ...\n"
241  " </arena>\n\n"
242  "In this example, there are five links: the base of the vehicle and four wheels.\n"
243  "The wheels are attached to the base using revolute joints. The joint node\n"
244  "requires the joint type attribute and in the case of prismatic and revolute\n"
245  "joints an axis which defines the joint's degree of freedom. In addition to the\n"
246  "axis attribute, prismatic and revolute joints can also be limited using the\n"
247  "limit attribute. In the case of prismatic joints, this value is interpreted as a\n"
248  "range in meters and in the case of revolute joints, this value is interpreted as\n"
249  "a range in degrees. The maximum and minimum values must be separated with the\n"
250  "colon character. The joint node requires two child nodes, namely the parent and\n"
251  "child nodes which describe the links involved in the joint. Each of these nodes\n"
252  "also specifies a position and an orientation, which defines the offset from the\n"
253  "link's local coordinate system to the joint's reference frame. Note that the\n"
254  "prototype entity is consistent with ARGoS conventions and the origin of all\n"
255  "coordinate systems is at the base of an entity and not at its center of mass.\n"
256  "Prismatic and revolute joints support sensors and actuators which can control\n"
257  "and monitor the position and velocity of a joint. Refer to the documentation on\n"
258  "the joints sensor and joints actuator by querying the joints plugin. It is also\n"
259  "possible to attach any number of devices to the links in a prototype entity:\n\n"
260  " <arena ...>\n"
261  " ...\n"
262  " <prototype id=\"block\" movable=\"true\">\n"
263  " <body position=\"0,0,0\" orientation=\"0,0,0\" />\n"
264  " <links ref=\"base\">\n"
265  " <link id=\"base\" geometry=\"box\" size=\".015,.015,.015\" mass=\"0.1\"\n"
266  " position=\"0,0,0\" orientation=\"0,0,0\" />\n"
267  " </links>\n"
268  " <devices>\n"
269  " <directional_leds medium=\"leds\">\n"
270  " <directional_led anchor=\"base\" color=\"red\" observable_angle=\"180\"\n"
271  " position=\"0,0,.0155\" orientation=\"0,0,0\"/>\n"
272  " </directional_leds>\n"
273  " <tags medium=\"tags\">\n"
274  " <tag position=\"0,0,.0155\" orientation=\"0,0,0\" payload=\"0\"\n"
275  " side_length=\".01\" observable_angle=\"180\" anchor=\"base\"/>\n"
276  " </tags>\n"
277  " <radios medium=\"nfc\">\n"
278  " <radio anchor=\"base\" duplex_mode=\"half\" range=\"1\"\n"
279  " position=\"0,0,0.0075\"/>\n"
280  " </radios>\n"
281  " </devices>\n"
282  " </prototype>\n"
283  " ...\n"
284  " </arena>\n\n"
285  "Each of these devices has three attributes in common: an anchor, a position, and\n"
286  "an orientation. Using these attributes you can attach any number of these\n"
287  "entities to any link in the prototype entity since each link creates an anchor.\n"
288  "In addition to these attributes, the entities have some attributes that define\n"
289  "their characteristics. For example, tags have a side length and payload, while\n"
290  "the directional LED has a color. The three devices in the example all require\n"
291  "mediums. These mediums index the entities and allow them to be detected by\n"
292  "sensors.",
293  "Usable"
294  );
295 
296  /****************************************/
297  /****************************************/
298 
300 
301  /****************************************/
302  /****************************************/
303 }
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
An entity that contains a pointer to the user-defined controller.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
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.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void SetMedium(CTagMedium &c_medium)
Sets the medium associated to this entity.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
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.
A container of CTagEntity.
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.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void SetMedium(CLEDMedium &c_medium)
Sets the medium associated to this entity.
void AddChildNode(TConfigurationNode &t_parent_node, TConfigurationNode &t_child_node)
Adds an XML node as child of another XML node.
A container of CLEDEntity.
Basic class for an entity that contains other entities.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
void SetMovable(bool b_movable)
Sets whether this entity is movable or not.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
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.
void SetMedium(CRadioMedium &c_medium)
Sets the medium associated to this entity.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
A container of CRadioEntity.
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
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
A container of CMagnetEntity.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
A container of CDirectionalLEDEntity.
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void SetMedium(CDirectionalLEDMedium &c_medium)
Sets the medium associated to this entity.
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
virtual void UpdateComponents()
Calls the Update() method on all the components.