From 9b25e6df339104bbcd666bd21007215376ca5d3b Mon Sep 17 00:00:00 2001 From: mt <141614344+marko-castral@users.noreply.github.com> Date: Tue, 9 Jun 2026 04:50:07 +0200 Subject: [PATCH] fix(simulation): preserve gravity and step_size in px4-rc.gzsim set_physics (#27488) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(simulation): preserve gravity and step_size in px4-rc.gzsim set_physics The set_physics service call in px4-rc.gzsim sent only real_time_factor. Other fields in gz.msgs.Physics (gravity, max_step_size) defaulted to 0 per protobuf semantics, silently zeroing world gravity in the gz physics engine when PX4_SIM_SPEED_FACTOR was set. Vehicles became weightless while PX4's accelerometer (sourced from the gz IMU plugin's cached SDF gravity) continued to report -9.81 m/s², making the bug invisible from PX4's side. Resolve the values via the live gz physics topic, fall back to the world SDF, and finally to Earth gravity / gz-harmonic default step size, then include the full set in the set_physics request. Fixes #27480 Signed-off-by: Marko T * fix(simulation): resolve world name in standalone and drop dead physics topic The gz physics topic queried for gravity/max_step_size does not exist in gz Harmonic (only a write-only set_physics service), so that branch always timed out and fell through. Read both values directly from the world SDF, which is the source of truth at load time. Also resolve PX4_GZ_WORLD from the running gz instance (clock topic) inside the readiness check, so standalone launches — where the world name is not known up front — can find the scene and the world SDF. Signed-off-by: Marko T * Update ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * Update ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * docs(simulation): clarify gravity-default fallback comment in px4-rc.gzsim Fix typo ("greavity") and reword the comment to clearly state that the hardcoded gravity/step-size defaults are only used when the world SDF cannot be located, and are wrong for custom-gravity worlds. Signed-off-by: Marko T --------- Signed-off-by: Marko T Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim index 5b4676e60e5..c5003d6db11 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim @@ -64,8 +64,13 @@ else echo "INFO [init] Standalone PX4 launch, waiting for Gazebo" fi -# Wait for Gazebo world to be ready before proceeding +# Wait for Gazebo world to be ready before proceeding. +# In standalone mode the world name is not known yet, so discover it from the running gz instance via the clock topic check_scene_info() { + if [ -z "${PX4_GZ_WORLD}" ]; then + PX4_GZ_WORLD=$( ${gz_command} topic -l 2>/dev/null | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) + [ -z "${PX4_GZ_WORLD}" ] && return 1 + fi SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1) if echo "$SERVICE_INFO" | grep -q "Service providers"; then return 0 @@ -150,12 +155,35 @@ if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then exit 1 fi - # Set physics parameters for faster-than-realtime simulation if needed + # Set physics parameters for faster-than-realtime or slower-than-realtime simulation if needed. if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then echo "INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}" + + WORLD_SDF="${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" + if [ -f "${WORLD_SDF}" ]; then + sdf_gravity=$(grep -m1 '' "${WORLD_SDF}" 2>/dev/null \ + | sed -e 's|.*||' -e 's|.*||') + if [ -n "${sdf_gravity}" ]; then + GZ_GRAVITY_X=$(echo "${sdf_gravity}" | awk '{print $1}') + GZ_GRAVITY_Y=$(echo "${sdf_gravity}" | awk '{print $2}') + GZ_GRAVITY_Z=$(echo "${sdf_gravity}" | awk '{print $3}') + fi + MAX_STEP_SIZE=$(grep -m1 '' "${WORLD_SDF}" 2>/dev/null \ + | sed -e 's|.*||' -e 's|.*||') + fi + + # Fall back to defaults. These are wrong for custom-gravity worlds and are only used when the + # world SDF cannot be located (e.g. standalone launch with PX4_GZ_WORLDS unset). + GZ_GRAVITY_X=${GZ_GRAVITY_X:-0} + GZ_GRAVITY_Y=${GZ_GRAVITY_Y:-0} + GZ_GRAVITY_Z=${GZ_GRAVITY_Z:--9.80665} + MAX_STEP_SIZE=${MAX_STEP_SIZE:-0.004} + + echo "INFO [init] Preserving physics: gravity=(${GZ_GRAVITY_X}, ${GZ_GRAVITY_Y}, ${GZ_GRAVITY_Z}), max_step_size=${MAX_STEP_SIZE}" + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \ --reptype gz.msgs.Boolean --timeout 5000 \ - --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1 + --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}, max_step_size: ${MAX_STEP_SIZE}, gravity: {x: ${GZ_GRAVITY_X}, y: ${GZ_GRAVITY_Y}, z: ${GZ_GRAVITY_Z}}" > /dev/null 2>&1 fi # Set up camera to follow the model if requested