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

AREXPORT void ArSonarDevice::processReadings(void)
{
  int i;
  ArSensorReading *reading;
  lockDevice();

  for (i = 0; i < myRobot->getNumSonar(); i++)
  {
    reading = myRobot->getSonarReading(i);
    if (reading == NULL || !reading->isNew(myRobot->getCounter()))
      continue;
    addReading(reading->getX(), reading->getY());
  }

  // delete too-far readings
  std::list<ArPoseWithTime *> *readingList;
  std::list<ArPoseWithTime *>::iterator it;
  double dx, dy, rx, ry;
    
  myCumulativeBuffer.beginInvalidationSweep();
  readingList = myCumulativeBuffer.getBuffer();
  rx = myRobot->getX();
  ry = myRobot->getY();
  // walk through the list and see if this makes any old readings bad
  if (readingList != NULL)
    {
      for (it = readingList->begin(); it != readingList->end(); ++it)
	{
	  dx = (*it)->getX() - rx;
	  dy = (*it)->getY() - ry;
	  if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist)) 
	    myCumulativeBuffer.invalidateReading(it);
	}
    }
  myCumulativeBuffer.endInvalidationSweep();
  // leave this unlock here or the world WILL end
  unlockDevice();
}

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
//.........这里部分代码省略.........

AREXPORT void ArIrrfDevice::processReadings(void)
{
  //int i;
  double rx, ry, nx, ny, dx, dy, dist;
  ArSensorReading *reading;
  std::list<ArSensorReading *>::iterator rawIt;
  std::list<ArPoseWithTime *> *readingList;
  std::list<ArPoseWithTime *>::iterator readIt;
  lockDevice();

  rx = myRobot->getX();
  ry = myRobot->getY();

  //i=0;
  for (rawIt = myRawReadings->begin();rawIt != myRawReadings->end();rawIt++)
  {
    reading = (*rawIt);
    nx = reading->getX();
    ny = reading->getY();
    dx = nx - rx;
    dy = nx - ry;
    dist = (dx*dx) + (dy*dy);
    if (!reading->isNew(myRobot->getCounter()))
      continue;

    if (dist < (myMaxRange * myMaxRange))
      myCurrentBuffer.addReading(nx, ny);

    if (dist < (myCumulativeMaxRange * myCumulativeMaxRange))
    {
      myCumulativeBuffer.beginInvalidationSweep();
      readingList = myCumulativeBuffer.getBuffer();

      if (readingList != NULL)
      {
        for (readIt = readingList->begin();
	     readIt != readingList->end();
	     readIt++)
        {
          dx = (*readIt)->getX() - nx;
          dy = (*readIt)->getY() - ny;
          if ((dx*dx + dy*dy) < (myFilterNearDist * myFilterNearDist))
            myCumulativeBuffer.invalidateReading(readIt);
        }
      }
      myCumulativeBuffer.endInvalidationSweep();
      myCumulativeBuffer.addReading(nx, ny);
    }
  }

  readingList = myCumulativeBuffer.getBuffer();

  rx = myRobot->getX();
  ry = myRobot->getY();

  myCumulativeBuffer.beginInvalidationSweep();
  if (readingList != NULL)
  {
    for (readIt = readingList->begin(); readIt != readingList->end();readIt++)
    {
      dx = (*readIt)->getX() - rx;
      dy = (*readIt)->getY() - ry;
      if ((dx*dx + dy*dy) > (myFilterFarDist * myFilterFarDist))
        myCumulativeBuffer.invalidateReading(readIt);
    }
  }
  myCumulativeBuffer.endInvalidationSweep();

  unlockDevice();
}