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