ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/proximity_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/proximity_sensor_equipped_entity.h>
00011 
00012 #include "proximity_default_sensor.h"
00013 
00014 namespace argos {
00015 
00016    /****************************************/
00017    /****************************************/
00018 
00019    static CRange<Real> UNIT(0.0f, 1.0f);
00020 
00021    /****************************************/
00022    /****************************************/
00023 
00024    CProximityDefaultSensor::CProximityDefaultSensor() :
00025       m_pcEmbodiedEntity(NULL),
00026       m_bShowRays(false),
00027       m_pcRNG(NULL),
00028       m_bAddNoise(false),
00029       m_cSpace(CSimulator::GetInstance().GetSpace()) {}
00030 
00031    /****************************************/
00032    /****************************************/
00033 
00034    void CProximityDefaultSensor::SetRobot(CComposableEntity& c_entity) {
00035       try {
00036          m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
00037          m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
00038          m_pcProximityEntity = &(c_entity.GetComponent<CProximitySensorEquippedEntity>("proximity_sensors"));
00039          m_pcProximityEntity->SetCanBeEnabledIfDisabled(true);
00040          m_pcProximityEntity->Enable();
00041       }
00042       catch(CARGoSException& ex) {
00043          THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the proximity default sensor", ex);
00044       }
00045    }
00046 
00047    /****************************************/
00048    /****************************************/
00049 
00050    void CProximityDefaultSensor::Init(TConfigurationNode& t_tree) {
00051       try {
00052          CCI_ProximitySensor::Init(t_tree);
00053          /* Show rays? */
00054          GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
00055          /* Parse noise level */
00056          Real fNoiseLevel = 0.0f;
00057          GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
00058          if(fNoiseLevel < 0.0f) {
00059             THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the proximity sensor");
00060          }
00061          else if(fNoiseLevel > 0.0f) {
00062             m_bAddNoise = true;
00063             m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
00064             m_pcRNG = CRandom::CreateRNG("argos");
00065          }
00066          m_tReadings.resize(m_pcProximityEntity->GetNumSensors());
00067       }
00068       catch(CARGoSException& ex) {
00069          THROW_ARGOSEXCEPTION_NESTED("Initialization error in default proximity sensor", ex);
00070       }
00071    }
00072 
00073    /****************************************/
00074    /****************************************/
00075    
00076    void CProximityDefaultSensor::Update() {
00077       /* Ray used for scanning the environment for obstacles */
00078       CRay3 cScanningRay;
00079       CVector3 cRayStart, cRayEnd;
00080       /* Buffers to contain data about the intersection */
00081       SEmbodiedEntityIntersectionItem sIntersection;
00082       /* Go through the sensors */
00083       for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
00084          /* Compute ray for sensor i */
00085          cRayStart = m_pcProximityEntity->GetSensor(i).Offset;
00086          cRayStart.Rotate(m_pcEmbodiedEntity->GetOrientation());
00087          cRayStart += m_pcEmbodiedEntity->GetPosition();
00088          cRayEnd = m_pcProximityEntity->GetSensor(i).Offset;
00089          cRayEnd += m_pcProximityEntity->GetSensor(i).Direction;
00090          cRayEnd.Rotate(m_pcEmbodiedEntity->GetOrientation());
00091          cRayEnd += m_pcEmbodiedEntity->GetPosition();
00092          cScanningRay.Set(cRayStart,cRayEnd);
00093          /* Compute reading */
00094          /* Get the closest intersection */
00095          if(GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
00096                                                      cScanningRay,
00097                                                      *m_pcEmbodiedEntity)) {
00098             /* There is an intersection */
00099             if(m_bShowRays) {
00100                m_pcControllableEntity->AddIntersectionPoint(cScanningRay,
00101                                                             sIntersection.TOnRay);
00102                m_pcControllableEntity->AddCheckedRay(true, cScanningRay);
00103             }
00104             m_tReadings[i] = CalculateReading(cScanningRay.GetDistance(sIntersection.TOnRay));
00105          }
00106          else {
00107             /* No intersection */
00108             m_tReadings[i] = 0.0f;
00109             if(m_bShowRays) {
00110                m_pcControllableEntity->AddCheckedRay(false, cScanningRay);
00111             }
00112          }
00113          /* Apply noise to the sensor */
00114          if(m_bAddNoise) {
00115             m_tReadings[i] += m_pcRNG->Uniform(m_cNoiseRange);
00116          }
00117          /* Trunc the reading between 0 and 1 */
00118          UNIT.TruncValue(m_tReadings[i]);
00119       }
00120    }
00121 
00122    /****************************************/
00123    /****************************************/
00124 
00125    void CProximityDefaultSensor::Reset() {
00126       for(UInt32 i = 0; i < GetReadings().size(); ++i) {
00127          m_tReadings[i] = 0.0f;
00128       }
00129    }
00130 
00131    /****************************************/
00132    /****************************************/
00133 
00134    Real CProximityDefaultSensor::CalculateReading(Real f_distance) {
00135       return Exp(-f_distance);
00136    }
00137 
00138    /****************************************/
00139    /****************************************/
00140 
00141    REGISTER_SENSOR(CProximityDefaultSensor,
00142                    "proximity", "default",
00143                    "Carlo Pinciroli [ilpincy@gmail.com]",
00144                    "1.0",
00145                    "A generic proximity sensor.",
00146                    "This sensor accesses a set of proximity sensors. The sensors all return a value\n"
00147                    "between 0 and 1, where 0 means nothing within range and 1 means an external\n"
00148                    "object is touching the sensor. Values between 0 and 1 depend on the distance of\n"
00149                    "the occluding object, and are calculated as value=exp(-distance). In\n"
00150                    "controllers, you must include the ci_proximity_sensor.h header.\n\n"
00151                    "REQUIRED XML CONFIGURATION\n\n"
00152                    "  <controllers>\n"
00153                    "    ...\n"
00154                    "    <my_controller ...>\n"
00155                    "      ...\n"
00156                    "      <sensors>\n"
00157                    "        ...\n"
00158                    "        <proximity implementation=\"default\" />\n"
00159                    "        ...\n"
00160                    "      </sensors>\n"
00161                    "      ...\n"
00162                    "    </my_controller>\n"
00163                    "    ...\n"
00164                    "  </controllers>\n\n"
00165                    "OPTIONAL XML CONFIGURATION\n\n"
00166                    "It is possible to draw the rays shot by the proximity sensor in the OpenGL\n"
00167                    "visualization. This can be useful for sensor debugging but also to understand\n"
00168                    "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
00169                    "they are not obstructed and in purple when they are. In case a ray is\n"
00170                    "obstructed, a black dot is drawn where the intersection occurred.\n"
00171                    "To turn this functionality on, add the attribute \"show_rays\" as in this\n"
00172                    "example:\n\n"
00173                    "  <controllers>\n"
00174                    "    ...\n"
00175                    "    <my_controller ...>\n"
00176                    "      ...\n"
00177                    "      <sensors>\n"
00178                    "        ...\n"
00179                    "        <proximity implementation=\"default\"\n"
00180                    "                   show_rays=\"true\" />\n"
00181                    "        ...\n"
00182                    "      </sensors>\n"
00183                    "      ...\n"
00184                    "    </my_controller>\n"
00185                    "    ...\n"
00186                    "  </controllers>\n\n"
00187                    "It is possible to add uniform noise to the sensors, thus matching the\n"
00188                    "characteristics of a real robot better. This can be done with the attribute\n"
00189                    "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
00190                    "reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
00191                    "  <controllers>\n"
00192                    "    ...\n"
00193                    "    <my_controller ...>\n"
00194                    "      ...\n"
00195                    "      <sensors>\n"
00196                    "        ...\n"
00197                    "        <proximity implementation=\"default\"\n"
00198                    "                   noise_level=\"0.1\" />\n"
00199                    "        ...\n"
00200                    "      </sensors>\n"
00201                    "      ...\n"
00202                    "    </my_controller>\n"
00203                    "    ...\n"
00204                    "  </controllers>\n\n",
00205                    "Usable"
00206                   );
00207 
00208 }