positioning_default_sensor.cpp
Go to the documentation of this file.
1 
7 #include <argos3/core/simulator/simulator.h>
8 #include <argos3/core/simulator/entity/embodied_entity.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 
12 
13 namespace argos {
14 
15  /****************************************/
16  /****************************************/
17 
19  m_pcEmbodiedEntity(nullptr),
20  m_pcRNG(nullptr),
21  m_bAddNoise(false) {}
22 
23  /****************************************/
24  /****************************************/
25 
27  m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
30  }
31 
32  /****************************************/
33  /****************************************/
34 
36  try {
38  /* Parse noise range */
39  GetNodeAttributeOrDefault(t_tree, "pos_noise_range", m_cPosNoiseRange, m_cPosNoiseRange);
42  if(m_cPosNoiseRange.GetSpan() != 0 ||
43  m_cAngleNoiseRange.GetSpan() != CRadians::ZERO ||
44  m_cAxisNoiseRange.GetSpan() != 0) {
45  m_bAddNoise = true;
46  m_pcRNG = CRandom::CreateRNG("argos");
47  }
48  /* sensor is enabled by default */
49  Enable();
50  }
51  catch(CARGoSException& ex) {
52  THROW_ARGOSEXCEPTION_NESTED("Initialization error in default positioning sensor", ex);
53  }
54  }
55 
56  /****************************************/
57  /****************************************/
58 
60  /* sensor is disabled--nothing to do */
61  if (IsDisabled()) {
62  return;
63  }
65  if(m_bAddNoise) {
75  }
76  else {
78  }
79  }
80 
81  /****************************************/
82  /****************************************/
83 
87  }
88 
89  /****************************************/
90  /****************************************/
91 
93  "positioning", "default",
94  "Carlo Pinciroli [ilpincy@gmail.com]",
95  "1.0",
96  "A generic positioning sensor.",
97 
98  "This sensor returns the current position and orientation of a robot. This sensor\n"
99  "can be used with any robot, since it accesses only the body component. In\n"
100  "controllers, you must include the ci_positioning_sensor.h header.\n\n"
101 
102  "This sensor is enabled by default.\n\n"
103 
104  "REQUIRED XML CONFIGURATION\n\n"
105  " <controllers>\n"
106  " ...\n"
107  " <my_controller ...>\n"
108  " ...\n"
109  " <sensors>\n"
110  " ...\n"
111  " <positioning implementation=\"default\" />\n"
112  " ...\n"
113  " </sensors>\n"
114  " ...\n"
115  " </my_controller>\n"
116  " ...\n"
117  " </controllers>\n\n"
118 
119  "OPTIONAL XML CONFIGURATION\n\n"
120 
121  "It is possible to add uniform noise to the sensor, thus matching the\n"
122  "characteristics of a real robot better. You can add noise through the\n"
123  "attributes 'pos_noise_range', 'angle_noise_range', and 'axis_noise_range'.\n"
124  "Attribute 'pos_noise_range' regulates the noise range on the position returned\n"
125  "by the sensor. Attribute 'angle_noise_range' sets the noise range on the angle\n"
126  "(values expressed in degrees). Attribute 'axis_noise_range' sets the noise for\n"
127  "the rotation axis. Angle and axis are used to calculate a quaternion, which is\n"
128  "the actual returned value for rotation.\n\n"
129 
130  " <controllers>\n"
131  " ...\n"
132  " <my_controller ...>\n"
133  " ...\n"
134  " <sensors>\n"
135  " ...\n"
136  " <positioning implementation=\"default\"\n"
137  " pos_noise_range=\"-0.1:0.2\"\n"
138  " angle_noise_range=\"-10.5:13.7\"\n"
139  " axis_noise_range=\"-0.3:0.4\" />\n"
140  " ...\n"
141  " </sensors>\n"
142  " ...\n"
143  " </my_controller>\n"
144  " ...\n"
145  " </controllers>\n\n"
146 
147  "OPTIONAL XML CONFIGURATION\n\n"
148 
149  "None.\n",
150 
151  "Usable"
152  );
153 
154 }
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
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.
REGISTER_SENSOR(CEPuckProximityDefaultSensor, "epuck_proximity", "default", "Danesh Tarapore [daneshtarapore@gmail.com]", "1.0", "The E-Puck proximity sensor.", "This sensor accesses the epuck proximity sensor. For a complete description\n" "of its usage, refer to the ci_epuck_proximity_sensor.h interface. For the XML\n" "configuration, refer to the default proximity sensor.\n", "Usable")
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual void Enable()
Enables updating of sensor information in the event loop.
Definition: ci_sensor.h:78
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
bool IsDisabled() const
Definition: ci_sensor.h:86
Basic class for an entity that contains other entities.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
This entity is a link to a body in the physics engine.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
The exception that wraps all errors in ARGoS.
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
static const CRadians ZERO
Set to zero radians.
Definition: angles.h:79
void ToAngleAxis(CRadians &c_angle, CVector3 &c_vector) const
Definition: quaternion.h:143
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
T GetSpan() const
Definition: range.h:64
static CRNG * CreateRNG(const std::string &str_category)
Creates a new RNG inside the given category.
Definition: rng.cpp:347
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
Definition: rng.cpp:87
A 3D vector class.
Definition: vector3.h:31
virtual void Update()
Updates the state of the entity associated to this sensor, if the sensor is currently enabled.
CRange< CRadians > m_cAngleNoiseRange
Noise range on angle.
CEmbodiedEntity * m_pcEmbodiedEntity
Reference to embodied entity associated to this sensor.
CRange< Real > m_cAxisNoiseRange
Noise range on axis.
bool m_bAddNoise
Whether to add noise or not.
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
virtual void Reset()
Resets the sensor to the state it had just after Init().
CRange< Real > m_cPosNoiseRange
Noise range on position.
CRandom::CRNG * m_pcRNG
Random number generator.