Gazebo HITL - No Ground Truth Data

Hi all, I brought this issue up on Slack first before I realized I should post here. I copy-pasted the content below:

Hi all, I am having problems getting ground-truth data to my PixRacer in HITL-Gazebo simulations.

I have followed the directions from Hardware in the Loop Simulation \(HITL\) | PX4 User Guide and successfully got the simulation running, but LOCAL_POSITION_NED is computed via EKF2 and (I’m assuming) the GPS data. I didn’t find anything related to ground-truth pose in the QGC Mavlink inspector or ls /obj via the MAVLink console.

I have also tried running HITL-Gazebo via rosrun gazebo_ros gazebo Tools/sitl_gazebo/worlds/hitl_iris.world instead of gazebo Tools/sitl_gazebo/worlds/hitl_iris.world and exposed the Gazebo topics and services. The closest topic is gazebo/model_state, but this gives the pose of ALL the models in the simulation. Example output:
rostopic echo /gazebo/model_states
name:

  • ground_plane
  • asphalt_plane
  • iris
    pose:
  • position:
    x: 0.0
    y: 0.0
    z: 0.0
    orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
  • position:
    x: 0.0
    y: 0.0
    z: 0.0
    orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
  • position:
    x: 1.0100224581607107
    y: 0.9801078336020872
    z: 0.1047546771028121
    orientation:
    x: 0.0004097790291969756
    y: 0.00014674860168787708
    z: 0.5396574669559622
    w: 0.8418845698221757
    twist:
  • linear:
    x: 0.0
    y: 0.0
    z: 0.0
    angular:
    x: 0.0
    y: 0.0
    z: 0.0
  • linear:
    x: 0.0
    y: 0.0
    z: 0.0
    angular:
    x: 0.0
    y: 0.0
    z: 0.0
  • linear:
    x: 6.858998213404666e-07
    y: -6.583106232018059e-07
    z: 1.6255198940406944e-07
    angular:
    x: 1.195846982630051e-05
    y: 1.245370268900992e-05
    z: -2.4280701727543477e-09

name:

  • ground_plane
  • asphalt_plane
  • iris
    pose:
  • position:
    x: 0.0
    y: 0.0
    z: 0.0
    orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
  • position:
    x: 0.0
    y: 0.0
    z: 0.0
    orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
  • position:
    x: 1.0100224609076285
    y: 0.9801078309796427
    z: 0.10475467777516753
    orientation:
    x: 0.00040981253966840996
    y: 0.00014675674226422624
    z: 0.5396574669451889
    w: 0.8418845698113508
    twist:
  • linear:
    x: 0.0
    y: 0.0
    z: 0.0
    angular:
    x: 0.0
    y: 0.0
    z: 0.0
  • linear:
    x: 0.0
    y: 0.0
    z: 0.0
    angular:
    x: 0.0
    y: 0.0
    z: 0.0
  • linear:
    x: 6.86731235219618e-07
    y: -6.556119565150757e-07
    z: 1.6810630922092784e-07
    angular:
    x: 1.1909429464814269e-05
    y: 1.2468835854874e-05
    z: -2.4049433382352926e-09

There is also the service called via rosservice call /gazebo/get_model_state iris world which gives me a message of type gazebo_msgs/GetModelState containing the pose information I want, but I don’t know how to use this to stream ground-truth data to the PixRacer. Example output:
rosservice call /gazebo/get_model_state iris world
header:
seq: 2
stamp:
secs: 650
nsecs: 720000000
frame_id: “world”
pose:
position:
x: 1.010001223247161
y: 0.9801647104052731
z: 0.10486537762426242
orientation:
x: 0.00022615031804504906
y: 8.671929393722755e-05
z: 0.5396653124035337
w: 0.8418796184278333
twist:
linear:
x: 6.84277397551137e-07
y: -6.621219556149449e-07
z: 1.5470630426456246e-07
angular:
x: 1.2030393578193457e-05
y: 1.242475494338432e-05
z: -1.7960479759178332e-09
success: True
status_message: “GetModelState: got properties”

There is also a Gazebo topic /gazebo/hitl_iris_world/iris/groundtruth which I have tried to echo, but I get the following output:
gz topic --verbose --echo /gazebo/hitl_iris_world/iris/groundtruth
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.203.50.74
[Err] [gz_topic.cc:217] Unable to create message of type[sensor_msgs.msgs.Groundtruth]

Does anyone have any idea on how to use ground-truth data instead of GPS? The closest forum post is https://github.com/PX4/PX4-Autopilot/issues/13550, but I could not replicate the results from @tsc21, even in SITL.

Additional hardware/software info:
Ubuntu 20.04
Firmware 1.12
ROS Noetic
Airframe DJI F450 w/ DJI ESCs
HITL enabled from QGC

After posting in Slack, I dug through gazebo_groundtruth_plugin.cpp and gazebo_mavlink_interface.cpp to see how ground truth data is being passed around.

In gazebo_groundtruth_plugin.cpp, I added

gzmsg << "[gazebo_groundtruth_plugin] final altitude data: " << groundtruth_msg.altitude() << "\n";

right before the data is published, and got the following output:

