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

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();
}

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