8 #include <argos3/core/simulator/entity/composable_entity.h>
9 #include <argos3/core/simulator/entity/controllable_entity.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/core/simulator/space/space.h>
18 static const Real FOOTBOT_RADIUS = 0.085036758f;
20 static const Real SHORT_RANGE_MIN_DISTANCE = 0.0f;
21 static const Real SHORT_RANGE_RAY_START = FOOTBOT_RADIUS;
22 static const Real SHORT_RANGE_RAY_END = FOOTBOT_RADIUS + 0.26f;
24 static const Real LONG_RANGE_MIN_DISTANCE = 0.12f;
25 static const Real LONG_RANGE_RAY_START = FOOTBOT_RADIUS;
26 static const Real LONG_RANGE_RAY_END = FOOTBOT_RADIUS + 1.42f;
28 static const Real SENSOR_ELEVATION = 0.123199866f;
36 m_cSpace(
CSimulator::GetInstance().GetSpace()),
49 if(m_cNoiseRange.
GetSpan() > 0.0f) {
66 m_pcDistScanEntity->
Enable();
83 CalculateRaysNotRotating();
85 m_cLastDistScanRotation = m_pcDistScanEntity->
GetRotation();
92 CalculateRaysRotating();
96 m_cLastDistScanRotation = m_pcDistScanEntity->
GetRotation();
116 void CFootBotDistanceScannerRotZOnlySensor::UpdateNotRotating() {
118 CRadians cAngle = m_cLastDistScanRotation;
120 Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
126 fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
132 fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
138 fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
146 #define ADD_READING(RAYS,MAP,INDEX,MINDIST) \
147 cAngle += cInterSensorSpan; \
148 cAngle.SignedNormalize(); \
149 fReading = CalculateReadingForRay(RAYS[INDEX],MINDIST); \
150 MAP[cAngle] = fReading; \
151 m_tReadingsMap[cAngle] = fReading;
153 #define ADD_READINGS(RAYS,MAP,MINDIST) \
154 ADD_READING(RAYS,MAP,1,MINDIST) \
155 ADD_READING(RAYS,MAP,2,MINDIST) \
156 ADD_READING(RAYS,MAP,3,MINDIST) \
157 ADD_READING(RAYS,MAP,4,MINDIST) \
158 ADD_READING(RAYS,MAP,5,MINDIST)
160 void CFootBotDistanceScannerRotZOnlySensor::UpdateRotating() {
161 CRadians cInterSensorSpan = (m_pcDistScanEntity->
GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f;
162 CRadians cStartAngle = m_cLastDistScanRotation;
164 CRadians cAngle = cStartAngle;
166 Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
173 fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
180 fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
187 fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
196 Real CFootBotDistanceScannerRotZOnlySensor::CalculateReadingForRay(
const CRay3& c_ray,
197 Real f_min_distance) {
199 SEmbodiedEntityIntersectionItem sIntersection;
202 *m_pcEmbodiedEntity)) {
205 Real fDistance = c_ray.GetDistance(sIntersection.TOnRay);
206 if(fDistance > f_min_distance) {
208 if(m_bShowRays) m_pcControllableEntity->
AddCheckedRay(
true, c_ray);
209 return fDistance * 100.0f;
213 if(m_bShowRays) m_pcControllableEntity->
AddCheckedRay(
true, c_ray);
219 if(m_bShowRays) m_pcControllableEntity->
AddCheckedRay(
false, c_ray);
228 #define CALCULATE_SHORT_RANGE_RAY(ANGLE,INDEX) \
229 m_cDirection.RotateZ(ANGLE); \
230 m_cOriginRayStart = m_cDirection; \
231 m_cOriginRayEnd = m_cDirection; \
232 m_cOriginRayStart *= SHORT_RANGE_RAY_START; \
233 m_cOriginRayEnd *= SHORT_RANGE_RAY_END; \
234 m_cRayStart = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
235 m_cRayStart += m_cOriginRayStart; \
236 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
237 m_cRayEnd = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
238 m_cRayEnd += m_cOriginRayEnd; \
239 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \
240 m_cShortRangeRays0[INDEX].Set(m_cRayStart, m_cRayEnd); \
241 m_cRayStart = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
242 m_cRayStart -= m_cOriginRayStart; \
243 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
244 m_cRayEnd = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
245 m_cRayEnd -= m_cOriginRayEnd; \
246 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \
247 m_cShortRangeRays2[INDEX].Set(m_cRayStart, m_cRayEnd); \
250 #define CALCULATE_LONG_RANGE_RAY(ANGLE,INDEX) \
251 m_cDirection.RotateZ(ANGLE); \
252 m_cOriginRayStart = m_cDirection; \
253 m_cOriginRayEnd = m_cDirection; \
254 m_cOriginRayStart *= LONG_RANGE_RAY_START; \
255 m_cOriginRayEnd *= LONG_RANGE_RAY_END; \
256 m_cRayStart = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
257 m_cRayStart += m_cOriginRayStart; \
258 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
259 m_cRayEnd = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
260 m_cRayEnd += m_cOriginRayEnd; \
261 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \
262 m_cLongRangeRays1[INDEX].Set(m_cRayStart, m_cRayEnd); \
263 m_cRayStart = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
264 m_cRayStart -= m_cOriginRayStart; \
265 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
266 m_cRayEnd = m_pcEmbodiedEntity->GetOriginAnchor().Position; \
267 m_cRayEnd -= m_cOriginRayEnd; \
268 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \
269 m_cLongRangeRays3[INDEX].Set(m_cRayStart, m_cRayEnd);
274 void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysNotRotating() {
277 CRadians cTmp1, cTmp2, cOrientationZ;
280 cOrientationZ += m_pcDistScanEntity->
GetRotation();
282 CVector2 cAbsoluteOrientation(1.0, cOrientationZ);
294 void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysRotating() {
297 CRadians cTmp1, cTmp2, cOrientationZ;
300 cOrientationZ += m_cLastDistScanRotation;
302 CVector2 cAbsoluteOrientation(1.0, cOrientationZ);
304 CVector2 cInterSensorSpan(1.0f, (m_pcDistScanEntity->
GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f);
327 "footbot_distance_scanner",
"rot_z_only",
328 "Carlo Pinciroli [ilpincy@gmail.com]",
330 "The foot-bot distance scanner sensor (optimized for 2D).",
331 "This sensor accesses the foot-bot distance scanner sensor. For a complete\n"
332 "description of its usage, refer to the common interface.\n"
333 "In this implementation, the readings are calculated under the assumption that\n"
334 "the foot-bot is always parallel to the XY plane, i.e., it rotates only around\n"
335 "the Z axis. This implementation is faster than a 3D one and should be used\n"
336 "only when the assumption about the foot-bot rotation holds.\n\n"
337 "REQUIRED XML CONFIGURATION\n\n"
340 " <my_controller ...>\n"
344 " <footbot_distance_scanner implementation=\"rot_z_only\" />\n"
348 " </my_controller>\n"
350 " </controllers>\n\n"
351 "OPTIONAL XML CONFIGURATION\n\n"
352 "It is possible to draw the rays shot by the distance scanner in the OpenGL\n"
353 "visualization. This can be useful for sensor debugging but also to understand\n"
354 "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
355 "they are not obstructed and in purple when they are. In case a ray is\n"
356 "obstructed, a black dot is drawn where the intersection occurred.\n"
357 "To turn this functionality on, add the attribute 'show_rays=\"true\"' in the\n"
358 "XML as in this example:\n\n"
361 " <my_controller ...>\n"
365 " <footbot_distance_scanner implementation=\"rot_z_only\"\n"
366 " show_rays=\"true\" />\n"
370 " </my_controller>\n"