ground_sensor_equipped_entity.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/simulator/space/space.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
17  CEntity(pc_parent) {
18  Disable();
19  }
20 
21  /****************************************/
22  /****************************************/
23 
25  const std::string& str_id) :
26  CEntity(pc_parent, str_id) {
27  Disable();
28  }
29 
30  /****************************************/
31  /****************************************/
32 
34  while(! m_tSensors.empty()) {
35  delete m_tSensors.back();
36  m_tSensors.pop_back();
37  }
38  }
39 
40  /****************************************/
41  /****************************************/
42 
44  try {
45  /*
46  * Parse basic entity stuff
47  */
48  CEntity::Init(t_tree);
49  /*
50  * Parse ground sensors
51  */
52  /* Not adding any sensor is a fatal error */
53  if(t_tree.NoChildren()) {
54  THROW_ARGOSEXCEPTION("No sensors defined");
55  }
56  /* Go through children */
58  for(it = it.begin(&t_tree); it != it.end(); ++it) {
59  std::string strAnchorId;
60  GetNodeAttribute(*it, "anchor", strAnchorId);
61  /*
62  * NOTE: here we get a reference to the embodied entity
63  * This line works under the assumption that:
64  * 1. the GroundSensorEquippedEntity has a parent;
65  * 2. the parent has a child whose id is "body"
66  * 3. the "body" is an embodied entity
67  * If any of the above is false, this line will bomb out.
68  */
70  if(it->Value() == "sensor") {
71  CVector2 cOffset;
72  GetNodeAttribute(*it, "offset", cOffset);
73  std::string strType;
74  GetNodeAttribute(*it, "type", strType);
75  AddSensor(cOffset, ParseType(strType), cBody.GetAnchor(strAnchorId));
76  }
77  else if(it->Value() == "ring") {
78  CVector2 cRingCenter;
79  GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter);
80  Real fRadius;
81  GetNodeAttribute(t_tree, "radius", fRadius);
82  CDegrees cRingStartAngleDegrees;
83  GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees);
84  CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees);
85  std::string strType;
86  GetNodeAttribute(*it, "type", strType);
87  ESensorType eType = ParseType(strType);
88  UInt32 unNumSensors;
89  GetNodeAttribute(t_tree, "num_sensors", unNumSensors);
90  AddSensorRing(cRingCenter,
91  fRadius,
92  cRingStartAngleRadians,
93  eType,
94  unNumSensors,
95  cBody.GetAnchor(strAnchorId));
96  }
97  else {
98  THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\"");
99  }
100  }
101  }
102  catch(CARGoSException& ex) {
103  THROW_ARGOSEXCEPTION_NESTED("Initialization error in ground sensor equipped entity", ex);
104  }
105  }
106 
107  /****************************************/
108  /****************************************/
109 
111  CEntity::Enable();
112  for(size_t i = 0; i < m_tSensors.size(); ++i) {
113  m_tSensors[i]->Anchor.Enable();
114  }
115  }
116 
117  /****************************************/
118  /****************************************/
119 
122  for(size_t i = 0; i < m_tSensors.size(); ++i) {
123  m_tSensors[i]->Anchor.Disable();
124  }
125  }
126 
127  /****************************************/
128  /****************************************/
129 
131  ESensorType e_type,
132  SAnchor& s_anchor) {
133  m_tSensors.push_back(new SSensor(c_offset, e_type, s_anchor));
134  }
135 
136  /****************************************/
137  /****************************************/
138 
140  Real f_radius,
141  const CRadians& c_start_angle,
142  ESensorType e_type,
143  UInt32 un_num_sensors,
144  SAnchor& s_anchor) {
145  CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
146  CRadians cAngle;
147  CVector2 cOffset;
148  for(UInt32 i = 0; i < un_num_sensors; ++i) {
149  cAngle = c_start_angle + i * cSensorSpacing;
150  cAngle.SignedNormalize();
151  cOffset.Set(f_radius, 0.0f);
152  cOffset.Rotate(cAngle);
153  cOffset += c_center;
154  AddSensor(cOffset, e_type, s_anchor);
155  }
156  }
157 
158  /****************************************/
159  /****************************************/
160 
162  if(str_type == "bw") return TYPE_BLACK_WHITE;
163  if(str_type == "gray") return TYPE_GRAYSCALE;
164  THROW_ARGOSEXCEPTION("Unrecognized ground sensor type \"" << str_type << "\"");
165  }
166 
167  /****************************************/
168  /****************************************/
169 
171 
172  /****************************************/
173  /****************************************/
174 
175 }
ESensorType ParseType(const std::string &str_type) const
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.
void Enable()
Enables the entity.
Definition: entity.h:265
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
The basic entity type.
Definition: entity.h:89
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
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...
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CGroundSensorEquippedEntity(CComposableEntity *pc_parent)
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
Basic class for an entity that contains other entities.
A 2D vector class.
Definition: vector2.h:25
An anchor related to the body of an entity.
Definition: physics_model.h:38
SSensor::TList m_tSensors
The list of sensors.
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.
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 Disable()
Disables the entity.
Definition: entity.h:275
void AddSensor(const CVector2 &c_offset, ESensorType e_type, SAnchor &s_anchor)
REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CEntity)
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:111
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
void AddSensorRing(const CVector2 &c_center, Real f_radius, const CRadians &c_start_angle, ESensorType e_type, UInt32 un_num_sensors, SAnchor &s_anchor)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CVector2 & Rotate(const CRadians &c_angle)
Rotates this vector by the wanted angle.
Definition: vector2.h:169
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.