C++ ArSensorReading::getIgnoreThisReading方法代码示例

AREXPORT void ArLaserReflectorDevice::processReadings(void)
{
  //int i;
  ArSensorReading *reading;
  myLaser->lockDevice();
  lockDevice();
  
  const std::list<ArSensorReading *> *rawReadings;
  std::list<ArSensorReading *>::const_iterator rawIt;
  rawReadings = myLaser->getRawReadings();
  myCurrentBuffer.beginRedoBuffer();

  if (myReflectanceThreshold < 0 || myReflectanceThreshold > 255)
    myReflectanceThreshold = 0;

  if (rawReadings->begin() != rawReadings->end())
  {
    for (rawIt = rawReadings->begin(); rawIt != rawReadings->end(); rawIt++)
    {
      reading = (*rawIt);
      if (!reading->getIgnoreThisReading() && 
	  reading->getExtraInt() > myReflectanceThreshold)
	myCurrentBuffer.redoReading(reading->getPose().getX(), 
				    reading->getPose().getY());
    }
  }

  myCurrentBuffer.endRedoBuffer();

  unlockDevice();
  myLaser->unlockDevice();
}

/*-------------------------------------------------------------
					getSonarsReadings
-------------------------------------------------------------*/
void CActivMediaRobotBase::getSonarsReadings( bool &thereIsObservation, CObservationRange	&obs )
{
#if MRPT_HAS_ARIA
	ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
	THE_ROBOT->lock();

	obs.minSensorDistance = 0;
	obs.maxSensorDistance = 30;

	int		i,N =THE_ROBOT->getNumSonar();

	obs.sensorLabel = "BASE_SONARS";
	obs.sensorConeApperture = DEG2RAD( 30 );
	obs.timestamp = system::now();

	obs.sensedData.clear();

	unsigned int time_cnt = THE_ROBOT->getCounter();

	if (m_lastTimeSonars == time_cnt)
	{
		thereIsObservation = false;
		THE_ROBOT->unlock();
		return;
	}

	for (i=0;i<N;i++)
	{
		ArSensorReading		*sr = THE_ROBOT->getSonarReading(i);

		if (sr->getIgnoreThisReading()) continue;

//		if (!sr->isNew(time_cnt))
//		{
			//thereIsObservation = false;
			//break;
//		}

		obs.sensedData.push_back( CObservationRange::TMeasurement() );
		CObservationRange::TMeasurement & newObs = obs.sensedData.back();

		newObs.sensorID = i;
		newObs.sensorPose.x = 0.001*sr->getSensorX();
		newObs.sensorPose.y = 0.001*sr->getSensorY();
		newObs.sensorPose.z = 0; //0.001*sr->getSensorZ();
		newObs.sensorPose.yaw = DEG2RAD( sr->getSensorTh() );
		newObs.sensorPose.pitch = 0;
		newObs.sensorPose.roll = 0;

		newObs.sensedDistance = 0.001*THE_ROBOT->getSonarRange(i);
	}
	THE_ROBOT->unlock();

	thereIsObservation = !obs.sensedData.empty();

	// keep the last time:
	if (thereIsObservation)
		m_lastTimeSonars = time_cnt;

#else
	MRPT_UNUSED_PARAM(thereIsObservation); MRPT_UNUSED_PARAM(obs);
	THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}

void ArLaserFilter::processReadings(void)
{
  myLaser->lockDevice();
  selfLockDevice();

  const std::list<ArSensorReading *> *rdRawReadings;
  std::list<ArSensorReading *>::const_iterator rdIt;
  
  if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
  {
    selfUnlockDevice();
    myLaser->unlockDevice();
    return;
  }

  size_t rawSize = myRawReadings->size();
  size_t rdRawSize = myLaser->getRawReadings()->size();
  
  while (rawSize < rdRawSize)
  {
    myRawReadings->push_back(new ArSensorReading);
    rawSize++;
  }

  // set where the pose was taken
  myCurrentBuffer.setPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());


  std::list<ArSensorReading *>::iterator it;
  ArSensorReading *rdReading;
  ArSensorReading *reading;

#ifdef DEBUGRANGEFILTER
  FILE *file = NULL;
  file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
