ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include <argos3/core/simulator/simulator.h> 00008 #include <argos3/core/simulator/entity/embodied_entity.h> 00009 #include <argos3/core/simulator/entity/composable_entity.h> 00010 #include <argos3/plugins/simulator/entities/light_entity.h> 00011 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h> 00012 00013 #include "footbot_light_rotzonly_sensor.h" 00014 00015 namespace argos { 00016 00017 /****************************************/ 00018 /****************************************/ 00019 00020 static CRange<Real> SENSOR_RANGE(0.0f, 1.0f); 00021 static CRadians SENSOR_SPACING = CRadians(ARGOS_PI / 12.0f); 00022 static CRadians SENSOR_HALF_SPACING = SENSOR_SPACING * 0.5; 00023 00024 /****************************************/ 00025 /****************************************/ 00026 00027 static SInt32 Modulo(SInt32 n_value, SInt32 un_modulo) { 00028 while(n_value < 0) n_value += un_modulo; 00029 while(n_value >= un_modulo) n_value -= un_modulo; 00030 return n_value; 00031 } 00032 00033 static Real ComputeReading(Real f_distance) { 00034 if(f_distance > 2.5f) { 00035 return 0.0f; 00036 } 00037 else { 00038 return ::exp(-f_distance * 2.0f); 00039 } 00040 } 00041 00042 static Real ScaleReading(const CRadians& c_angular_distance) { 00043 if(c_angular_distance > CRadians::PI_OVER_TWO) { 00044 return 0.0f; 00045 } 00046 else { 00047 return (1.0f - 2.0f * c_angular_distance / CRadians::PI); 00048 } 00049 } 00050 00051 /****************************************/ 00052 /****************************************/ 00053 00054 CFootBotLightRotZOnlySensor::CFootBotLightRotZOnlySensor() : 00055 m_pcEmbodiedEntity(NULL), 00056 m_bShowRays(false), 00057 m_pcRNG(NULL), 00058 m_bAddNoise(false), 00059 m_cSpace(CSimulator::GetInstance().GetSpace()) {} 00060 00061 /****************************************/ 00062 /****************************************/ 00063 00064 void CFootBotLightRotZOnlySensor::SetRobot(CComposableEntity& c_entity) { 00065 try { 00066 m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body")); 00067 m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller")); 00068 m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors")); 00069 m_pcLightEntity->SetCanBeEnabledIfDisabled(true); 00070 m_pcLightEntity->Enable(); 00071 } 00072 catch(CARGoSException& ex) { 00073 THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the foot-bot light default sensor", ex); 00074 } 00075 } 00076 00077 /****************************************/ 00078 /****************************************/ 00079 00080 void CFootBotLightRotZOnlySensor::Init(TConfigurationNode& t_tree) { 00081 try { 00082 /* Show rays? */ 00083 GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays); 00084 /* Parse noise level */ 00085 Real fNoiseLevel = 0.0f; 00086 GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel); 00087 if(fNoiseLevel < 0.0f) { 00088 THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor"); 00089 } 00090 else if(fNoiseLevel > 0.0f) { 00091 m_bAddNoise = true; 00092 m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel); 00093 m_pcRNG = CRandom::CreateRNG("argos"); 00094 } 00095 m_tReadings.resize(m_pcLightEntity->GetNumSensors()); 00096 } 00097 catch(CARGoSException& ex) { 00098 THROW_ARGOSEXCEPTION_NESTED("Initialization error in rot_z_only light sensor", ex); 00099 } 00100 } 00101 00102 /****************************************/ 00103 /****************************************/ 00104 00105 void CFootBotLightRotZOnlySensor::Update() { 00106 /* Erase readings */ 00107 for(size_t i = 0; i < m_tReadings.size(); ++i) { 00108 m_tReadings[i].Value = 0.0f; 00109 } 00110 /* Get foot-bot orientation */ 00111 CRadians cTmp1, cTmp2, cOrientationZ; 00112 m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); 00113 /* Ray used for scanning the environment for obstacles */ 00114 CRay3 cOcclusionCheckRay; 00115 cOcclusionCheckRay.SetStart(m_pcEmbodiedEntity->GetPosition()); 00116 CVector3 cRobotToLight; 00117 /* Buffer for the angle of the light wrt to the foot-bot */ 00118 CRadians cAngleLightWrtFootbot; 00119 /* Buffers to contain data about the intersection */ 00120 SEmbodiedEntityIntersectionItem sIntersection; 00121 /* List of light entities */ 00122 CSpace::TMapPerType& mapLights = m_cSpace.GetEntitiesByType("light"); 00123 /* 00124 * 1. go through the list of light entities in the scene 00125 * 2. check if a light is occluded 00126 * 3. if it isn't, distribute the reading across the sensors 00127 * NOTE: the readings are additive 00128 * 4. go through the sensors and clamp their values 00129 */ 00130 for(CSpace::TMapPerType::iterator it = mapLights.begin(); 00131 it != mapLights.end(); 00132 ++it) { 00133 /* Get a reference to the light */ 00134 CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second)); 00135 /* Consider the light only if it has non zero intensity */ 00136 if(cLight.GetIntensity() > 0.0f) { 00137 /* Set the ray end */ 00138 cOcclusionCheckRay.SetEnd(cLight.GetPosition()); 00139 /* Check occlusion between the foot-bot and the light */ 00140 if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection, 00141 cOcclusionCheckRay, 00142 *m_pcEmbodiedEntity)) { 00143 /* The light is not occluded */ 00144 if(m_bShowRays) { 00145 m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay); 00146 } 00147 /* Get the distance between the light and the foot-bot */ 00148 cOcclusionCheckRay.ToVector(cRobotToLight); 00149 /* 00150 * Linearly scale the distance with the light intensity 00151 * The greater the intensity, the smaller the distance 00152 */ 00153 cRobotToLight /= cLight.GetIntensity(); 00154 /* Get the angle wrt to foot-bot rotation */ 00155 cAngleLightWrtFootbot = cRobotToLight.GetZAngle(); 00156 cAngleLightWrtFootbot -= cOrientationZ; 00157 /* 00158 * Find closest sensor index to point at which ray hits footbot body 00159 * Rotate whole body by half a sensor spacing (corresponding to placement of first sensor) 00160 * Division says how many sensor spacings there are between first sensor and point at which ray hits footbot body 00161 * Increase magnitude of result of division to ensure correct rounding 00162 */ 00163 Real fIdx = (cAngleLightWrtFootbot - SENSOR_HALF_SPACING) / SENSOR_SPACING; 00164 SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f; 00165 /* Set the actual readings */ 00166 Real fReading = cRobotToLight.Length(); 00167 /* 00168 * Take 6 readings before closest sensor and 6 readings after - thus we 00169 * process sensors that are with 180 degrees of intersection of light 00170 * ray with robot body 00171 */ 00172 for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) { 00173 UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 24); 00174 CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtFootbot - m_tReadings[unIdx].Angle).SignedNormalize()); 00175 /* 00176 * ComputeReading gives value as if sensor was perfectly in line with 00177 * light ray. We then linearly decrease actual reading from 1 (dist 00178 * 0) to 0 (dist PI/2) 00179 */ 00180 m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint); 00181 } 00182 } 00183 else { 00184 /* The ray is occluded */ 00185 if(m_bShowRays) { 00186 m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay); 00187 m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersection.TOnRay); 00188 } 00189 } 00190 } 00191 } 00192 /* Apply noise to the sensors */ 00193 if(m_bAddNoise) { 00194 for(size_t i = 0; i < 24; ++i) { 00195 m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); 00196 } 00197 } 00198 /* Trunc the reading between 0 and 1 */ 00199 for(size_t i = 0; i < 24; ++i) { 00200 SENSOR_RANGE.TruncValue(m_tReadings[i].Value); 00201 } 00202 } 00203 00204 /****************************************/ 00205 /****************************************/ 00206 00207 void CFootBotLightRotZOnlySensor::Reset() { 00208 for(UInt32 i = 0; i < GetReadings().size(); ++i) { 00209 m_tReadings[i].Value = 0.0f; 00210 } 00211 } 00212 00213 /****************************************/ 00214 /****************************************/ 00215 00216 REGISTER_SENSOR(CFootBotLightRotZOnlySensor, 00217 "footbot_light", "rot_z_only", 00218 "Carlo Pinciroli [ilpincy@gmail.com]", 00219 "1.0", 00220 "The foot-bot light sensor (optimized for 2D).", 00221 "This sensor accesses a set of light sensors. The sensors all return a value\n" 00222 "between 0 and 1, where 0 means nothing within range and 1 means the perceived\n" 00223 "light saturates the sensor. Values between 0 and 1 depend on the distance of\n" 00224 "the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n" 00225 "distance between a sensor and the light, and I is the reference intensity of the\n" 00226 "perceived light. The reference intensity corresponds to the minimum distance at\n" 00227 "which the light saturates a sensor. The reference intensity depends on the\n" 00228 "individual light, and it is set with the \"intensity\" attribute of the light\n" 00229 "entity. In case multiple lights are present in the environment, each sensor\n" 00230 "reading is calculated as the sum of the individual readings due to each light.\n" 00231 "In other words, light wave interference is not taken into account. In\n" 00232 "controllers, you must include the ci_light_sensor.h header.\n\n" 00233 "REQUIRED XML CONFIGURATION\n\n" 00234 " <controllers>\n" 00235 " ...\n" 00236 " <my_controller ...>\n" 00237 " ...\n" 00238 " <sensors>\n" 00239 " ...\n" 00240 " <light implementation=\"rot_z_only\" />\n" 00241 " ...\n" 00242 " </sensors>\n" 00243 " ...\n" 00244 " </my_controller>\n" 00245 " ...\n" 00246 " </controllers>\n\n" 00247 "OPTIONAL XML CONFIGURATION\n\n" 00248 "It is possible to draw the rays shot by the light sensor in the OpenGL\n" 00249 "visualization. This can be useful for sensor debugging but also to understand\n" 00250 "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n" 00251 "they are not obstructed and in purple when they are. In case a ray is\n" 00252 "obstructed, a black dot is drawn where the intersection occurred.\n" 00253 "To turn this functionality on, add the attribute \"show_rays\" as in this\n" 00254 "example:\n\n" 00255 " <controllers>\n" 00256 " ...\n" 00257 " <my_controller ...>\n" 00258 " ...\n" 00259 " <sensors>\n" 00260 " ...\n" 00261 " <light implementation=\"rot_z_only\"\n" 00262 " show_rays=\"true\" />\n" 00263 " ...\n" 00264 " </sensors>\n" 00265 " ...\n" 00266 " </my_controller>\n" 00267 " ...\n" 00268 " </controllers>\n\n" 00269 "It is possible to add uniform noise to the sensors, thus matching the\n" 00270 "characteristics of a real robot better. This can be done with the attribute\n" 00271 "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n" 00272 "reading. The final sensor reading is always normalized in the [0-1] range.\n\n" 00273 " <controllers>\n" 00274 " ...\n" 00275 " <my_controller ...>\n" 00276 " ...\n" 00277 " <sensors>\n" 00278 " ...\n" 00279 " <light implementation=\"rot_z_only\"\n" 00280 " noise_level=\"0.1\" />\n" 00281 " ...\n" 00282 " </sensors>\n" 00283 " ...\n" 00284 " </my_controller>\n" 00285 " ...\n" 00286 " </controllers>\n\n" 00287 "OPTIONAL XML CONFIGURATION\n\n" 00288 "None.\n", 00289 "Usable" 00290 ); 00291 00292 }