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

/**
   @param sx the coords of the sensor return relative to sensor (mm)
   @param sy the coords of the sensor return relative to sensor (mm)
   @param robotPose the robot's pose when the reading was taken
   @param encoderPose the robot's encoder pose when the reading was taken
   @param trans transform of reading from local to global position
   @param counter the counter from the robot when the sensor reading was taken
   @param timeTaken the time the reading was taken
   @param ignoreThisReading if this reading should be ignored or not
   @param extraInt extra laser device-specific value associated with this
   reading (e.g. SICK LMS-200 reflectance)
*/
AREXPORT void ArSensorReading::newData(int sx, int sy, ArPose robotPose,
				       ArPose encoderPose, ArTransform trans, 
				       unsigned int counter, ArTime timeTaken,
				       bool ignoreThisReading, int extraInt)
{
  // TODO calculate the x and y position of the sensor
  double rx, ry;
  myRange = (int)sqrt((double)(sx*sx + sy*sy));
  myCounterTaken = counter;
  myReadingTaken = robotPose;
  myEncoderPoseTaken = encoderPose;
  rx = getSensorX() + sx;
  ry = getSensorY() + sy;
  myLocalReading.setPose(rx, ry);
  myReading = trans.doTransform(myLocalReading);
  myTimeTaken = timeTaken;
  myIgnoreThisReading = ignoreThisReading;
  myExtraInt = extraInt;
  myAdjusted = false;
}

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 function is called every 100 milliseconds.
*/
AREXPORT void ArIRs::processReadings(void)
{
    ArUtil::BITS bit = ArUtil::BIT0;
    if(myParams.haveTableSensingIR())
    {
        for (int i = 0; i < myParams.getNumIR(); ++i)
        {
            switch(i)
            {
            case 0:
                bit = ArUtil::BIT0;
                break;
            case 1:
                bit = ArUtil::BIT1;
                break;
            case 2:
                bit = ArUtil::BIT2;
                break;
            case 3:
                bit = ArUtil::BIT3;
                break;
            case 4:
                bit = ArUtil::BIT4;
                break;
            case 5:
                bit = ArUtil::BIT5;
                break;
            case 6:
                bit = ArUtil::BIT6;
                break;
            case 7:
                bit = ArUtil::BIT7;
                break;
            }

            if(myParams.haveNewTableSensingIR() && myRobot->getIODigInSize() > 3)
            {
                if((myParams.getIRType(i) && !(myRobot->getIODigIn(3) & bit)) ||
                        (!myParams.getIRType(i) && (myRobot->getIODigIn(3) & bit)))
                {
                    if(cycleCounters[i] < myParams.getIRCycles(i))
                    {
                        cycleCounters[i] = cycleCounters[i] + 1;
                    }
                    else
                    {
                        cycleCounters[i] = 1;
                        ArPose pose;
                        pose.setX(myParams.getIRX(i));
                        pose.setY(myParams.getIRY(i));

                        ArTransform global = myRobot->getToGlobalTransform();
                        pose = global.doTransform(pose);

                        myCurrentBuffer.addReading(pose.getX(), pose.getY());
                    }
                }
                else
                {
                    cycleCounters[i] = 1;
                }
            }
            else
            {
                if(!(myRobot->getDigIn() & bit))
                {
                    if(cycleCounters[i] < myParams.getIRCycles(i))
                    {
                        cycleCounters[i] = cycleCounters[i] + 1;
                    }
                    else
                    {
                        cycleCounters[i] = 1;

                        ArPose pose;
                        pose.setX(myParams.getIRX(i));
                        pose.setY(myParams.getIRY(i));

                        ArTransform global = myRobot->getToGlobalTransform();
                        pose = global.doTransform(pose);

                        myCurrentBuffer.addReading(pose.getX(), pose.getY());
                    }
                }
                else
                {
                    cycleCounters[i] = 1;
                }
            }
        }
    }
}

/**
   @param trans the transform to apply to the encoder pose taken
*/
AREXPORT void ArSensorReading::applyEncoderTransform(ArTransform trans)
{
  myEncoderPoseTaken = trans.doTransform(myEncoderPoseTaken);
}

/**
   @param trans the transform to apply to the reading and where the reading was taken
*/
AREXPORT void ArSensorReading::applyTransform(ArTransform trans)
{
  myReading = trans.doTransform(myReading);
  myReadingTaken = trans.doTransform(myReadingTaken);
}

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