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; } } }
virtual void setRobot(ArRobot *robot) { ArAction::setRobot(robot); mysonar = robot->findRangeDevice("sonar"); }