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