ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/foot-bot/simulator/footbot_light_rotzonly_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 "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 }