[PlayerStage-Gazebo] Problem with angle meassurement

[Tom K. <2v...@gm...>]

Hello,

currently I am writing on program which uses Gazebo.
For this application I need to getthe error in position and turn changes.
For this I use a Truth Widget in my Pioneer:

<model:Pioneer2AT>
    <id>robot1</id>
    <xyz>6.0 4.0 0.0</xyz>

    <model:TruthWidget>
      <id>truth1</id>
    </model:TruthWidget>

    <model:SickLMS200>
      <id>laser1</id>
      <xyz>0.0 0.0 0.30</xyz>

      <model:SonyVID30>
        <id>camera1</id>
        <xyz>0 0 0.20</xyz>
        <rpy>0 0 0</rpy>
        <updateRate>1</updateRate>
      </model:SonyVID30>
    </model:SickLMS200>
  </model:Pioneer2AT>


Further I use Player and the PositionProxy2D to get a "disturbed" signal
source.
The true position I get via:

    gz_client_t *client = gz_client_alloc();
    gz_truth_t *true_pose;
    gz_error_init(1, 9);
    gz_client_connect_wait(client, 0, 0)
    true_pose = gz_truth_alloc();
    gz_truth_open(true_pose, client, "truth1")

    gz_client_wait(client);
    gz_truth_lock(true_pose, 1);
    gz_truth_unlock(true_pose);

My aim is it to compare the x,y and yaw position of the sources.
I ran a few Tests and drove a bit around with the robot to get some
position information.
This information I wrote into a file with the following format:
trueX trueY trueRotYaw proxyX proxyY ProxyRotYaw

Code:
fprintf(logFile, "%f %f %f || %f %f %f\n", // || %f %f %f\n",
    true_pose->data->pos[0], true_pose->data->pos[1],
true_pose->data->rot[2],
    6 + odoPosition.getX(), 4 + odoPosition.getY(),
odoPosition.getOrientation());

Here are some lines of it:

6.000046 4.000000 -0.000638 || 6.000056 4.000000 -0.000000
6.000046 4.000000 -0.000638 || 6.000056 4.000000 -0.000000
6.000046 4.000000 -0.000638 || 6.000056 4.000000 -0.000000
6.000046 4.000000 -0.000638 || 6.000056 4.000000 -0.000000
6.000046 4.000000 -0.000638 || 6.000056 4.000000 -0.000000
6.000601 4.000000 -0.001147 || 6.000056 4.000000 -0.000000
6.001439 4.000000 -0.001363 || 6.001456 4.000000 -0.000000
6.029394 4.000000 -0.011561 || 6.023456 4.000000 -0.000000
6.075261 4.000000 -0.007479 || 6.053856 4.000000 -0.000000
6.107547 4.000000 -0.006650 || 6.053856 4.000000 -0.000000
6.143922 4.000000 -0.004845 || 6.107655 4.000000 0.000000
6.198478 4.000000 -0.004686 || 6.168255 4.000000 0.000000
6.276509 4.000000 -0.005272 || 6.244055 4.000000 0.000000
...
...
9.055434 4.019617 -0.000619 || 9.029948 4.016208 0.105077
9.207117 4.035112 0.000044 || 9.156819 4.034275 0.196018
9.330768 4.057180 -0.000182 || 9.281331 4.064546 0.302197
9.450539 4.091749 -0.000899 || 9.401550 4.108825 0.428952
9.564016 4.141216 -0.000272 || 9.514443 4.169298 0.586018
9.628628 4.178575 -0.000308 || 9.514443 4.169298 0.586018
9.690449 4.221245 -0.000582 || 9.616489 4.246689 0.743094
9.749380 4.268831 -0.000690 || 9.705174 4.339092 0.900174
9.893024 4.415460 -0.000422 || 9.778312 4.444230 1.057253
9.942682 4.476048 -0.000445 || 9.834101 4.559514 1.214331
...

I encountered sveral problems which I can not solve:
1. For X and Y position I have to add 6 to the proxy-X and 4 to the
proxy-Y - 6 and 4 are the starting coordinates of the robot - Why does
it not know the correct position? I guess it simulates a odomotry model
and at the beginning it has not "driven" at all - so it meassures the
movement - not the position (e.g. the relative position to the starting
point) - is this correct?

2. Truth Widget seems somehow instable - at the beinning, the robot
procy sees no movement at all - which makes sense as I am just standing
at the start point - and the truth widget suddenly starts turning. This
happens also in other moments - it's values just change a little all the
time. Why?

3. I read that the angles are meassured in degrees are referencing a
euler "space". But I can not image that these values are degrees. The
truth widgets values (3rd column) are way to small and the the proxy
values (6th column) are bigger but still to small. For the Proxy-Data I
think it is between -pi and pi. But I am not sure.
How are the angles meassured for truth widget and PositionProxy2D?

I know these are pretty much questions - but I did not find a solution.

Thanks in advance
Tom