Пример #1
0
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)
      {
	printf("No sonar on the robot, can't do the test.\n");
	break;
      }
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }
      pose = myRobot->getPose();
      pose.setX(pose.getX() + 4000);
      pose.setY(pose.getY() + 4000);
      myRobot->moveTo(pose);
      printf("Moved robot.\n");
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }

      break;
    case 6:
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      printf("Disconnecting from the robot, then reconnecting.\n");
      myRobot->disconnect();
      myRobot->blockingConnect();      
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      break;
    default:
      printf("No test for second button.\n");
      break;
    } 
  }
}
Пример #2
0
	virtual void setRobot(ArRobot *robot)					{	ArAction::setRobot(robot);	mysonar = robot->findRangeDevice("sonar");	}