ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "footbot_distance_scanner_rotzonly_sensor.h" 00008 #include <argos3/core/simulator/entity/composable_entity.h> 00009 #include <argos3/core/simulator/entity/controllable_entity.h> 00010 #include <argos3/core/simulator/simulator.h> 00011 #include <argos3/core/simulator/space/space.h> 00012 00013 namespace argos { 00014 00015 /****************************************/ 00016 /****************************************/ 00017 00018 static const Real FOOTBOT_RADIUS = 0.085036758f; 00019 00020 static const Real SHORT_RANGE_MIN_DISTANCE = 0.0f; 00021 static const Real SHORT_RANGE_RAY_START = FOOTBOT_RADIUS; 00022 static const Real SHORT_RANGE_RAY_END = FOOTBOT_RADIUS + 0.26f; 00023 00024 static const Real LONG_RANGE_MIN_DISTANCE = 0.12f; 00025 static const Real LONG_RANGE_RAY_START = FOOTBOT_RADIUS; 00026 static const Real LONG_RANGE_RAY_END = FOOTBOT_RADIUS + 1.42f; 00027 00028 static const Real SENSOR_ELEVATION = 0.123199866f; 00029 00030 /****************************************/ 00031 /****************************************/ 00032 00033 CFootBotDistanceScannerRotZOnlySensor::CFootBotDistanceScannerRotZOnlySensor() : 00034 m_pcRNG(NULL), 00035 m_bAddNoise(false), 00036 m_cSpace(CSimulator::GetInstance().GetSpace()), 00037 m_bShowRays(false) {} 00038 00039 /****************************************/ 00040 /****************************************/ 00041 00042 void CFootBotDistanceScannerRotZOnlySensor::Init(TConfigurationNode& t_tree) { 00043 try { 00044 CCI_FootBotDistanceScannerSensor::Init(t_tree); 00045 /* Show rays? */ 00046 GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays); 00047 /* Noise range */ 00048 GetNodeAttributeOrDefault(t_tree, "noise_range", m_cNoiseRange, m_cNoiseRange); 00049 if(m_cNoiseRange.GetSpan() > 0.0f) { 00050 m_bAddNoise = true; 00051 m_pcRNG = CRandom::CreateRNG("argos"); 00052 } 00053 } 00054 catch(CARGoSException& ex) { 00055 THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot distance scanner rot_z_only sensor.", ex); 00056 } 00057 } 00058 00059 /****************************************/ 00060 /****************************************/ 00061 00062 void CFootBotDistanceScannerRotZOnlySensor::SetRobot(CComposableEntity& c_entity) { 00063 m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body")); 00064 m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller")); 00065 m_pcDistScanEntity = &(c_entity.GetComponent<CFootBotDistanceScannerEquippedEntity>("distance_scanner")); 00066 m_pcDistScanEntity->SetCanBeEnabledIfDisabled(true); 00067 m_pcDistScanEntity->Enable(); 00068 } 00069 00070 /****************************************/ 00071 /****************************************/ 00072 00073 void CFootBotDistanceScannerRotZOnlySensor::Update() { 00074 /* Clear the maps */ 00075 m_tReadingsMap.clear(); 00076 m_tShortReadingsMap.clear(); 00077 m_tLongReadingsMap.clear(); 00078 /* Perform calculations only if the sensor is on */ 00079 if(m_pcDistScanEntity->GetMode() != CFootBotDistanceScannerEquippedEntity::MODE_OFF) { 00080 /* Update the readings wrt to device mode */ 00081 if(m_pcDistScanEntity->GetMode() == CFootBotDistanceScannerEquippedEntity::MODE_POSITION_CONTROL) { 00082 /* Sensor blocked in a position */ 00083 /* Recalculate the rays */ 00084 CalculateRaysNotRotating(); 00085 /* Save the rotation for next time */ 00086 m_cLastDistScanRotation = m_pcDistScanEntity->GetRotation(); 00087 /* Update the values */ 00088 UpdateNotRotating(); 00089 } 00090 else { 00091 /* Rotating sensor */ 00092 /* Recalculate the rays */ 00093 CalculateRaysRotating(); 00094 /* Update the values */ 00095 UpdateRotating(); 00096 /* Save the rotation for next time */ 00097 m_cLastDistScanRotation = m_pcDistScanEntity->GetRotation(); 00098 } 00099 } 00100 } 00101 00102 /****************************************/ 00103 /****************************************/ 00104 00105 void CFootBotDistanceScannerRotZOnlySensor::Reset() { 00106 /* Clear the maps */ 00107 m_tReadingsMap.clear(); 00108 m_tShortReadingsMap.clear(); 00109 m_tLongReadingsMap.clear(); 00110 /* Zero the last rotation */ 00111 m_cLastDistScanRotation = CRadians::ZERO; 00112 } 00113 00114 /****************************************/ 00115 /****************************************/ 00116 00117 void CFootBotDistanceScannerRotZOnlySensor::UpdateNotRotating() { 00118 /* Short range [0] */ 00119 CRadians cAngle = m_cLastDistScanRotation; 00120 cAngle.SignedNormalize(); 00121 Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE); 00122 m_tShortReadingsMap[cAngle] = fReading; 00123 m_tReadingsMap[cAngle] = fReading; 00124 /* Long range [1] */ 00125 cAngle += CRadians::PI_OVER_TWO; 00126 cAngle.SignedNormalize(); 00127 fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE); 00128 m_tLongReadingsMap[cAngle] = fReading; 00129 m_tReadingsMap[cAngle] = fReading; 00130 /* Short range [2] */ 00131 cAngle += CRadians::PI_OVER_TWO; 00132 cAngle.SignedNormalize(); 00133 fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE); 00134 m_tShortReadingsMap[cAngle] = fReading; 00135 m_tReadingsMap[cAngle] = fReading; 00136 /* Long range [3] */ 00137 cAngle += CRadians::PI_OVER_TWO; 00138 cAngle.SignedNormalize(); 00139 fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE); 00140 m_tLongReadingsMap[cAngle] = fReading; 00141 m_tReadingsMap[cAngle] = fReading; 00142 } 00143 00144 /****************************************/ 00145 /****************************************/ 00146 00147 #define ADD_READING(RAYS,MAP,INDEX,MINDIST) \ 00148 cAngle += cInterSensorSpan; \ 00149 cAngle.SignedNormalize(); \ 00150 fReading = CalculateReadingForRay(RAYS[INDEX],MINDIST); \ 00151 MAP[cAngle] = fReading; \ 00152 m_tReadingsMap[cAngle] = fReading; 00153 00154 #define ADD_READINGS(RAYS,MAP,MINDIST) \ 00155 ADD_READING(RAYS,MAP,1,MINDIST) \ 00156 ADD_READING(RAYS,MAP,2,MINDIST) \ 00157 ADD_READING(RAYS,MAP,3,MINDIST) \ 00158 ADD_READING(RAYS,MAP,4,MINDIST) \ 00159 ADD_READING(RAYS,MAP,5,MINDIST) 00160 00161 void CFootBotDistanceScannerRotZOnlySensor::UpdateRotating() { 00162 CRadians cInterSensorSpan = (m_pcDistScanEntity->GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f; 00163 CRadians cStartAngle = m_cLastDistScanRotation; 00164 /* Short range [0] */ 00165 CRadians cAngle = cStartAngle; 00166 cAngle.SignedNormalize(); 00167 Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE); 00168 m_tShortReadingsMap[cAngle] = fReading; 00169 m_tReadingsMap[cAngle] = fReading; 00170 ADD_READINGS(m_cShortRangeRays0, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE); 00171 /* Short range [2] */ 00172 cAngle = cStartAngle + CRadians::PI; 00173 cAngle.SignedNormalize(); 00174 fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE); 00175 m_tShortReadingsMap[cAngle] = fReading; 00176 m_tReadingsMap[cAngle] = fReading; 00177 ADD_READINGS(m_cShortRangeRays2, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE); 00178 /* Long range [1] */ 00179 cAngle = cStartAngle + CRadians::PI_OVER_TWO; 00180 cAngle.SignedNormalize(); 00181 fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE); 00182 m_tLongReadingsMap[cAngle] = fReading; 00183 m_tReadingsMap[cAngle] = fReading; 00184 ADD_READINGS(m_cLongRangeRays1, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE); 00185 /* Long range [3] */ 00186 cAngle = cStartAngle + CRadians::PI_OVER_TWO + CRadians::PI; 00187 cAngle.SignedNormalize(); 00188 fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE); 00189 m_tLongReadingsMap[cAngle] = fReading; 00190 m_tReadingsMap[cAngle] = fReading; 00191 ADD_READINGS(m_cLongRangeRays3, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE); 00192 } 00193 00194 /****************************************/ 00195 /****************************************/ 00196 00197 Real CFootBotDistanceScannerRotZOnlySensor::CalculateReadingForRay(const CRay3& c_ray, 00198 Real f_min_distance) { 00199 /* Get the closest intersection */ 00200 SEmbodiedEntityIntersectionItem sIntersection; 00201 if(GetClosestEmbodiedEntityIntersectedByRay(sIntersection, 00202 c_ray, 00203 *m_pcEmbodiedEntity)) { 00204 if(m_bShowRays) m_pcControllableEntity->AddIntersectionPoint(c_ray, sIntersection.TOnRay); 00205 /* There is an intersection! */ 00206 Real fDistance = c_ray.GetDistance(sIntersection.TOnRay); 00207 if(fDistance > f_min_distance) { 00208 /* The distance is returned in meters, but the reading must be in cm */ 00209 if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray); 00210 return fDistance * 100.0f; 00211 } 00212 else { 00213 /* The detected intersection was too close */ 00214 if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray); 00215 return -1.0f; 00216 } 00217 } 00218 else { 00219 /* No intersection */ 00220 if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(false, c_ray); 00221 return -2.0f; 00222 } 00223 } 00224 00225 /****************************************/ 00226 /****************************************/ 00227 00228 /* Highly reuse the vectors to speed up the computation */ 00229 #define CALCULATE_SHORT_RANGE_RAY(ANGLE,INDEX) \ 00230 m_cDirection.RotateZ(ANGLE); \ 00231 m_cOriginRayStart = m_cDirection; \ 00232 m_cOriginRayEnd = m_cDirection; \ 00233 m_cOriginRayStart *= SHORT_RANGE_RAY_START; \ 00234 m_cOriginRayEnd *= SHORT_RANGE_RAY_END; \ 00235 m_cRayStart = m_pcEmbodiedEntity->GetPosition(); \ 00236 m_cRayStart += m_cOriginRayStart; \ 00237 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \ 00238 m_cRayEnd = m_pcEmbodiedEntity->GetPosition(); \ 00239 m_cRayEnd += m_cOriginRayEnd; \ 00240 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \ 00241 m_cShortRangeRays0[INDEX].Set(m_cRayStart, m_cRayEnd); \ 00242 m_cRayStart = m_pcEmbodiedEntity->GetPosition(); \ 00243 m_cRayStart -= m_cOriginRayStart; \ 00244 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \ 00245 m_cRayEnd = m_pcEmbodiedEntity->GetPosition(); \ 00246 m_cRayEnd -= m_cOriginRayEnd; \ 00247 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \ 00248 m_cShortRangeRays2[INDEX].Set(m_cRayStart, m_cRayEnd); \ 00249 \ 00250 /* Highly reuse the vectors to speed up the computation */ 00251 #define CALCULATE_LONG_RANGE_RAY(ANGLE,INDEX) \ 00252 m_cDirection.RotateZ(ANGLE); \ 00253 m_cOriginRayStart = m_cDirection; \ 00254 m_cOriginRayEnd = m_cDirection; \ 00255 m_cOriginRayStart *= LONG_RANGE_RAY_START; \ 00256 m_cOriginRayEnd *= LONG_RANGE_RAY_END; \ 00257 m_cRayStart = m_pcEmbodiedEntity->GetPosition(); \ 00258 m_cRayStart += m_cOriginRayStart; \ 00259 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \ 00260 m_cRayEnd = m_pcEmbodiedEntity->GetPosition(); \ 00261 m_cRayEnd += m_cOriginRayEnd; \ 00262 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \ 00263 m_cLongRangeRays1[INDEX].Set(m_cRayStart, m_cRayEnd); \ 00264 m_cRayStart = m_pcEmbodiedEntity->GetPosition(); \ 00265 m_cRayStart -= m_cOriginRayStart; \ 00266 m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \ 00267 m_cRayEnd = m_pcEmbodiedEntity->GetPosition(); \ 00268 m_cRayEnd -= m_cOriginRayEnd; \ 00269 m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION); \ 00270 m_cLongRangeRays3[INDEX].Set(m_cRayStart, m_cRayEnd); 00271 00272 /****************************************/ 00273 /****************************************/ 00274 00275 void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysNotRotating() { 00276 /* We make the assumption that the foot-bot is rotated only around Z */ 00277 /* Get the foot-bot orientation */ 00278 CRadians cTmp1, cTmp2, cOrientationZ; 00279 m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); 00280 /* Sum the distance scanner orientation */ 00281 cOrientationZ += m_pcDistScanEntity->GetRotation(); 00282 /* Calculate the 2D vector representing this rotation */ 00283 CVector2 cAbsoluteOrientation(1.0, cOrientationZ); 00284 /* The short range sensors are oriented along the foot-bot local X */ 00285 m_cDirection = CVector3::X; 00286 CALCULATE_SHORT_RANGE_RAY(cAbsoluteOrientation, 0); 00287 /* The short range sensors are oriented along the foot-bot local Y */ 00288 m_cDirection = CVector3::Y; 00289 CALCULATE_LONG_RANGE_RAY(cAbsoluteOrientation, 0); 00290 } 00291 00292 /****************************************/ 00293 /****************************************/ 00294 00295 void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysRotating() { 00296 /* We make the assumption that the foot-bot is rotated only around Z */ 00297 /* Get the foot-bot orientation */ 00298 CRadians cTmp1, cTmp2, cOrientationZ; 00299 m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); 00300 /* Sum the distance scanner orientation */ 00301 cOrientationZ += m_cLastDistScanRotation; 00302 /* Calculate the 2D vector representing this rotation */ 00303 CVector2 cAbsoluteOrientation(1.0, cOrientationZ); 00304 /* The sensor is rotating, so calculate the span between each successive ray */ 00305 CVector2 cInterSensorSpan(1.0f, (m_pcDistScanEntity->GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f); 00306 /* The short range sensors are oriented along the foot-bot local X */ 00307 m_cDirection = CVector3::X; 00308 CALCULATE_SHORT_RANGE_RAY(cAbsoluteOrientation, 0); 00309 CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 1); 00310 CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 2); 00311 CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 3); 00312 CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 4); 00313 CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 5); 00314 /* The long range sensors are oriented along the foot-bot local Y */ 00315 m_cDirection = CVector3::Y; 00316 CALCULATE_LONG_RANGE_RAY(cAbsoluteOrientation, 0); 00317 CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 1); 00318 CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 2); 00319 CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 3); 00320 CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 4); 00321 CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 5); 00322 } 00323 00324 /****************************************/ 00325 /****************************************/ 00326 00327 REGISTER_SENSOR(CFootBotDistanceScannerRotZOnlySensor, 00328 "footbot_distance_scanner", "rot_z_only", 00329 "Carlo Pinciroli [ilpincy@gmail.com]", 00330 "1.0", 00331 "The foot-bot distance scanner sensor (optimized for 2D).", 00332 "This sensor accesses the foot-bot distance scanner sensor. For a complete\n" 00333 "description of its usage, refer to the common interface.\n" 00334 "In this implementation, the readings are calculated under the assumption that\n" 00335 "the foot-bot is always parallel to the XY plane, i.e., it rotates only around\n" 00336 "the Z axis. This implementation is faster than a 3D one and should be used\n" 00337 "only when the assumption about the foot-bot rotation holds.\n\n" 00338 "REQUIRED XML CONFIGURATION\n\n" 00339 " <controllers>\n" 00340 " ...\n" 00341 " <my_controller ...>\n" 00342 " ...\n" 00343 " <sensors>\n" 00344 " ...\n" 00345 " <footbot_distance_scanner implementation=\"rot_z_only\" />\n" 00346 " ...\n" 00347 " </sensors>\n" 00348 " ...\n" 00349 " </my_controller>\n" 00350 " ...\n" 00351 " </controllers>\n\n" 00352 "OPTIONAL XML CONFIGURATION\n\n" 00353 "It is possible to draw the rays shot by the distance scanner in the OpenGL\n" 00354 "visualization. This can be useful for sensor debugging but also to understand\n" 00355 "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n" 00356 "they are not obstructed and in purple when they are. In case a ray is\n" 00357 "obstructed, a black dot is drawn where the intersection occurred.\n" 00358 "To turn this functionality on, add the attribute 'show_rays=\"true\"' in the\n" 00359 "XML as in this example:\n\n" 00360 " <controllers>\n" 00361 " ...\n" 00362 " <my_controller ...>\n" 00363 " ...\n" 00364 " <sensors>\n" 00365 " ...\n" 00366 " <footbot_distance_scanner implementation=\"rot_z_only\"\n" 00367 " show_rays=\"true\" />\n" 00368 " ...\n" 00369 " </sensors>\n" 00370 " ...\n" 00371 " </my_controller>\n" 00372 " ...\n" 00373 " </controllers>\n", 00374 "Usable" 00375 ); 00376 00377 }