Spawn models separately from gazebo models

Detach model spawning process from the world description file, so that the same model can be used with different worlds

Tools/sitl_run.sh

@@ -2,8 +2,8 @@
 
 set -e
 
-if [ "$#" -lt 6 ]; then
-	echo usage: sitl_run.sh sitl_bin debugger program model src_path build_path
+if [ "$#" -lt 7 ]; then
+	echo usage: sitl_run.sh sitl_bin debugger program model world src_path build_path
 	exit 1
 fi
 
@@ -16,8 +16,9 @@ sitl_bin="$1"
 debugger="$2"
 program="$3"
 model="$4"
-src_path="$5"
-build_path="$6"
+world="$5"
+src_path="$6"
+build_path="$7"
 # The rest of the arguments are files to copy into the working dir.
 
 echo SITL ARGS
@@ -26,6 +27,7 @@ echo sitl_bin: $sitl_bin
 echo debugger: $debugger
 echo program: $program
 echo model: $model
+echo world: $world
 echo src_path: $src_path
 echo build_path: $build_path
 
@@ -64,7 +66,7 @@ fi
 cp "$src_path/Tools/posix_lldbinit" "$rootfs/.lldbinit"
 cp "$src_path/Tools/posix.gdbinit" "$rootfs/.gdbinit"
 
-shift 6
+shift 7
 for file in "$@"; do
 	cp "$file" $rootfs/
 done
@@ -77,11 +79,34 @@ if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]; then
 	SIM_PID=`echo $!`
 elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then
 	if [ -x "$(command -v gazebo)" ]; then
-	# Set the plugin path so Gazebo finds our model and sim
-	source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
+    # Set the plugin path so Gazebo finds our model and sim
+    source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
+		if [ -z $PX4_SITL_WORLD ]; then
+			#Spawn predefined world
+			if [ "$world" == "none" ]; then
+				if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${model}.world ]; then
+					echo "empty world, default world ${model}.world for model found"
+					gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" &
+				else
+					echo "empty world, setting empty.world as default"
+					gzserver "${src_path}/Tools/sitl_gazebo/worlds/empty.world" &
+				fi
+			else
+				#Spawn empty world if world with model name doesn't exist
+				gzserver "${src_path}/Tools/sitl_gazebo/worlds/${world}.world" &
+			fi
+		else
+			if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world ]; then
+				# Spawn world by name if exists in the worlds directory from environment variable
+				gzserver "${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world" &
+			else
+				# Spawn world from environment variable with absolute path
+				gzserver "$PX4_SITL_WORLD" &
+			fi
+        fi
+		gz model --spawn-file="${src_path}/Tools/sitl_gazebo/models/${model}/${model}.sdf" --model-name=${model} -x 1.01 -y 0.98 -z 0.83
 
-			gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" &
-			SIM_PID=`echo $!`
+		SIM_PID=`echo $!`
 
 	if [[ -n "$HEADLESS" ]]; then
 		echo "not running gazebo gui"