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_gazebo

@@ -1 +1 @@
-Subproject commit 11b6409df9f2087c6069ba4db29ae8cef33622da
+Subproject commit dc60f209db24712721dd9481e2244b7b0b35a9db

Tools/sitl_run.sh

@@ -2,8 +2,8 @@
 
 set -e
 
-if [ "$#" -lt 6 ]; then
+if [ "$#" -lt 7 ]; then
-	echo usage: sitl_run.sh sitl_bin debugger program model src_path build_path
+	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"
+world="$5"
-build_path="$6"
+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
+    # Set the plugin path so Gazebo finds our model and sim
-	source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
+    source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
-
+		if [ -z $PX4_SITL_WORLD ]; then
-			gzserver "${src_path}/Tools/sitl_gazebo/worlds/${model}.world" &
+			#Spawn predefined world
-			SIM_PID=`echo $!`
+			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
+
+		SIM_PID=`echo $!`
 
 	if [[ -n "$HEADLESS" ]]; then
 		echo "not running gazebo gui"

platforms/posix/cmake/sitl_target.cmake

@@ -66,30 +66,34 @@ set(models none shell
 	standard_vtol tailsitter tiltrotor
 	rover boat
 	uuv_hippocampus)
+set(worlds none empty warehouse)
 set(all_posix_vmd_make_targets)
 foreach(viewer ${viewers})
 	foreach(debugger ${debuggers})
 		foreach(model ${models})
-			if (debugger STREQUAL "none")
+			foreach(world ${worlds})
-				if (model STREQUAL "none")
+				if (world STREQUAL "none")
-					set(_targ_name "${viewer}")
+					if (debugger STREQUAL "none")
-				else()
+						if (model STREQUAL "none")
-					set(_targ_name "${viewer}_${model}")
+							set(_targ_name "${viewer}")
-				endif()
+						else()
-			else()
+							set(_targ_name "${viewer}_${model}")
-				if (model STREQUAL "none")
+						endif()
-					set(_targ_name "${viewer}___${debugger}")
+					else()
-				else()
+						if (model STREQUAL "none")
-					set(_targ_name "${viewer}_${model}_${debugger}")
+							set(_targ_name "${viewer}___${debugger}")
-				endif()
+						else()
-			endif()
+							set(_targ_name "${viewer}_${model}_${debugger}")
+						endif()
+					endif()
 
-			add_custom_target(${_targ_name}
+					add_custom_target(${_targ_name}
 					COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
 						$<TARGET_FILE:px4>
 						${debugger}
 						${viewer}
 						${model}
+						${world}
 						${PX4_SOURCE_DIR}
 						${PX4_BINARY_DIR}
 					WORKING_DIRECTORY ${SITL_WORKING_DIR}
@@ -97,12 +101,47 @@ foreach(viewer ${viewers})
 					DEPENDS
 						logs_symlink
 					)
-			list(APPEND all_posix_vmd_make_targets ${_targ_name})
+					list(APPEND all_posix_vmd_make_targets ${_targ_name})
-			if (viewer STREQUAL "gazebo")
+					if (viewer STREQUAL "gazebo")
-				add_dependencies(${_targ_name} px4 sitl_gazebo)
+						add_dependencies(${_targ_name} px4 sitl_gazebo)
-			elseif(viewer STREQUAL "jmavsim")
+					elseif(viewer STREQUAL "jmavsim")
-				add_dependencies(${_targ_name} px4 git_jmavsim)
+						add_dependencies(${_targ_name} px4 git_jmavsim)
-			endif()
+					endif()
+				else()
+					if (viewer STREQUAL "gazebo")
+						if (debugger STREQUAL "none")
+							if (model STREQUAL "none")
+								set(_targ_name "${viewer}___${world}")
+							else()
+								set(_targ_name "${viewer}_${model}__${world}")
+							endif()
+						else()
+							if (model STREQUAL "none")
+								set(_targ_name "${viewer}__${debugger}_${world}")
+							else()
+								set(_targ_name "${viewer}_${model}_${debugger}_${world}")
+							endif()
+						endif()
+
+						add_custom_target(${_targ_name}
+						COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh
+							$<TARGET_FILE:px4>
+							${debugger}
+							${viewer}
+							${model}
+							${world}
+							${PX4_SOURCE_DIR}
+							${PX4_BINARY_DIR}
+						WORKING_DIRECTORY ${SITL_WORKING_DIR}
+						USES_TERMINAL
+						DEPENDS
+							logs_symlink
+						)
+						list(APPEND all_posix_vmd_make_targets ${_targ_name})
+						add_dependencies(${_targ_name} px4 sitl_gazebo)
+					endif()
+				endif()
+			endforeach()
 		endforeach()
 	endforeach()
 endforeach()

platforms/posix/cmake/sitl_tests.cmake

@@ -61,6 +61,7 @@ foreach(test_name ${tests})
 			none
 			none
 			test_${test_name}_generated
+			none
 			${PX4_SOURCE_DIR}
 			${PX4_BINARY_DIR}
 		WORKING_DIRECTORY ${SITL_WORKING_DIR})
@@ -79,6 +80,7 @@ add_test(NAME mavlink
 		none
 		none
 		test_mavlink
+		none
 		${PX4_SOURCE_DIR}
 		${PX4_BINARY_DIR}
 	WORKING_DIRECTORY ${SITL_WORKING_DIR})
@@ -96,6 +98,7 @@ if(NOT CMAKE_SYSTEM_NAME STREQUAL "CYGWIN")
 			none
 			none
 			test_shutdown
+			none
 			${PX4_SOURCE_DIR}
 			${PX4_BINARY_DIR}
 		WORKING_DIRECTORY ${SITL_WORKING_DIR})
@@ -112,6 +115,7 @@ add_test(NAME dyn
 		none
 		none
 		test_dyn_hello
+		none
 		${PX4_SOURCE_DIR}
 		${PX4_BINARY_DIR}
 		$<TARGET_FILE:examples__dyn_hello>
@@ -135,6 +139,7 @@ foreach(cmd_name ${test_cmds})
 			none
 			none
 			cmd_${cmd_name}_generated
+			none
 			${PX4_SOURCE_DIR}
 			${PX4_BINARY_DIR}
 		WORKING_DIRECTORY ${SITL_WORKING_DIR})