/**
@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)
{
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;
}
}
}
