Fixed state-grount-truth plugin

The way that the offset it used to transform the position seemed to be incorrect. The bug can be seen when the skidpad mission. When the car is moved forward the y coordinate increases in the /ground_truth/odom topic while it should be x that increases. I think it's because the position is not rotated because of the initial orientation, and the angle should be multiplied by the transpose of the offset. I found that the offset can just be subtracted from the pos and it does all the correct calculations.