Gazebo HITL - No Ground Truth Data

JJ_Garrard · September 30, 2021, 7:13pm

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 https://docs.px4.io/master/en/simulation/hitl.html 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.

JJ_Garrard · October 2, 2021, 12:16am

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.

hamishwillee · October 6, 2021, 1:14am

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.

JJ_Garrard · October 21, 2021, 8:50pm

Hi @hamishwillee,

Thank you for your response.

I don’t know how to make a PR for this, but I think it should be placed in HITL because the issue I was facing doesn’t have anything to do with ROS. The ROS stuff I did was to see if the ground truth happened to be exposed in ROS for whatever reason, but is not necessary at all. As long as <hitl_state_level> is set to 1, the simulation sends the ground-truth in place of GPS data to the HITL FCU/FMU.

I saw my error in how to ask for help after I posted in Slack. I didn’t want to delete posts in the PX4 community in either Slack or here, but I’ll be sure to follow the proper steps next time I run into an issue.

Topic		Replies	Views
[VTOL] HIL with offboard attitude setpoints not working PX4 Autopilot	7	962	October 4, 2019
PX4 Jetson Nano HITL with Pixhawk 6C PX4 Autopilot	8	610	April 28, 2024
Fixed Wing HITL with Gazebo PX4 Autopilot	8	1759	March 22, 2024
PX4 Dev Call: November 23, 2022 PX4 Autopilot	6	657	December 1, 2022
PX4 Community Q&A: June 14, 2023 PX4 Autopilot	2	1107	June 13, 2023

Gazebo HITL - No Ground Truth Data

UPDATE 1: Obtained Ground Truth Data, New Battery Issues

UPDATE 2: How to Bypass Battery Check, Successful Takeoff

UPDATE 1: Obtained Ground Truth Data, New Battery Issues

UPDATE 2: How to Bypass Battery Check, Successful Takeoff

Related topics