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/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 }