void CEPuckRangeAndBearingSensor::Update() {
/* Clear the previous received packets */
ClearRABReceivedPackets();
/* Get robot position */
const CVector3& cRobotPosition = m_pcEmbodiedEntity->GetPosition();
/* Get robot orientation */
CRadians cTmp1, cTmp2, cOrientationZ;
m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
/* Buffer for calculating the message--robot distance */
CVector3 cVectorToMessage;
CVector3 cVectorRobotToMessage;
Real fMessageDistance;
/* Buffer for the received packet */
TEPuckRangeAndBearingReceivedPacket tPacket;
/* Initialize the occlusion check ray start to the position of the robot */
CRay cOcclusionCheckRay;
cOcclusionCheckRay.SetStart(cRobotPosition);
/* Buffer to store the intersection data */
CSpace::SEntityIntersectionItem<CEmbodiedEntity> sIntersectionData;
/* Ignore the sensing robot when checking for occlusions */
TEmbodiedEntitySet tIgnoreEntities;
tIgnoreEntities.insert(m_pcEmbodiedEntity);
/*
* 1. Go through all the CRABEquippedEntities<2> (those compatible with this sensor)
* 2. For each of them
* a) Check that the receiver is not out of range
* b) Check if there is an occlusion
* c) If there isn't, get the info and set reading for that robot
*/
CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("rab_equipped_entity<2>");
for(CSpace::TAnyEntityMap::iterator it = tEntityMap.begin();
it != tEntityMap.end();
++it) {
CRABEquippedEntity<2>& cRABEntity = *(any_cast<CRABEquippedEntity<2>*>(it->second));
/* Check the RAB equipped entity is not this robot (avoid self-messaging) */
if(&cRABEntity != m_pcRABEquippedEntity) {
/* Get the position of the RAB equipped entity */
cVectorToMessage = cRABEntity.GetPosition();
cVectorRobotToMessage = (cVectorToMessage - cRobotPosition) * 100; // in cms
/* Check that the distance is lower than the range */
fMessageDistance = cVectorRobotToMessage.Length();
if(fMessageDistance < cRABEntity.GetRange()) {
/* Set the ray end */
cOcclusionCheckRay.SetEnd(cVectorToMessage);
/* Check occlusion between robot and message location */
if(!m_bCheckOcclusions ||
(! m_cSpace.GetClosestEmbodiedEntityIntersectedByRay(sIntersectionData, cOcclusionCheckRay, tIgnoreEntities)) ||
sIntersectionData.IntersectedEntity->GetId() == cRABEntity.GetId()) {
/* The message is not occluded */
if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay);
/* Set the reading */
tPacket.Id = m_unLatestPacketId++;
CRadians cVertical = CRadians::ZERO;
cVectorRobotToMessage.ToSphericalCoordsHorizontal(tPacket.Range,
cVertical,
tPacket.BearingHorizontal);
tPacket.BearingHorizontal -= cOrientationZ;
tPacket.BearingHorizontal.SignedNormalize();
cRABEntity.GetData(tPacket.Data);
m_tLastReceivedPackets.push_back(tPacket);
}
else {
/* The message is occluded */
if(m_bShowRays) {
m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay);
m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersectionData.TOnRay);
}
}
}
}
}
}
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;
}
}
