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

void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet)
{ 
  robot.lock();
  ArNetPacket sending;
  sending.empty();
  ArLaser* laser = robot.findLaser(1);
  if(!laser){
      printf("Could not connect to Laser... exiting\n");
      Aria::exit(1);}	
  laser->lockDevice();
  const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc
  sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size()));
  for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){
	ArSensorReading* laserRead =*it2;
        sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange()));
	//printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading());
  }
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getX()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getY()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel()));
  //printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh());
  laser->unlockDevice();
  robot.unlock();
  sending.finalizePacket();
  //sending.printHex();
  client->sendPacketTcp(&sending);
}

void sonar_stop(ArRobot* robot)
{
	int numSonar; //Number of sonar on the robot
	int i; //Counter for looping
	//int j;
	numSonar = robot->getNumSonar(); //Get number of sonar
	ArSensorReading* sonarReading; //To hold each reading
	//for (j = 1; j < 6; j++)
	//{
	robot->setVel(200); //Set translational velocity to 200 mm/s
	for (;;)
	{
		for (i = 0; i < numSonar; i++) //Loop through sonar
	
		
		{
			sonarReading = robot->getSonarReading(i); //Get each sonar reading
			cout << "Sonar reading " << i << " = " << sonarReading->getRange()
				<< " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
			//getchar();

			if (sonarReading->getSensorTh() > -90 && sonarReading->getSensorTh() < 90 && sonarReading->getRange() < 500)

				robot->setVel(0);
		}
	}
			//<< " Angle " << i << " = " <<
			//printf("Sonar Reading", i, "=",sonarReading)
			//robot->unlock(); //Lock robot during set up 18
			//robot->comInt(ArCommands::ENABLE, 1); //Turn on the motors 19	

		//robot->setVel(200); //Set translational velocity to 200 mm/s
		//if (sonarReading[1] < 500)
		//{
			//robot->setRotVel(0);
		//}
		
	}

void readSonars(ArRobot& robot, int numSonar){
	char angle[64], value[64];
	G_id += 1;
	ArSensorReading* sonarReading;
	string res;
	sprintf(value,"id=%d;",G_id);
	res += value;
	for (int i=0; i < numSonar; i++){
		sonarReading = robot.getSonarReading(i);
		sprintf(value,"v%d=%05d;", i, sonarReading->getRange());
		res += value;
		//cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
	}
	res += "\n";
	fseek(G_SONAR_FD, SEEK_SET, 0);
	fwrite(res.c_str(), sizeof(char), res.size(), G_SONAR_FD);
}

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

//.........这里部分代码省略.........
        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)
        {
            fprintf(myFile, "sick1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    fprintf(myFile, "%d ", reading->getRange());
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    fprintf(myFile, "%d ", reading->getRange());
                }
            }
            fprintf(myFile, "\n");
        }
        if (myNewReadings)
        {
            fprintf(myFile, "scan1: ");

            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, "%.0f %.0f  ",
                                reading->getLocalX() - mySick->getSensorPositionX(),
                                reading->getLocalY() - mySick->getSensorPositionY());
                    else
                        fprintf(myFile, "0 0  ");
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%.0f %.0f  ",
                                reading->getLocalX() - mySick->getSensorPositionX(),
                                reading->getLocalY() - mySick->getSensorPositionY());
                    else
                        fprintf(myFile, "0 0  ");
                }
            }
            fprintf(myFile, "\n");
        }
        mySick->unlockDevice();
    }
}

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 sonarPrinter(void)
{
  fprintf(stdout, "in sonarPrinter()\n"); fflush(stdout);
  double scale = (double)half_size / (double)sonar.getMaxRange();

  /*
  ArSonarDevice *sd;

  std::list<ArPoseWithTime *>::iterator it;
  std::list<ArPoseWithTime *> *readings;
  ArPose *pose;

  sd = (ArSonarDevice *)robot->findRangeDevice("sonar");
  if (sd != NULL)
  {
    sd->lockDevice();
    readings = sd->getCurrentBuffer();
    if (readings != NULL)
    {
      for (it = readings->begin(); it != readings->end(); ++it)
      {
        pose = (*it);
        //pose->log();
      }
    }
    sd->unlockDevice();
  }
*/
  double range;
  double angle;

  /*
   * example to show how to find closest readings within polar sections
   */
  printf("Closest readings within polar sections:\n");

  double start_angle = -45;
  double end_angle = 45;
  range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
  printf(" front quadrant: %5.0f  ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");
#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
  //if (isInitialized && range != sonar.getMaxRange())
  if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
  {
    double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
    double y = range * sin(vpMath::rad(angle));

    // Conversion in pixels so that the robot frame is in the middle of the image
    double j = -y * scale + half_size; // obstacle position
    double i = -x * scale + half_size;

    vpDisplay::display(I);
    vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5);
    vpDisplay::displayLine(I, half_size, half_size, 0, 2*half_size-1, vpColor::red, 5);
    vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3);
    vpDisplay::displayCross(I, i, j, 7, vpColor::blue);
  }
