private void ParseState(USARParser msg)
{
float[] curPose = USARParser.parseFloats(msg.getSegment("Pose").Get("Pose"), ",");
x = curPose[0];
y = curPose[1];
theta = curPose[2];
}
private void ParseState(USARParser msg)
{
float[] curLocation = USARParser.parseFloats(msg.getSegment("Location").Get("Location"), ",");
float[] curRotation = USARParser.parseFloats(msg.getSegment("Orientation").Get("Orientation"), ",");
normalRotation(curRotation);
p3Loc = new Vector3(curLocation[0], curLocation[1], curLocation[2]);
p3Ori = new Vector3(curRotation[0], curRotation[1], curRotation[2]);
}