C++ ArTransform::setTransform方法代码示例

int main(void)
{
  ArPose p1(100, 100, 0), p2(1000, 1000, 90), p3;
  ArTransform trans;
  
  trans.setTransform(p1);
  
  p3.setPose(-900,-900,0);
  p3 = trans.doTransform(p3);
  p3.log();
  
  p2.setPose(200, 200, 0);
  trans.setTransform(p1, p2);
  p3.setPose(0,0,0);
  p3 = trans.doInvTransform(p3);
  p3.log();





}

/**
  This is the packet handler for the PB9 data, which is sent via the micro
  controller, to the client.  This will read the data from the packets,
  and then call processReadings to filter add the data to the current and
  cumulative buffers.
*/
AREXPORT bool ArIrrfDevice::packetHandler(ArRobotPacket *packet)
{
  int /*portNum,*/ i, dist, packetCounter;
  double conv;
  ArTransform packetTrans;
  std::list<ArSensorReading *>::iterator it;
  ArSensorReading *reading;
  ArPose pose;
  ArTransform encoderTrans;
  ArPose encoderPose;

  pose = myRobot->getPose();
  conv = 2.88;

  packetTrans.setTransform(pose);
  packetCounter = myRobot->getCounter();

  if (packet->getID() != 0x10)
    return false;

  // Which Aux port the IRRF is connected to
  //portNum =
  packet->bufToByte2();
  encoderTrans = myRobot->getEncoderTransform();
  encoderPose = encoderTrans.doInvTransform(pose);

  i = 0;
  for (i=0, it = myRawReadings->begin();it != myRawReadings->end();it++, i++)
  {
    reading = (*it);
    dist = (int) ((packet->bufToUByte2()) / conv);
    reading->newData(dist, pose, encoderPose, packetTrans, packetCounter, packet->getTimeReceived());
  }

  myLastReading.setToNow();

  processReadings();

  return true;
}