#endif

  range = sonar.currentReadingPolar(-135, -45, &angle);
  printf(" right quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  range = sonar.currentReadingPolar(45, 135, &angle);
  printf(" left quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  range = sonar.currentReadingPolar(-135, 135, &angle);
  printf(" back quadrant: %5.0f ", range);
  //if (range != sonar.getMaxRange())
  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
    printf("%3.0f ", angle);
  printf("\n");

  /*
   * example to show how get all sonar sensor data
   */
  ArSensorReading *reading;
  for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
  {
    reading = robot->getSonarReading(sensor);
    if (reading != NULL)
    {
      angle = reading->getSensorTh();
      range = reading->getRange();
      double sx = reading->getSensorX(); // position of the sensor in the robot frame
      double sy = reading->getSensorY();
      double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
      double oy = range * sin(vpMath::rad(angle));
      double x = sx + ox; // position of the obstacle in the robot frame
//.........这里部分代码省略.........

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

  std::stringstream bumper_info(std::stringstream::out);
  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
  {
    bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
    bumper_info << " " << (front_bumpers & (1 << (i+1)));
  }
  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

  bumper_info.str("");
  // Rear bumpers have reverse order (rightmost is LSB)
  unsigned int numRearBumpers = robot->getNumRearBumpers();
  for (unsigned int i=0; i<numRearBumpers; i++)
  {
    bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
    bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
  }
  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
  
  bumpers_pub.publish(bumpers);

  //Publish battery information
  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
  std_msgs::Float64 batteryVoltage;
  batteryVoltage.data = robot->getRealBatteryVoltageNow();
  voltage_pub.publish(batteryVoltage);

  if(robot->haveStateOfCharge())
  {
    std_msgs::Float32 soc;
    soc.data = robot->getStateOfCharge()/100.0;
    state_of_charge_pub.publish(soc);
  }

  // publish recharge state if changed
  char s = robot->getChargeState();
  if(s != recharge_state.data)
  {
    ROS_INFO("RosAria: publishing new recharge state %d.", s);
    recharge_state.data = s;
    recharge_state_pub.publish(recharge_state);
  }

  // publish motors state if changed
  bool e = robot->areMotorsEnabled();
  if(e != motors_state.data || !published_motors_state)
  {
	ROS_INFO("RosAria: publishing new motors state %d.", e);
	motors_state.data = e;
	motors_state_pub.publish(motors_state);
	published_motors_state = true;
  }

  // Publish sonar information, if enabled.
  if (use_sonar) {
    sensor_msgs::PointCloud cloud;	//sonar readings.
    cloud.header.stamp = position.header.stamp;	//copy time.
    // sonar sensors relative to base_link
    cloud.header.frame_id = frame_id_sonar;
    

    // Log debugging info
    std::stringstream sonar_debug_info;
    sonar_debug_info << "Sonar readings: ";
    for (int i = 0; i < robot->getNumSonar(); i++) {
      ArSensorReading* reading = NULL;
      reading = robot->getSonarReading(i);
      if(!reading) {
        ROS_WARN("RosAria: Did not receive a sonar reading.");
        continue;
      }
      
      // getRange() will return an integer between 0 and 5000 (5m)
      sonar_debug_info << reading->getRange() << " ";

      // local (x,y). Appears to be from the centre of the robot, since values may
      // exceed 5000. This is good, since it means we only need 1 transform.
      // x & y seem to be swapped though, i.e. if the robot is driving north
      // x is north/south and y is east/west.
      //
      //ArPose sensor = reading->getSensorPosition();  //position of sensor.
      // sonar_debug_info << "(" << reading->getLocalX() 
      //                  << ", " << reading->getLocalY()
      //                  << ") from (" << sensor.getX() << ", " 
      //                  << sensor.getY() << ") ;; " ;
      
      //add sonar readings (robot-local coordinate frame) to cloud
      geometry_msgs::Point32 p;
      p.x = reading->getLocalX() / 1000.0;
      p.y = reading->getLocalY() / 1000.0;
      p.z = 0.0;
      cloud.points.push_back(p);
    }
    ROS_DEBUG_STREAM(sonar_debug_info.str());
    
    sonar_pub.publish(cloud);
  }

}

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