void CEPuckLightSensor::Update() { /* Here we assume that the e-puck is rotated only wrt to the Z axis */ /* Erase readings */ for(size_t i = 0; i < m_tReadings.size(); ++i) { m_tReadings[i].Value = 0.0f; } /* Get e-puck position */ const CVector3& cEPuckPosition = GetEntity().GetEmbodiedEntity().GetPosition(); /* Get e-puck orientation */ CRadians cTmp1, cTmp2, cOrientationZ; GetEntity().GetEmbodiedEntity().GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2); /* Buffer for calculating the light--e-puck distance */ CVector3 cLightDistance; /* Buffer for the angle of the sensor wrt to the e-puck */ CRadians cLightAngle; /* Initialize the occlusion check ray start to the baseline of the e-puck */ CRay cOcclusionCheckRay; cOcclusionCheckRay.SetStart(cEPuckPosition); /* Buffer to store the intersection data */ CSpace::SEntityIntersectionItem<CEmbodiedEntity> sIntersectionData; /* Ignore the sensing ropuck when checking for occlusions */ TEmbodiedEntitySet tIgnoreEntities; tIgnoreEntities.insert(&GetEntity().GetEmbodiedEntity()); /* * 1. go through the list of light entities in the scene * 2. check if a light is occluded * 3. if it isn't, distribute the reading across the sensors * NOTE: the readings are additive * 4. go through the sensors and clamp their values */ try{ CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("light_entity"); for(CSpace::TAnyEntityMap::iterator it = tEntityMap.begin(); it != tEntityMap.end(); ++it) { /* Get a reference to the light */ CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second)); /* Consider the light only if it has non zero intensity */ if(cLight.GetIntensity() > 0.0f) { /* Get the light position */ const CVector3& cLightPosition = cLight.GetPosition(); /* Set the ray end */ cOcclusionCheckRay.SetEnd(cLightPosition); /* Check occlusion between the e-puck and the light */ if(! m_cSpace.GetClosestEmbodiedEntityIntersectedByRay(sIntersectionData, cOcclusionCheckRay, tIgnoreEntities)) { /* The light is not occluded */ if(m_bShowRays) GetEntity().GetControllableEntity().AddCheckedRay(false, cOcclusionCheckRay); /* Get the distance between the light and the e-puck */ cOcclusionCheckRay.ToVector(cLightDistance); /* Linearly scale the distance with the light intensity The greater the intensity, the smaller the distance */ cLightDistance /= cLight.GetIntensity(); /* Get the angle wrt to e-puck rotation */ cLightAngle = cLightDistance.GetZAngle(); cLightAngle -= cOrientationZ; /* Transform it into counter-clockwise rotation */ cLightAngle.Negate().UnsignedNormalize(); /* Find reading corresponding to the sensor */ SInt16 nMin = 0; for(SInt16 i = 1; i < NUM_READINGS; ++i){ if((cLightAngle - m_tReadings[i].Angle).GetAbsoluteValue() < (cLightAngle - m_tReadings[nMin].Angle).GetAbsoluteValue()) nMin = i; } /* Set the actual readings */ Real fReading = cLightDistance.Length(); m_tReadings[Modulo((SInt16)(nMin-1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin-1, NUM_READINGS)].Angle)); m_tReadings[ nMin ].Value += ComputeReading(fReading); m_tReadings[Modulo((SInt16)(nMin+1), NUM_READINGS)].Value += ComputeReading(fReading * Cos(cLightAngle - m_tReadings[Modulo(nMin+1, NUM_READINGS)].Angle)); } else { /* The ray is occluded */ if(m_bShowRays) { GetEntity().GetControllableEntity().AddCheckedRay(true, cOcclusionCheckRay); GetEntity().GetControllableEntity().AddIntersectionPoint(cOcclusionCheckRay, sIntersectionData.TOnRay); } } } } } catch(argos::CARGoSException& e){ } /* Now go through the sensors, add noise and clamp their values if above 1024 or under 1024 */ for(size_t i = 0; i < m_tReadings.size(); ++i) { if(m_fNoiseLevel>0.0f) AddNoise(i); if(m_tReadings[i].Value > 1024.0f) m_tReadings[i].Value = 1024.0f; if(m_tReadings[i].Value < 0.0f) m_tReadings[i].Value = 0.0f; } }
void CFootBotLightRotZOnlySensor::Update() { /* Erase readings */ for(size_t i = 0; i < m_tReadings.size(); ++i) { m_tReadings[i].Value = 0.0f; } /* Get foot-bot orientation */ CRadians cTmp1, cTmp2, cOrientationZ; m_pcEmbodiedEntity->GetOriginAnchor().Orientation.ToEulerAngles(cOrientationZ, cTmp1, cTmp2); /* Ray used for scanning the environment for obstacles */ CRay3 cOcclusionCheckRay; cOcclusionCheckRay.SetStart(m_pcEmbodiedEntity->GetOriginAnchor().Position); CVector3 cRobotToLight; /* Buffer for the angle of the light wrt to the foot-bot */ CRadians cAngleLightWrtFootbot; /* Buffers to contain data about the intersection */ SEmbodiedEntityIntersectionItem sIntersection; /* List of light entities */ CSpace::TMapPerTypePerId::iterator itLights = m_cSpace.GetEntityMapPerTypePerId().find("light"); if (itLights != m_cSpace.GetEntityMapPerTypePerId().end()) { CSpace::TMapPerType& mapLights = itLights->second; /* * 1. go through the list of light entities in the scene * 2. check if a light is occluded * 3. if it isn't, distribute the reading across the sensors * NOTE: the readings are additive * 4. go through the sensors and clamp their values */ for(CSpace::TMapPerType::iterator it = mapLights.begin(); it != mapLights.end(); ++it) { /* Get a reference to the light */ CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second)); /* Consider the light only if it has non zero intensity */ if(cLight.GetIntensity() > 0.0f) { /* Set the ray end */ cOcclusionCheckRay.SetEnd(cLight.GetPosition()); /* Check occlusion between the foot-bot and the light */ if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection, cOcclusionCheckRay, *m_pcEmbodiedEntity)) { /* The light is not occluded */ if(m_bShowRays) { m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay); } /* Get the distance between the light and the foot-bot */ cOcclusionCheckRay.ToVector(cRobotToLight); /* * Linearly scale the distance with the light intensity * The greater the intensity, the smaller the distance */ cRobotToLight /= cLight.GetIntensity(); /* Get the angle wrt to foot-bot rotation */ cAngleLightWrtFootbot = cRobotToLight.GetZAngle(); cAngleLightWrtFootbot -= cOrientationZ; /* * Find closest sensor index to point at which ray hits footbot body * Rotate whole body by half a sensor spacing (corresponding to placement of first sensor) * Division says how many sensor spacings there are between first sensor and point at which ray hits footbot body * Increase magnitude of result of division to ensure correct rounding */ Real fIdx = (cAngleLightWrtFootbot - SENSOR_HALF_SPACING) / SENSOR_SPACING; SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f; /* Set the actual readings */ Real fReading = cRobotToLight.Length(); /* * Take 6 readings before closest sensor and 6 readings after - thus we * process sensors that are with 180 degrees of intersection of light * ray with robot body */ for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) { UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 24); CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtFootbot - m_tReadings[unIdx].Angle).SignedNormalize()); /* * ComputeReading gives value as if sensor was perfectly in line with * light ray. We then linearly decrease actual reading from 1 (dist * 0) to 0 (dist PI/2) */ m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint); } } else { /* The ray is occluded */ if(m_bShowRays) { m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay); m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersection.TOnRay); } } } } /* Apply noise to the sensors */ if(m_bAddNoise) { for(size_t i = 0; i < 24; ++i) { m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); } } /* Trunc the reading between 0 and 1 */ for(size_t i = 0; i < 24; ++i) { SENSOR_RANGE.TruncValue(m_tReadings[i].Value); } } else { /* There are no lights in the environment */ if(m_bAddNoise) { /* Go through the sensors */ for(UInt32 i = 0; i < m_tReadings.size(); ++i) { /* Apply noise to the sensor */ m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); /* Trunc the reading between 0 and 1 */ SENSOR_RANGE.TruncValue(m_tReadings[i].Value); } } } }