gazebo Tools/sitl_gazebo/worlds/hitl_iris.world --verbose
Gazebo multi-robot simulator, version 11.8.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
Gazebo multi-robot simulator, version 11.8.1
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.203.50.74
[Msg] Loading world file [/home/jj/PX4-Autopilot/Tools/sitl_gazebo/worlds/hitl_iris.world]
[Wrn] [gazebo_gps_plugin.cpp:76] [gazebo_gps_plugin]: iris::gps0 using gps topic "gps0"
[Msg] [gazebo_gps_plugin] Home latitude is set to 34.1745.
[Msg] [gazebo_gps_plugin] Home longitude is set to -118.481.
[Msg] [gazebo_gps_plugin] Home altitude is set to 0.
[Wrn] [gazebo_gps_plugin.cpp:201] [gazebo_gps_plugin] Using default update rate of 5hz 
[Msg] [gazebo_groundtruth_plugin] Got model ptr
[Msg] [gazebo_groundtruth_plugin] Got world ptr
[Msg] [gazebo_groundtruth_plugin] env_lat:34.174528
[Msg] [gazebo_groundtruth_plugin] Home latitude is set to 34.1745.
[Msg] [gazebo_groundtruth_plugin] Home longitude is set to -118.481.
[Msg] [gazebo_groundtruth_plugin] Home altitude is set to 0.
[Msg] [gazebo_barometer_plugin] Home altitude is set to 0.
[Msg] Connecting to PX4 SITL using serial
[Msg] Lockstep is disabled
[Msg] Using MAVLink protocol v2.0
Opened serial device /dev/ttyACM0
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.83
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.829843
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.829529
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.829059
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.828431
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.827646
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.826705
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.825607
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.824351
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.822939
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.82137
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.819644
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.817761
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.815722
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.813525
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.811171
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.808661
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.805993
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.803169
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.800188
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.79705
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.793755
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.790303
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.786694
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.782928
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.779006
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.774926
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.77069
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.766296
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.761746
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.757039
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.752175
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.747154
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.741976
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.736641
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.731149
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.725501
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.719695
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.713733
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.707614
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.701337
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.694904
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.688314
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.681567
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.674663
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.667603
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.660385
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.65301
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.645479
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.637791
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.629945
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.621943
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.613784
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.605468
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.596995
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.588365
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.579579
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.203.50.74
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.570635
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.561535
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.552277
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.542863
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.533292
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.523563
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.513678
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.503636
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.493437
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.483082
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.472569
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.461899
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.451073
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.44009
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.428949
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.417652
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.406198
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.394587
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.382819
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.370894
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.358812
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.346574
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.334178
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.321626
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.308917
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.29605
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.283027
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.269847
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.25651
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.243016
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.229365
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.215558
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.201593
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.187472
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.173193
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.158758
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.144166
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.129416
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.11451
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.0994475
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001
[Msg] [gazebo_groundtruth_plugin] final altitude data: 0.104001

From this output, I know that there isn’t a silent error causing the plugin to not generate data, but I don’t know where the data ends goes.

In the MAVLink module file mavlink_main.cpp, I added the following under case MAVLINK_MODE_CONFIG:

configure_stream_local("HIL_STATE", 10.0f);
configure_stream_local("HIL_STATE_QUATERNION", 10.0f);

After I compiled and uploaded the code to my Pixracer, started the Gazebo simulator, and went to the QGC MAVLink inspector, I did not see these messages.

What other things should I try so I can get ground truth data on my drone?

UPDATE 1: Obtained Ground Truth Data, New Battery Issues

I successfully obtained the ground truth data by changing

<hil_state_level>0</hil_state_level>

to

<hil_state_level>1</hil_state_level>

However, I cannot successfully execute commander takeoff after this change. The error that pops up is Arming denied! Check battery. I can apply forces via Gazebo and verified that the response doesn’t have noise and is consistent with what I expected.

Now the question is: How do I make the HITL simulation permit takeoffs?

UPDATE 2: How to Bypass Battery Check, Successful Takeoff

To permit takeoff while not passing the battery checks, edit the parameter CBRK_SUPPLY_CHK to 894281. After I did this and restarted the Pixracer and simulator, commander takeoff worked.

I’ll consider my issues resolved for now.

Copy-pasting my solution so I can mark it as solved:

UPDATE 1: Obtained Ground Truth Data, New Battery Issues

I successfully obtained the ground truth data by changing

<hil_state_level>0</hil_state_level>

to

<hil_state_level>1</hil_state_level>

However, I cannot successfully execute commander takeoff after this change. The error that pops up is Arming denied! Check battery. I can apply forces via Gazebo and verified that the response doesn’t have noise and is consistent with what I expected.

Now the question is: How do I make the HITL simulation permit takeoffs?

UPDATE 2: How to Bypass Battery Check, Successful Takeoff

To permit takeoff while not passing the battery checks, edit the parameter CBRK_SUPPLY_CHK to 894281. After I did this and restarted the Pixracer and simulator, commander takeoff worked.

I’ll consider my issues resolved for now.

Thank you so much for taking the time to post your solution.

Much of PX4 docs assumes global positioning - i.e. no need for ground truth, and that colours a lot of the docs.

We’d be interested in a PR if you can find a good place for this. Not sure where - probably in ROS docs or VIO/MOCAP - depends on where you would most likely find it - to me seems more likely in a testing section in VIO/MOACAP than in either the HITL .

PS Just to be pendantic, normally you’d post first here on discuss. If you don’t get an answer in a day or so it is perfectly fine to post a summary of the problem (without all the logs etc) and a link to discuss. You probably got that - just wanted to make sure.