ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/light_default_sensor.cpp
Go to the documentation of this file.
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 }