void ArUrg::sensorInterp(void)
{
  ArTime readingRequested;
  std::string reading;
  myReadingMutex.lock();
  if (myReading.empty())
  {
    myReadingMutex.unlock();
    return;
  }

  readingRequested = myReadingRequested;
  reading = myReading;
  myReading = "";
  myReadingMutex.unlock();

  ArTime time = readingRequested;
  ArPose pose;
  int ret;
  int retEncoder;
  ArPose encoderPose;

  //time.addMSec(-13);
  if (myRobot == NULL || !myRobot->isConnected())
  {
    pose.setPose(0, 0, 0);
    encoderPose.setPose(0, 0, 0);
  } 
  else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
	   (retEncoder = 
	    myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
  {
    ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
    return;
  }

  ArTransform transform;
  transform.setTransform(pose);

  unsigned int counter = 0; 
  if (myRobot != NULL)
    counter = myRobot->getCounter();

  lockDevice();
  myDataMutex.lock();

  //double angle;
  int i;
  int len = reading.size();

  int range;
  int big; 
  int little;
  //int onStep;

  std::list<ArSensorReading *>::reverse_iterator it;
  ArSensorReading *sReading;

  bool ignore;
  for (it = myRawReadings->rbegin(), i = 0; 
       it != myRawReadings->rend() && i < len - 1; 
       it++, i += 2)
  {
    ignore = false;
    big = reading[i] - 0x30;
    little = reading[i+1] - 0x30;
    range = (big << 6 | little);
    if (range < 20)
    {
      /* Well that didn't work...
      // if the range is 1 to 5 that means it has low intensity, which
      // could be black or maybe too far... try ignoring it and see if
      // it helps with too much clearing
      if (range >= 1 || range <= 5)
	ignore = true;
      */
      range = 4096;
    }
    sReading = (*it);
    sReading->newData(range, pose, encoderPose, transform, counter, 
		      time, ignore, 0);
  }

  myDataMutex.unlock();

  laserProcessReadings();
  unlockDevice();
}

void ArUrg_2_0::sensorInterp(void)
{
  ArTime readingRequested;
  std::string reading;
  myReadingMutex.lock();
  if (myReading.empty())
  {
    myReadingMutex.unlock();
    return;
  }

  readingRequested = myReadingRequested;
  reading = myReading;
  myReading = "";
  myReadingMutex.unlock();

  ArTime time = readingRequested;
  ArPose pose;
  int ret;
  int retEncoder;
  ArPose encoderPose;

  //time.addMSec(-13);
  if (myRobot == NULL || !myRobot->isConnected())
  {
    pose.setPose(0, 0, 0);
    encoderPose.setPose(0, 0, 0);
  } 
  else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
	   (retEncoder = 
	    myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
  {
    ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
    return;
  }

  ArTransform transform;
  transform.setTransform(pose);

  unsigned int counter = 0; 
  if (myRobot != NULL)
    counter = myRobot->getCounter();

  lockDevice();
  myDataMutex.lock();

  //double angle;
  int i;
  int len = reading.size();

  int range;
  int giant;
  int big; 
  int little;
  //int onStep;

  std::list<ArSensorReading *>::reverse_iterator it;
  ArSensorReading *sReading;
  
  int iMax;
  int iIncr;

  if (myUseThreeDataBytes)
  {
    iMax = len - 2;
    iIncr = 3;
  }
  else
  {
    iMax = len - 1;
    iIncr = 2;
  }

  bool ignore;
  for (it = myRawReadings->rbegin(), i = 0; 
       it != myRawReadings->rend() && i < iMax; //len - 2; 
       it++, i += iIncr) //3)
  {
    ignore = false;

    if (myUseThreeDataBytes)
    {
      giant = reading[i] - 0x30;
      big = reading[i+1] - 0x30;
      little = reading[i+2] - 0x30;
      range = (giant << 12 | big << 6 | little);
    }
    else
    {
      big = reading[i] - 0x30;
      little = reading[i+1] - 0x30;
      range = (big << 6 | little);
    }
    
    if (range < myDMin)
      range = myDMax+1;

    sReading = (*it);
    sReading->newData(range, pose, encoderPose, transform, counter, 
		      time, ignore, 0);
//.........这里部分代码省略.........

void ArSZSeries::sensorInterp(void) {
	ArSZSeriesPacket *packet;

	while (1) {
		myPacketsMutex.lock();
		if (myPackets.empty()) {
			myPacketsMutex.unlock();
			return;
		}
		packet = myPackets.front();
		myPackets.pop_front();
		myPacketsMutex.unlock();

		//set up the times and poses

		ArTime time = packet->getTimeReceived();
		
		ArPose pose;
		int ret;
		int retEncoder;
		ArPose encoderPose;
		int dist;
		int j;

		unsigned char *buf = (unsigned char *) packet->getBuf();

		// this value should be found more empirically... but we used 1/75
		// hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
		if (!time.addMSec(-30)) {
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() error adding msecs (-30)", getName());
		}

		if (myRobot == NULL || !myRobot->isConnected())
		{
			pose.setPose(0, 0, 0);
			encoderPose.setPose(0, 0, 0);
		}
		else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0
				|| (retEncoder = myRobot->getEncoderPoseInterpPosition(time,
						&encoderPose)) < 0)
		{
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() reading too old to process", getName());
			delete packet;
			continue;
		}

		ArTransform transform;
		transform.setTransform(pose);

		unsigned int counter = 0;
		if (myRobot != NULL)
			counter = myRobot->getCounter();

		lockDevice();
		myDataMutex.lock();

		//std::list<ArSensorReading *>::reverse_iterator it;
		ArSensorReading *reading;

		myNumChans = packet->getNumReadings();

		double eachAngularStepWidth;
		int eachNumberData;

		// PS - test for SZ-16D, each reading is .36 degrees for 270 degrees

		if (packet->getNumReadings() == 751)
		{
			eachNumberData = packet->getNumReadings();
		}
		else
		{
			ArLog::log(ArLog::Normal,
					"%s::sensorInterp() The number of readings is not correct = %d",
					getName(), myNumChans);

			// PS 12/6/12 - unlock before continuing

			delete packet;
			myDataMutex.unlock();
			unlockDevice();
			continue;
		}

		// If we don't have any sensor readings created at all, make 'em all
		if (myRawReadings->size() == 0) {
			for (j = 0; j < eachNumberData; j++) {
				myRawReadings->push_back(new ArSensorReading);
			}
		}

		if (eachNumberData > myRawReadings->size())
		{
			ArLog::log(ArLog::Terse,
					"%s::sensorInterp() Bad data, in theory have %d readings but can only have 751... skipping this packet",
					getName(), eachNumberData);

			// PS 12/6/12 - unlock and delete before continuing
//.........这里部分代码省略.........

AREXPORT void ArRangeDevice::adjustRawReadings(bool interlaced)
{
  std::list<ArSensorReading *>::iterator rawIt;

  // make sure we have raw readings and a robot, and a delay to
  // correct for (note that if we don't have a delay to correct for
  // but have already been adjusting (ie someone changed the delay)
  // we'll just keep adjusting)
  if (myRawReadings == NULL || myRobot == NULL || 
      (myAdjustedRawReadings == NULL && myRobot->getOdometryDelay() == 0))
    return;
  

  // if we don't already have a list then make one
  if (myAdjustedRawReadings == NULL)
    myAdjustedRawReadings = new std::list<ArSensorReading *>;
  
  // if we've already adjusted these readings then don't do it again
  if (myRawReadings->begin() != myRawReadings->end() &&
      myRawReadings->front()->getAdjusted())
    return;

  std::list<ArSensorReading *>::iterator adjIt;
  ArSensorReading *adjReading;
  ArSensorReading *rawReading;

  ArTransform trans;
  ArTransform encTrans;
  ArTransform interlacedTrans;
  ArTransform interlacedEncTrans;

  bool first = true;
  bool second = true;

  int onReading;
  for (rawIt = myRawReadings->begin(), adjIt = myAdjustedRawReadings->begin(), 
       onReading = 0; 
       rawIt != myRawReadings->end(); 
       rawIt++, onReading++)
  {
    rawReading = (*rawIt);
    if (adjIt != myAdjustedRawReadings->end())
    {
      adjReading = (*adjIt);
      adjIt++;
    }
    else
    {
      adjReading = new ArSensorReading;
      myAdjustedRawReadings->push_back(adjReading);
    }
    (*adjReading) = (*rawReading);
    if (first || (interlaced && second))
    {
      ArPose origPose;
      ArPose corPose;
      ArPose origEncPose;
      ArPose corEncPose;
      ArTime corTime;


      corTime = rawReading->getTimeTaken();
      corTime.addMSec(-myRobot->getOdometryDelay());
      if (myRobot->getPoseInterpPosition(corTime, 
					 &corPose) == 1 && 
	  myRobot->getEncoderPoseInterpPosition(corTime, 
						&corEncPose) == 1)
      {
	origPose = rawReading->getPoseTaken();
	origEncPose = rawReading->getEncoderPoseTaken();
	/*
	printf("Difference was %g %g %g (rotVel %.0f, rotvel/40 %g)\n", 
	       origEncPose.getX() - corEncPose.getX(),
	       origEncPose.getY() - corEncPose.getY(),
	       origEncPose.getTh() - corEncPose.getTh(),
	       myRobot->getRotVel(), myRobot->getRotVel() / 40);
	*/
	if (first)
	{
	  trans.setTransform(origPose, corPose);
	  encTrans.setTransform(origEncPose, corEncPose);
	}
	else if (interlaced && second)
	{
	  interlacedTrans.setTransform(origPose, corPose);
	  interlacedEncTrans.setTransform(origEncPose, corEncPose);
	}
      }
      else
      {
	//printf("Couldn't correct\n");
      }

      if (first)
	first = false;
      else if (interlaced && second)
	second = false;

    }
    if (!interlaced && (onReading % 2) == 0)
//.........这里部分代码省略.........

void ArLMS1XX::sensorInterp(void)
{
  ArLMS1XXPacket *packet;
  
  while (1)
  {
    myPacketsMutex.lock();
    if (myPackets.empty())
    {
      myPacketsMutex.unlock();
      return;
    }
    packet = myPackets.front();
    myPackets.pop_front();
    myPacketsMutex.unlock();
	   
    // if its not a reading packet just skip it 
    if (strcasecmp(packet->getCommandName(), "LMDscandata") != 0)
    {
      delete packet;
      continue;
    }

    //set up the times and poses

    ArTime time = packet->getTimeReceived();
    ArPose pose;
    int ret;
    int retEncoder;
    ArPose encoderPose;
    
    // this value should be found more empirically... but we used 1/75
    // hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
    time.addMSec(-20);
    if (myRobot == NULL || !myRobot->isConnected())
    {
      pose.setPose(0, 0, 0);
      encoderPose.setPose(0, 0, 0);
    } 
    else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
	     (retEncoder = 
	      myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
    {
      ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
      delete packet;
      continue;
    }
    
    ArTransform transform;
    transform.setTransform(pose);
    
    unsigned int counter = 0; 
    if (myRobot != NULL)
      counter = myRobot->getCounter();
    
    lockDevice();
    myDataMutex.lock();
    
    int i;
    int dist;
    //int onStep;
    
    std::list<ArSensorReading *>::reverse_iterator it;
    ArSensorReading *reading;

    // read the extra stuff
    myVersionNumber = packet->bufToUByte2();
    myDeviceNumber = packet->bufToUByte2();
    mySerialNumber = packet->bufToUByte4();
    myDeviceStatus1 = packet->bufToUByte();
    myDeviceStatus2 = packet->bufToUByte();
    myMessageCounter = packet->bufToUByte2();
    myScanCounter = packet->bufToUByte2();
    myPowerUpDuration = packet->bufToUByte4();
    myTransmissionDuration = packet->bufToUByte4();
    myInputStatus1 = packet->bufToUByte();
    myInputStatus2 = packet->bufToUByte();
    myOutputStatus1 = packet->bufToUByte();
    
    myOutputStatus2 = packet->bufToUByte();
    myReserved = packet->bufToUByte2();
    myScanningFreq = packet->bufToUByte4();
    myMeasurementFreq = packet->bufToUByte4();

    if (myDeviceStatus1 != 0 || myDeviceStatus2 != 0)
      ArLog::log(myLogLevel, "%s: DeviceStatus %d %d", 
		 myDeviceStatus1, myDeviceStatus2); 

    /*
      printf("Received: %s %s ver %d devNum %d serNum %d scan %d sf %d mf %d\n", 
	   packet->getCommandType(), packet->getCommandName(), 
	   myVersionNumber, myDeviceNumber, 
	   mySerialNumber, myScanCounter, myScanningFreq, myMeasurementFreq);
    */
    myNumberEncoders = packet->bufToUByte2();
    //printf("\tencoders %d\n", myNumberEncoders);
    if (myNumberEncoders > 0)
      ArLog::log(myLogLevel, "%s: Encoders %d", getName(), myNumberEncoders);

    for (i = 0; i < myNumberEncoders; i++)
//.........这里部分代码省略.........