C++ (Cpp) selfUnlockDevice示例

示例#1

文件： ArLaserFilter.cpp 项目： Aharobot/ArAndroidApp

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
    }

    buf[0] = '\0';
    bool goodAll = true;
    bool goodAny = false;
    if (myAnyFactor <= 0)
      goodAny = true;
    for (j = i - 1; 
	 (j >= 0 && //good && 
	  fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
				reading->getSensorTh())) <= myAngleToCheck);
	 j--)
    {
      if (readingMap[j]->getIgnoreThisReading())
      {
#ifdef DEBUGRANGEFILTER
	sprintf(buf, "%s %6s", buf, "i");
#endif
	continue;
      }
#ifdef DEBUGRANGEFILTER
      sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
      if (myAllFactor > 0 && 
	  !checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAllFactor))
	goodAll = false;
      if (myAnyFactor > 0 &&
	  checkRanges(reading->getRange(), 
		      readingMap[j]->getRange(), myAnyFactor))
	goodAny = true;
    }
#ifdef DEBUGRANGEFILTER
    sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif 
    for (j = i + 1; 
	 (j < numReadings && //good &&
	  fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
				reading->getSensorTh())) <= myAngleToCheck);
	 j++)
    {
      if (readingMap[j]->getIgnoreThisReading())
      {
#ifdef DEBUGRANGEFILTER
	sprintf(buf, "%s %6s", buf, "i");
#endif
	continue;
      }
#ifdef DEBUGRANGEFILTER
      sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
      if (myAllFactor > 0 && 
	  !checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAllFactor))
	goodAll = false;
      if (myAnyFactor > 0 &&
	  checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAnyFactor))
	goodAny = true;
    }
    
    if (!goodAll || !goodAny)
      reading->setIgnoreThisReading(true);
    else
      lastAddedReading = reading;

#ifdef DEBUGRANGEFILTER
    if (file != NULL)
      fprintf(file, 
	      "%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
	      good ? 'g' : 'b', buf);
#endif
	    
  }


#ifdef DEBUGRANGEFILTER
  if (file != NULL)
    fclose(file);
#endif

  laserProcessReadings();

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

示例#2

文件： ArLaserFilter.cpp 项目： YGskty/avoid_side_Aria

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;
    }
    */
    if (myAnyMinRange >= 0 && reading->getRange() < myAnyMinRange &&
	(reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
	 reading->getSensorTh() > myAnyMinRangeGreaterThanAngle))
    {
#ifdef DEBUGRANGEFILTER
      if (file != NULL)
	fprintf(file, "%.1f within min range at %d\n", 
		reading->getSensorTh(), reading->getRange());
#endif
      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
    }
    */

    buf[0] = '\0';
    bool goodAll = true;
    bool goodAny = false;
    bool goodMinRange = true;
    if (myAnyFactor <= 0)
      goodAny = true;
    for (j = i - 1; 
	 (j >= 0 && //good && 
	  fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
				reading->getSensorTh())) <= myAngleToCheck);
	 j--)
    {
      /* You can't skip these, or you get onesided filtering
      if (readingMap[j]->getIgnoreThisReading())
      {
#ifdef DEBUGRANGEFILTER
	sprintf(buf, "%s %6s", buf, "i");
#endif
	continue;
      }
      */
#ifdef DEBUGRANGEFILTER
      sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
      if (myAllFactor > 0 && 
	  !checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAllFactor))
	goodAll = false;
      if (myAnyFactor > 0 &&
	  checkRanges(reading->getRange(), 
		      readingMap[j]->getRange(), myAnyFactor))
	goodAny = true;
      if (myAnyMinRange > 0 && 
	  (reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
	   reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
	  readingMap[j]->getRange() <= myAnyMinRange)
	goodMinRange = false;
	
    }
#ifdef DEBUGRANGEFILTER
    sprintf(buf, "%s %6d*", buf, reading->getRange());
#endif 
    for (j = i + 1; 
	 (j < numReadings && //good &&
	  fabs(ArMath::subAngle(readingMap[j]->getSensorTh(),
				reading->getSensorTh())) <= myAngleToCheck);
	 j++)
    {
      // you can't ignore these or you get one sided filtering
      /*
      if (readingMap[j]->getIgnoreThisReading())
      {
#ifdef DEBUGRANGEFILTER
	sprintf(buf, "%s %6s", buf, "i");
#endif
	continue;
      }
      */
#ifdef DEBUGRANGEFILTER
      sprintf(buf, "%s %6d", buf, readingMap[j]->getRange());
#endif
      if (myAllFactor > 0 && 
	  !checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAllFactor))
	goodAll = false;
      if (myAnyFactor > 0 &&
	  checkRanges(reading->getRange(), 
		       readingMap[j]->getRange(), myAnyFactor))
	goodAny = true;
      if (myAnyMinRange > 0 && 
	  (reading->getSensorTh() < myAnyMinRangeLessThanAngle ||
	   reading->getSensorTh() > myAnyMinRangeGreaterThanAngle) &&
	  readingMap[j]->getRange() <= myAnyMinRange)
	goodMinRange = false;
    }
    

    if (!goodAll || !goodAny || !goodMinRange)
      reading->setIgnoreThisReading(true);
    /*
    else
      lastAddedReading = reading;
    */
#ifdef DEBUGRANGEFILTER
    if (file != NULL)
      fprintf(file, 
	      "%5.1f %6d %c\t%s\n", reading->getSensorTh(), reading->getRange(),
	      goodAll && goodAny && goodMinRange ? 'g' : 'b', buf);
#endif
	    
  }


#ifdef DEBUGRANGEFILTER
  if (file != NULL)
    fclose(file);
#endif

  laserProcessReadings();
  copyReadingCount(myLaser);

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