From 3b66166d6cabf3667d574c1200ceb6fe46b476c1 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Date: Mon, 17 Mar 2025 10:29:10 -0800
Subject: [PATCH] gz: wait for world before spawning model, delay 1s after
 model spawn to allow time (#24516)

---
 .../init.d-posix/px4-rc.simulator             | 57 ++++++++++---------
 1 file changed, 29 insertions(+), 28 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index 5f5831ebef0..2e62285f3e9 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -95,6 +95,32 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 				echo "INFO  [init] Starting gz gui"
 				${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
 			fi
+
+			# Wait for Gazebo world to be ready before proceeding
+			check_scene_info() {
+				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
+				else
+					return 1
+				fi
+			}
+
+			ATTEMPTS=30
+			while [ $ATTEMPTS -gt 0 ]; do
+				if check_scene_info; then
+					echo "INFO  [init] Gazebo world is ready"
+					break
+				fi
+				ATTEMPTS=$((ATTEMPTS-1))
+				if [ $ATTEMPTS -eq 0 ]; then
+					echo "ERROR [init] Timed out waiting for Gazebo world"
+					exit 1
+				fi
+				echo "INFO  [init] Waiting for Gazebo world..."
+				sleep 1
+			done
+
 		else
 			# Gazebo is already running
 			echo "INFO  [init] gazebo already running world: ${gz_world}"
@@ -131,6 +157,9 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 				--reptype gz.msgs.Boolean --timeout 5000 \
 				--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
 
+		# Wait for model to spawn
+		sleep 1
+
 		# Start gz_bridge
 		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then
 			echo "ERROR [init] gz_bridge failed to start and spawn model"
@@ -138,30 +167,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 		fi
 
 		# Set physics parameters for faster-than-realtime simulation if needed
-		check_scene_info() {
-			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
-			else
-				return 1
-			fi
-		}
-
-		ATTEMPTS=30
-		while [ $ATTEMPTS -gt 0 ]; do
-			if check_scene_info; then
-				echo "INFO  [init] Gazebo world is ready"
-				break
-			fi
-			ATTEMPTS=$((ATTEMPTS-1))
-			if [ $ATTEMPTS -eq 0 ]; then
-				echo "ERROR [init] Timed out waiting for Gazebo world"
-				exit 1
-			fi
-			echo "INFO  [init] Waiting for Gazebo world..."
-			sleep 1
-		done
-
 		if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
 			echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
 			${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
@@ -169,13 +174,9 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 						--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
 		fi
 
-
 		# Set up camera to follow the model if requested
 		if [ -n "${PX4_GZ_FOLLOW}" ]; then
 
-			# Wait for model to spawn
-			sleep 1
-
 			echo "INFO  [init] Setting camera to follow ${MODEL_NAME_INSTANCE}"
 
 			# Set camera to follow the model