gz: Use pose specified by PX4_GZ_MODEL_POSE (#24956)

* gz: use pose specified by PX4_GZ_MODEL_POSE

* gz: fix empty PX4_GZ_MODEL_POSE case

* gz: no pose rather than zero on empty argument

ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim

@@ -110,25 +110,36 @@ if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
 	MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
 	MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
 
-	POSE_ARG=""
-	if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
-		pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
-		pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
-		pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
-		pos_x=${pos_x:-0}
-		pos_y=${pos_y:-0}
-		pos_z=${pos_z:-0}
+	sdf_pose_str=""
 
-		POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
-		echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
+	if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
+		pose_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
+		pose_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
+		pose_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
+		pose_roll=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $4}')
+		pose_pitch=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $5}')
+		pose_yaw=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $6}')
+
+		pose_x=${pose_x:-0}
+		pose_y=${pose_y:-0}
+		pose_z=${pose_z:-0}
+		pose_roll=${pose_roll:-0}
+		pose_pitch=${pose_pitch:-0}
+		pose_yaw=${pose_yaw:-0}
+
+		sdf_pose_str="<pose> ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw} </pose>"
+		echo "INFO  [init] Gazebo model pose: ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw}"
 	fi
 
-	echo "INFO  [init] Spawning model"
+	echo "INFO  [init] Spawning Gazebo model"
+
+	# include the actual SDF in this one, containing the pose if given
+	sdf_str="<sdf version=\"1.6\"> <include> <uri>file://${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf</uri> ${sdf_pose_str} </include> </sdf>"
 
 	# Spawn model
 	${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
 			--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
+			--req "name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false, sdf: '${sdf_str}'" > /dev/null 2>&1
 
 	# Wait for model to spawn
 	sleep 1