#endif

  std::map<int, ArSensorReading *> readingMap;
  int numReadings = 0;

  // first pass to copy the readings and put them into a map
  for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
       rdIt != rdRawReadings->end() && it != myRawReadings->end();
       rdIt++, it++)
  {
    rdReading = (*rdIt);
    reading = (*it);
    *reading = *rdReading;
    
    readingMap[numReadings] = reading;
    numReadings++;
  }
  
  char buf[1024];
  int i;
  int j;
  ArSensorReading *lastAddedReading = NULL;
  
  // now walk through the readings to filter them
  for (i = 0; i < numReadings; i++)
  {
    reading = readingMap[i];

    // if we're ignoring this reading then just get on with life
    if (reading->getIgnoreThisReading())
      continue;

    if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
    {
      reading->setIgnoreThisReading(true);
      continue;
    }
      
    if (lastAddedReading != NULL)
    {

      if (lastAddedReading->getPose().findDistanceTo(reading->getPose()) < 50)
      {
#ifdef DEBUGRANGEFILTER
	if (file != NULL)
	  fprintf(file, "%.1f too close from last %6.0f\n", 
		  reading->getSensorTh(),
		  lastAddedReading->getPose().findDistanceTo(
			  reading->getPose()));
#endif
	reading->setIgnoreThisReading(true);
	continue;
      }
#ifdef DEBUGRANGEFILTER
      else if (file != NULL)
	fprintf(file, "%.1f from last %6.0f\n", 
		reading->getSensorTh(),
		lastAddedReading->getPose().findDistanceTo(
			reading->getPose()));
#endif
    }

//.........这里部分代码省略.........

void ArLaser::laserProcessReadings(void)
{
  // if we have no readings... don't do anything
  if (myRawReadings == NULL || myRawReadings->begin() == myRawReadings->end())
    return;

  std::list<ArSensorReading *>::iterator sensIt;
  ArSensorReading *sReading;
  double x, y;
  double lastX = 0.0, lastY = 0.0;
  //unsigned int i = 0;
  ArTime len;
  len.setToNow();

  bool clean;
  if (myCumulativeCleanInterval <= 0 ||
      (myCumulativeLastClean.mSecSince() > 
       myCumulativeCleanInterval))
  {
    myCumulativeLastClean.setToNow();
    clean = true;
  }
  else
  {
    clean = false;
  }
  
  myCurrentBuffer.setPoseTaken(myRawReadings->front()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myRawReadings->front()->getEncoderPoseTaken());
  myCurrentBuffer.beginRedoBuffer();	  

  // walk the buffer of all the readings and see if we want to add them
  for (sensIt = myRawReadings->begin(); 
       sensIt != myRawReadings->end(); 
       ++sensIt)
  {
    sReading = (*sensIt);

    // if we have ignore readings then check them here
    if (!myIgnoreReadings.empty() && 
	(myIgnoreReadings.find(
		(int) ceil(sReading->getSensorTh())) != 
	 myIgnoreReadings.end()) || 
	myIgnoreReadings.find(
		(int) floor(sReading->getSensorTh())) != 
	myIgnoreReadings.end())
      sReading->setIgnoreThisReading(true);

    // see if the reading is valid
    if (sReading->getIgnoreThisReading())
      continue;

    // if we have a max range then check it here... 
    if (myMaxRange != 0 && 
	sReading->getRange() > myMaxRange)
    {
      sReading->setIgnoreThisReading(true);
    }

    // see if the reading is valid... this is set up this way so that
    // max range readings can cancel out other readings, but will
    // still be ignored other than that... ones ignored for other
    // reasons were skipped above
    if (sReading->getIgnoreThisReading())
    {
      internalProcessReading(sReading->getX(), sReading->getY(), 
			     sReading->getRange(), clean, true);
      continue;
    }

    // get our coords
    x = sReading->getX();
    y = sReading->getY();
    
    // see if we're checking on the filter near dist... if we are
    // and the reading is a good one we'll check the cumulative
    // buffer
    if (myMinDistBetweenCurrentSquared > 0.0000001)
    {
      // see where the last reading was
      //squaredDist = (x-lastX)*(x-lastX) + (y-lastY)*(y-lastY);
      // see if the reading is far enough from the last reading
      if (ArMath::squaredDistanceBetween(x, y, lastX, lastY) > 
	  myMinDistBetweenCurrentSquared)
      {
	lastX = x;
	lastY = y;
	// since it was a good reading, see if we should toss it in
	// the cumulative buffer... 
	internalProcessReading(x, y, sReading->getRange(), clean, false);
	
	/* we don't do this part anymore since it wound up leaving
	// too many things not really tehre... if its outside of our
	// sensor angle to use to filter then don't let this one
	// clean  (ArMath::fabs(sReading->getSensorTh()) > 50)
	// filterAddAndCleanCumulative(x, y, false); else*/
      }
      // it wasn't far enough, skip this one and go to the next one
      else
//.........这里部分代码省略.........

void ArSickLogger::internalTakeReading(void)
{
    const std::list<ArSensorReading *> *readings;
    std::list<ArSensorReading *>::const_iterator it;
    std::list<ArSensorReading *>::const_reverse_iterator rit;
    ArPose poseTaken;
    time_t msec;
    ArSensorReading *reading;
    bool usingAdjustedReadings;

    // we take readings in any of the following cases if we haven't
    // taken one yet or if we've been explicitly told to take one or if
    // we've gone further than myDistDiff if we've turned more than
    // myDegDiff if we've switched sign on velocity and gone more than
    // 50 mm (so it doesn't oscilate and cause us to trigger)

    if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit ||
                                   myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff ||
                                   fabs(ArMath::subAngle(myLast.getTh(),
                                           myRobot->getEncoderPose().getTh())) > myDegDiff ||
                                   (( (myLastVel < 0 && myRobot->getVel() > 0) ||
                                      (myLastVel > 0 && myRobot->getVel() < 0)) &&
                                    myLast.findDistanceTo(myRobot->getEncoderPose()) > 50)))
    {
        myWrote = true;
        mySick->lockDevice();
        /// use the adjusted raw readings if we can, otherwise just use
        /// the raw readings like before
        if ((readings = mySick->getAdjustedRawReadings()) != NULL)
        {
            usingAdjustedReadings = true;
        }
        else
        {
            usingAdjustedReadings = false;
            readings = mySick->getRawReadings();
        }
        if (readings == NULL || (it = readings->begin()) == readings->end() ||
                myFile == NULL)
        {
            mySick->unlockDevice();
            return;
        }
        myTakeReadingExplicit = false;
        myScanNumber++;
        if (usingAdjustedReadings)
            ArLog::log(ArLog::Normal,
                       "Taking adjusted readings from the %d laser values",
                       readings->size());
        else
            ArLog::log(ArLog::Normal,
                       "Taking readings from the %d laser values",
                       readings->size());
        myFirstTaken = true;
        myLast = myRobot->getEncoderPose();
        poseTaken = (*readings->begin())->getEncoderPoseTaken();
        myLastVel = myRobot->getVel();
        msec = myStartTime.mSecSince();
        fprintf(myFile, "scan1Id: %d\n", myScanNumber);
        fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
        /* ScanStudio isn't using these yet so don't print them
          fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(),
          myRobot->getVel());*/
        internalPrintPos(poseTaken);

        if (myUseReflectorValues)
        {
            fprintf(myFile, "reflector1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            fprintf(myFile, "\n");
        }
        /**
           Note that the the sick1: or scan1: must be the last thing in
           that timestamp, ie that you should put any other data before
           it.
         **/
        if (myOldReadings)
//.........这里部分代码省略.........

AREXPORT void ArLineFinder::fillPointsFromLaser(void)
{
  const std::list<ArSensorReading *> *readings;
  std::list<ArSensorReading *>::const_iterator it;
  std::list<ArSensorReading *>::const_reverse_iterator rit;
  ArSensorReading *reading;
  int pointCount = 0;

  if (myPoints != NULL)
    delete myPoints;

  myPoints = new std::map<int, ArPose>;
  
  myRangeDevice->lockDevice();
  readings = myRangeDevice->getRawReadings();

  if (!myFlippedFound)
  {
    if (readings->begin() != readings->end())
    {
      int size;
      size = readings->size();
      it = readings->begin();
      // advance along 10 readings
      for (int i = 0; i < 10 && i < size / 2; i++)
	it++;
      // see if we're flipped
      if (ArMath::subAngle((*(readings->begin()))->getSensorTh(), 
			   (*it)->getSensorTh()) > 0)
	myFlipped = true;
      else
	myFlipped = false;
      myFlippedFound = true;
      //printf("@@@ LINE %d %.0f\n", myFlipped, ArMath::subAngle((*(readings->begin()))->getSensorTh(), (*it)->getSensorTh()));

      
    }
  }




  if (readings->begin() == readings->end())
  {
    myRangeDevice->unlockDevice();
    return;
  }
  myPoseTaken = (*readings->begin())->getPoseTaken();

  if (myFlipped)
  {
    for (rit = readings->rbegin(); rit != readings->rend(); rit++)
    {
      reading = (*rit);
      if (reading->getRange() > 5000 || reading->getIgnoreThisReading())
	continue;
      (*myPoints)[pointCount] = reading->getPose();
      pointCount++;
    }
  }
  else
  {
    for (it = readings->begin(); it != readings->end(); it++)
    {
      reading = (*it);
      if (reading->getRange() > 5000 || reading->getIgnoreThisReading())
	continue;
      (*myPoints)[pointCount] = reading->getPose();
      pointCount++;
    }
  }
  myRangeDevice->unlockDevice();
}

void ArLaserFilter::processReadings(void)
{
  myLaser->lockDevice();
  selfLockDevice();

  const std::list<ArSensorReading *> *rdRawReadings;
  std::list<ArSensorReading *>::const_iterator rdIt;
  
  if ((rdRawReadings = myLaser->getRawReadings()) == NULL)
  {
    selfUnlockDevice();
    myLaser->unlockDevice();
    return;
  }

  size_t rawSize = myRawReadings->size();
  size_t rdRawSize = myLaser->getRawReadings()->size();
  
  while (rawSize < rdRawSize)
  {
    myRawReadings->push_back(new ArSensorReading);
    rawSize++;
  }

  // set where the pose was taken
  myCurrentBuffer.setPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getPoseTaken());
  myCurrentBuffer.setEncoderPoseTaken(
	  myLaser->getCurrentRangeBuffer()->getEncoderPoseTaken());


  std::list<ArSensorReading *>::iterator it;
  ArSensorReading *rdReading;
  ArSensorReading *reading;

#ifdef DEBUGRANGEFILTER
  FILE *file = NULL;
  //file = ArUtil::fopen("/mnt/rdsys/tmp/filter", "w");
  file = ArUtil::fopen("/tmp/filter", "w");
#endif

  std::map<int, ArSensorReading *> readingMap;
  int numReadings = 0;

  // first pass to copy the readings and put them into a map
  for (rdIt = rdRawReadings->begin(), it = myRawReadings->begin();
       rdIt != rdRawReadings->end() && it != myRawReadings->end();
       rdIt++, it++)
  {
    rdReading = (*rdIt);
    reading = (*it);
    *reading = *rdReading;

    readingMap[numReadings] = reading;
    numReadings++;
  }

  // if we're not doing any filtering, just short circuit out now
  if (myAllFactor <= 0 && myAnyFactor <= 0 && myAnyMinRange <= 0)
  {
    laserProcessReadings();
    copyReadingCount(myLaser);

    selfUnlockDevice();
    myLaser->unlockDevice();
#ifdef DEBUGRANGEFILTER
    if (file != NULL)
      fclose(file);
#endif
    return;
  }
  
  char buf[1024];
  int i;
  int j;
  //ArSensorReading *lastAddedReading = NULL;
  
  // now walk through the readings to filter them
  for (i = 0; i < numReadings; i++)
  {
    reading = readingMap[i];

    // if we're ignoring this reading then just get on with life
    if (reading->getIgnoreThisReading())
      continue;

    /* Taking this check out since the base class does it now and if
     * it gets marked ignore now it won't get used for clearing
     * cumulative readings

    if (myMaxRange >= 0 && reading->getRange() > myMaxRange)
    {
#ifdef DEBUGRANGEFILTER
      if (file != NULL)
	fprintf(file, "%.1f beyond max range at %d\n", 
		reading->getSensorTh(), reading->getRange());
#endif
      reading->setIgnoreThisReading(true);
      continue;
    }
//.........这里部分代码省略.........