Async position ned function?

1

Hello,

I am using gazebo for SITL, I would like to have the local position of the quad in gazebo in real time.
By reading the SDK, I thought about using the position_velocity_ned_async() in order to recover this values
in real time.

During the test I printed the position from the /gazebo/dafault/pose/info in a terminal, and launched my code
in a different one, the x, y values where very different from the ones printed by gazebo.

For example, here are the Y values or the East in position ned of 3 quadcopters recoverd from the SDK function:

0.55581
0.716298
0.548354
0.555076
0.715983
0.548088
0.553975
0.715908
0.547941
0.554683
0.715726
0.548069
0.555382
0.716241
0.548653
0.555123
0.71639
0.548989
0.554848
0.716364
0.549448
0.554562
0.716297
0.54925
0.554249
0.715924
0.549104
0.554444
0.715926
0.549401
0.554698
0.716091

And here are the position information recovered by gazebo pose/info topic:

time { sec: 1398 nsec: 962000000 } pose { name: "iris_1" id: 42 position { x: 3.599438443085119 y: 0.35564963712424574 z: 0.1044579829735158 } orientation { x: -0.00043751701823432394 y: 0.0007984126010138733 z: 0.5434604435386261 w: 0.83943428416105159 } } pose { name: "iris_1::base_link" id: 43 position { x: 0 y: 0 z: 0 } orientation {
x: 0 y: 0 z: 0 w: 1 } } pose { name: "iris_1::/imu_link" id: 48 position { x: -1.999199784128076e-09 y: -3.5128961117295867e-09 z: 8.7308655252091786e-09 } orientation {
x: 1.6910008803915251e-09 y: -9.7351125302345617e-09 z: -4.81256867690405e-10 w: 1 } } pose { name: "iris_1::rotor_0" id: 49 position { x: 0.12999999712605945 y: -0.22000000387402513 z: 0.023000008422416351 } orientation { x: 1.6159322251420012e-06 y: -4.6211187750413519e-06 z: 0.66817297167299439 w: -0.7440059676519496 } } pose { name: "iris_1::rotor_1" id: 54 position { x: -0.13000000256107108 y: 0.19999999663095169 z: 0.0230000079041404 } orientation { x: -1.7971821553150891e-06 y: 1.5941229982065075e-05 z: -0.856809632107163 w: 0.51563286752401194 } } pose { name: "iris_1::rotor_2" id: 59 position { x: 0.12999999703937068 y: 0.21999999629785763 z: 0.023000013912385868 } orientation { x: 1.9334953841083241e-07 y: -1.0887737659605388e-06 z: 0.98073617956487258 w: -0.19533700645621482 } } pose { name: "iris_1::rotor_3" id: 64 position {
x: -0.13000000350505447 y: -0.20000000356522057 z: 0.023000004047376937 } orientation { x: -1.1190149783297748e-06 y: -9.0486869859667538e-07 z: -0.80149946618558154 w: -0.59799548970051331 } } pose { name: "iris" id: 15 position { x: 3.7341541676558219 y: 3.5606459450314354 z: 0.10451233993591215 } orientation { x: -0.00038581070191812431 y: -0.00043472023526426861 z: 0.54999490953556307 w: 0.8351678044880485 } } pose { name: "iris::base_link" id: 16 position { x: 0 y: 0 z: 0 } orientation { x: 0 y: 0 z: 0 w: 1 } } pose { name: "iris::/imu_link" id: 21 position { x: -2.1157448614582017e-09 y: -3.6891263685876832e-09 z: 9.7225008668260342e-09 } orientation { x: 1.1172326514451159e-09 y: -1.604098516484135e-08 z: -5.4329751808523952e-10 w: 1 } } pose { name: "iris::rotor_0" id: 22 position { x: 0.1299999969273257 y: -0.22000000411098974 z: 0.023000009354274084 } orientation { x: 7.65151828411789e-06 y: -3.2198511895103819e-05 z: -0.99444937944464762 w: 0.1052161139125822 } } pose { name: "iris::rotor_1" id: 27 position { x: -0.13000000272561052 y: 0.19999999641778984 z: 0.023000008834674367 } orientation { x: -1.5038329751504731e-06 y: 5.2570497162600274e-06 z: -0.69013899063416728 w: 0.72367684333309623 } } pose { name: "iris::rotor_2" id: 32 position { x: 0.129999996921997 y: 0.2199999961564838 z: 0.023000014758987847 } orientation { x:
1.6621351113754687e-06 y: -3.1869275770567888e-06 z: 0.6370804259353926 w: -0.77079733450308419 } } pose { name: "iris::rotor_3" id: 37 position { x: -0.13000000361385461 y: -0.20000000374356067 z: 0.023000004918563206 } orientation { x: -1.3045417959954727e-05 y: -4.0787006149755646e-06 z: -0.17051608519809597 w: 0.9853548926665443 } } pose { name: "iris_2" id: 69 position { x: 0.5827067722833067 y: 3.3437887435362441 z: 0.10455721843038902 } orientation { x: -0.00051925538510161 y: -0.00035073747009964487 z: 0.54712128981192221 w: 0.83705310559821167 } } pose { name: "iris_2::base_link" id: 70 position { x: 0 y: 0 z: 0 } orientation { x: 0 y: 0 z: 0 w: 1 } } pose { name: "iris_2::/imu_link" id: 75 position { x: -2.110367831949147e-09 y: -3.6234917724589434e-09 z: 9.7089796654184568e-09 } orientation { x: 5.0042395899861647e-11 y: -1.7095886210643455e-08 z: -5.1664172939780428e-10 w: 1 } } pose { name: "iris_2::rotor_0" id: 76 position { x: 0.12999999670459123 y: -0.22000000403158743 z: 0.023000010758412071 } orientation { x: 1.4662043906815059e-06 y: -6.6476753721339548e-06 z: -0.94062020994260576 w: -0.33946077917954942 } } pose { name: "iris_2::rotor_1" id: 81 position { x: -0.13000000290752423 y: 0.19999999640535734 z: 0.023000008444018543 } orientation { x: -5.590388300320315e-06 y: 1.8065633792531336e-05 z: 0.99638305337661393 w: 0.084975352816426139 } } pose { name: "iris_2::rotor_2" id: 86 position { x: 0.1299999969262238 y: 0.21999999625409336 z: 0.023000014433517534 } orientation { x: -5.971157891542032e-06 y: 3.4379820091289993e-05 z: -0.95127967223876331 w: 0.30832934334520351 } } pose { name: "iris_2::rotor_3" id: 91 position { x: -0.13000000360139588 y: -0.2000000036868258 z: 0.023000004890296959 } orientation { x: 3.9243829361632859e-06 y: 1.6417131934893254e-07 z: 0.42278070974728721 w: -0.90623201855272828 }

Sorry for gazebo output,

So the question is Why they are different?? Am I missing something??
Best regards,

2

The coordinate frame of the SDK is NED (North, East, Down), however, the coordinate frame of Gazebo might be NWU (North, West, Up) (I’m not 100 % sure on this).

Would it make sense in that case?

3

Not really,

The z values are correct

After analyzing the results, it is only the numbers on the right of the comma are correct.

I would like to know How it was implemented the function position_velocity_ned_async() ??

Does it use the subscribe system of Gazebo to recover the values?

From gazebo:

http://playerstage.sourceforge.net/doc/Gazebo-manual-0.8.0-pre1-html/config_syntax.html

Coordinate Systems and Units
By convention, Gazebo uses a right-handed coordinate system, with x and y in the plane, and z increasing with altitude. Most models are designed such that the are upright (with respect to the z axis) and pointing along the positive x axis. The tag <xyz;> is used to indicate an object's position (x, y and z coordinates); the tag <rpy;> is used to indicate an objects orientation (Euler angles; i.e., roll, pitch and yaw). For example, <xyz;>1 2 3</xyz;> indicates a translation of 1~m along the x-axis, 2~m along the y-axis and 3~m along the z-axis; <rpy;>10 20 30</rpy;> indicates a rotation of 30~degrees about the z-axis (yaw), followed by a rotation of 20~degrees about the y-axis (pitch) and a rotation of 10~degrees about the x-axis (roll).

Unless otherwise specified, the world file uses SI units (meters, seconds, kilograms, etc). One exception is angles and angular velocities, which are measured in degrees and degrees/sec, respectively.
4

I would like to see another output, just to be sure that if there is a bug or I missed up something

5

I don’t understand what you mean.

So that would be x/forward/North, y/left/West, z/up/Up → NWU

6

Hello,

I mean that if you look at name: “iris” id: 15 position { x: 3.7341541676558219 y: 3.5606459450314354 z: 0.10451233993591215 }
and the Y value in the position_ned values, I notices that for instance: 0.554562 is similar to y: 3.5606459450314354, but when 3 is equal 0.
I thought that, maybe there is some kind of cast or conversion that is assigning the first digit to 0.

Because it is not normal to have only the z values that are the same, and the others should be different.

I understand your point about NWU. However, I do not understand why values are not similar.

Finally I recover the values by subscribing to the gazebo topic. But I think it is very important
to have a function implemented in the SDK that allow us to have the local position values of the simulator
directly,

One point to add, It is possible to covert the GPS position values to the local one using a well
mathematical formulas. But why to do that if the SDK provides better values.

Thanks,

7

Ok I think now I understand the confusion. So we are not necessarily comparing the same numbers here.

The numbers you get using the SDK are from the local position. The local position is initialized wherever the EKF first initializes its position, so then the position is local to this EKF origin.

In Gazebo however, the number is relative to the origin of Gazebo, so where the green, red, blue lines cross. If you start up the simulation, you can actually see that the drone is not set in the origin but offset a bit. I assume that is the difference that you see.