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

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 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
    }

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

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 Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
//.........这里部分代码省略.........