MobileSim true pose and other simulation-specific Data

From MobileRobots Research and Academic Customer Support

MobileSim provides some simulator-specific information to client software via the SIMSTAT and SIMINFO packets, such as true position of the robot, timing statistics, and information about the simulated robot devices. You can request one SIMSTAT packet using robot.comInt(ArCommands::SIM_STAT, 1) or request a stream of packets with an argument of 2. Use a packet handler callback attached to ArRobot to check for packet ID 0x62, and unpack the packet (register the handler with ArRobot::addPacketHandler()).

See Aria/tests/testMobileSim.cpp (handleSimStatPacket() and also search for test_simstat) and code below for an example and the MobileSim README for more explanation.

Note that the component values of the pose are signed 4-byte integers not the normal 2-byte short integers usually used in the Pioneer protocol.

The SIMSTAT packet is as follows. New fields may be appended in future versions.

Name	Data Size/Type	Description
Packet ID	ubyte=0x62
unused/reserved NULL byte	ubyte=0	reserved/unused
unused/reserved NULL byte	ubyte=0	reserved/unused
Flags	4ubyte	reserved
SimInterval	2ubyte	Configured simulated time one loop interval takes (normally same as RealInterval), ms
RealInterval	2ubyte	Configured real time one loop interval should take, ms
LastInterval	2ubyte	Real time the last loop interval was actually measured at, ms
True Pose X	4byte	Robot's "true" X position in the simulator, mm
True Pose Y	4byte	Robot's "true" Y position in the simulator, mm
True Pose Z	4byte	Robot's "true" Z position in the simulator (always 0 in 2D MobileSim), mm
True Pose Theta	4byte	Robot's "true" rotation on its vertical axis in the simulator, degrees

Here is an example of a packet handler function that extracts some data from the SIMSTAT packet:

bool handleSimStatPacket(ArRobotPacket* pkt)
{
   if(pkt->getID() != 0x62) return false; // SIMSTAT has id 0x62
   printf("SIMSTAT pkt received:\n"    );
   char a = pkt->bufToByte();  // unused byte
   char b = pkt->bufToByte();  // unused byte
   ArTypes::UByte4 flags = pkt->bufToUByte4();
   printf("\tFlags=0x%x\n", flags);
   simint = pkt->bufToUByte2();
   realint = pkt->bufToUByte2();
   lastint = pkt->bufToUByte2();
   printf("\tSimInterval=%d, RealInterval=%d, LastInterval=%d.\n", simint, realint, lastint);
   realX = pkt->bufToByte4();
   realY = pkt->bufToByte4();
   realZ = pkt->bufToByte4();
   realTh = pkt->bufToByte4();
   printf("\tTrue Pose = (%d, %d, %d, %d)\n", realX, realY, realZ, realTh);
   return true;
}