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 "light_default_sensor.h" 00014 00015 namespace argos { 00016 00017 /****************************************/ 00018 /****************************************/ 00019 00020 static CRange<Real> UNIT(0.0f, 1.0f); 00021 00022 /****************************************/ 00023 /****************************************/ 00024 00025 CLightDefaultSensor::CLightDefaultSensor() : 00026 m_pcEmbodiedEntity(NULL), 00027 m_bShowRays(false), 00028 m_pcRNG(NULL), 00029 m_bAddNoise(false), 00030 m_cSpace(CSimulator::GetInstance().GetSpace()) {} 00031 00032 /****************************************/ 00033 /****************************************/ 00034 00035 void CLightDefaultSensor::SetRobot(CComposableEntity& c_entity) { 00036 try { 00037 m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body")); 00038 m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller")); 00039 m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors")); 00040 m_pcLightEntity->SetCanBeEnabledIfDisabled(true); 00041 m_pcLightEntity->Enable(); 00042 } 00043 catch(CARGoSException& ex) { 00044 THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the light default sensor", ex); 00045 } 00046 } 00047 00048 /****************************************/ 00049 /****************************************/ 00050 00051 void CLightDefaultSensor::Init(TConfigurationNode& t_tree) { 00052 try { 00053 CCI_LightSensor::Init(t_tree); 00054 /* Show rays? */ 00055 GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays); 00056 /* Parse noise level */ 00057 Real fNoiseLevel = 0.0f; 00058 GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel); 00059 if(fNoiseLevel < 0.0f) { 00060 THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor"); 00061 } 00062 else if(fNoiseLevel > 0.0f) { 00063 m_bAddNoise = true; 00064 m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel); 00065 m_pcRNG = CRandom::CreateRNG("argos"); 00066 } 00067 m_tReadings.resize(m_pcLightEntity->GetNumSensors()); 00068 } 00069 catch(CARGoSException& ex) { 00070 THROW_ARGOSEXCEPTION_NESTED("Initialization error in default light sensor", ex); 00071 } 00072 } 00073 00074 /****************************************/ 00075 /****************************************/ 00076 00077 void CLightDefaultSensor::Update() { 00078 /* Erase readings */ 00079 for(size_t i = 0; i < m_tReadings.size(); ++i) m_tReadings[i] = 0.0f; 00080 /* Ray used for scanning the environment for obstacles */ 00081 CRay3 cScanningRay; 00082 CVector3 cRayStart; 00083 CVector3 cSensorToLight; 00084 /* Buffers to contain data about the intersection */ 00085 SEmbodiedEntityIntersectionItem sIntersection; 00086 /* List of light entities */ 00087 CSpace::TMapPerType& mapLights = m_cSpace.GetEntitiesByType("light"); 00088 /* Go through the sensors */ 00089 for(UInt32 i = 0; i < m_tReadings.size(); ++i) { 00090 /* Set ray start */ 00091 cRayStart = m_pcLightEntity->GetSensor(i).Position; 00092 cRayStart.Rotate(m_pcEmbodiedEntity->GetOrientation()); 00093 cRayStart += m_pcEmbodiedEntity->GetPosition(); 00094 /* Go through all the light entities */ 00095 for(CSpace::TMapPerType::iterator it = mapLights.begin(); 00096 it != mapLights.end(); 00097 ++it) { 00098 /* Get a reference to the light */ 00099 CLightEntity& cLight = *any_cast<CLightEntity*>(it->second); 00100 /* Consider the light only if it has non zero intensity */ 00101 if(cLight.GetIntensity() > 0.0f) { 00102 /* Set ray end to light position */ 00103 cScanningRay.Set(cRayStart, cLight.GetPosition()); 00104 /* Check occlusions */ 00105 if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection, 00106 cScanningRay)) { 00107 /* No occlusion, the light is visibile */ 00108 if(m_bShowRays) { 00109 m_pcControllableEntity->AddCheckedRay(false, cScanningRay); 00110 } 00111 /* Calculate reading */ 00112 cScanningRay.ToVector(cSensorToLight); 00113 m_tReadings[i] += CalculateReading(cSensorToLight.Length(), 00114 cLight.GetIntensity()); 00115 } 00116 else { 00117 /* There is an occlusion, the light is not visible */ 00118 if(m_bShowRays) { 00119 m_pcControllableEntity->AddIntersectionPoint(cScanningRay, 00120 sIntersection.TOnRay); 00121 m_pcControllableEntity->AddCheckedRay(true, cScanningRay); 00122 } 00123 } 00124 } 00125 } 00126 /* Apply noise to the sensor */ 00127 if(m_bAddNoise) { 00128 m_tReadings[i] += m_pcRNG->Uniform(m_cNoiseRange); 00129 } 00130 /* Trunc the reading between 0 and 1 */ 00131 UNIT.TruncValue(m_tReadings[i]); 00132 } 00133 } 00134 00135 /****************************************/ 00136 /****************************************/ 00137 00138 void CLightDefaultSensor::Reset() { 00139 for(UInt32 i = 0; i < GetReadings().size(); ++i) { 00140 m_tReadings[i] = 0.0f; 00141 } 00142 } 00143 00144 /****************************************/ 00145 /****************************************/ 00146 00147 Real CLightDefaultSensor::CalculateReading(Real f_distance, Real f_intensity) { 00148 return (f_intensity * f_intensity) / (f_distance * f_distance); 00149 } 00150 00151 /****************************************/ 00152 /****************************************/ 00153 00154 REGISTER_SENSOR(CLightDefaultSensor, 00155 "light", "default", 00156 "Carlo Pinciroli [ilpincy@gmail.com]", 00157 "1.0", 00158 "A generic light sensor.", 00159 "This sensor accesses a set of light sensors. The sensors all return a value\n" 00160 "between 0 and 1, where 0 means nothing within range and 1 means the perceived\n" 00161 "light saturates the sensor. Values between 0 and 1 depend on the distance of\n" 00162 "the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n" 00163 "distance between a sensor and the light, and I is the reference intensity of the\n" 00164 "perceived light. The reference intensity corresponds to the minimum distance at\n" 00165 "which the light saturates a sensor. The reference intensity depends on the\n" 00166 "individual light, and it is set with the \"intensity\" attribute of the light\n" 00167 "entity. In case multiple lights are present in the environment, each sensor\n" 00168 "reading is calculated as the sum of the individual readings due to each light.\n" 00169 "In other words, light wave interference is not taken into account. In\n" 00170 "controllers, you must include the ci_light_sensor.h header.\n\n" 00171 "REQUIRED XML CONFIGURATION\n\n" 00172 " <controllers>\n" 00173 " ...\n" 00174 " <my_controller ...>\n" 00175 " ...\n" 00176 " <sensors>\n" 00177 " ...\n" 00178 " <light implementation=\"default\" />\n" 00179 " ...\n" 00180 " </sensors>\n" 00181 " ...\n" 00182 " </my_controller>\n" 00183 " ...\n" 00184 " </controllers>\n\n" 00185 "OPTIONAL XML CONFIGURATION\n\n" 00186 "It is possible to draw the rays shot by the light sensor in the OpenGL\n" 00187 "visualization. This can be useful for sensor debugging but also to understand\n" 00188 "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n" 00189 "they are not obstructed and in purple when they are. In case a ray is\n" 00190 "obstructed, a black dot is drawn where the intersection occurred.\n" 00191 "To turn this functionality on, add the attribute \"show_rays\" as in this\n" 00192 "example:\n\n" 00193 " <controllers>\n" 00194 " ...\n" 00195 " <my_controller ...>\n" 00196 " ...\n" 00197 " <sensors>\n" 00198 " ...\n" 00199 " <light implementation=\"default\"\n" 00200 " show_rays=\"true\" />\n" 00201 " ...\n" 00202 " </sensors>\n" 00203 " ...\n" 00204 " </my_controller>\n" 00205 " ...\n" 00206 " </controllers>\n\n" 00207 "It is possible to add uniform noise to the sensors, thus matching the\n" 00208 "characteristics of a real robot better. This can be done with the attribute\n" 00209 "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n" 00210 "reading. The final sensor reading is always normalized in the [0-1] range.\n\n" 00211 " <controllers>\n" 00212 " ...\n" 00213 " <my_controller ...>\n" 00214 " ...\n" 00215 " <sensors>\n" 00216 " ...\n" 00217 " <light implementation=\"default\"\n" 00218 " noise_level=\"0.1\" />\n" 00219 " ...\n" 00220 " </sensors>\n" 00221 " ...\n" 00222 " </my_controller>\n" 00223 " ...\n" 00224 " </controllers>\n\n" 00225 "OPTIONAL XML CONFIGURATION\n\n" 00226 "None.\n", 00227 "Usable" 00228 ); 00229 00230 }