Example #1
0
 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);
                }
             }
          }
       }
    }
 }
Example #2
0
   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;
      }
   }