From 3ef852c8e38fb52303bc1646dc276d026fd1a7a1 Mon Sep 17 00:00:00 2001
From: JaeyoungLim <jaeyoung@auterion.com>
Date: Wed, 24 Jun 2020 08:23:16 +0200
Subject: [PATCH] sitl: enable spawning different vehicle types for
 multivehicle SITL in gazebo (#15147)

This can be run with the command `Toos/gazebo_multiple_sitl_run.sh -s plane:3,iris:1
---
 Tools/gazebo_sitl_multiple_run.sh | 99 ++++++++++++++++++++-----------
 1 file changed, 66 insertions(+), 33 deletions(-)

diff --git a/Tools/gazebo_sitl_multiple_run.sh b/Tools/gazebo_sitl_multiple_run.sh
index 92500c682d..5b8da524f1 100755
--- a/Tools/gazebo_sitl_multiple_run.sh
+++ b/Tools/gazebo_sitl_multiple_run.sh
@@ -11,37 +11,58 @@ function cleanup() {
 	pkill gzclient
 }
 
+function spawn_model() {
+	MODEL=$1
+	N=$2 #Instance Number
+
+	if [ "$MODEL" != "iris" ] && [ "$MODEL" != "plane" ] && [ "$MODEL" != "standard_vtol" ]
+	then
+		echo "Currently only the following vehicle models are supported! [iris, plane, standard_vtol]"
+		exit 1
+	fi
+
+	working_dir="$build_path/instance_$n"
+	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
+
+	pushd "$working_dir" &>/dev/null
+	echo "starting instance $n in $(pwd)"
+	../bin/px4 -i $n -d "$src_path/ROMFS/px4fmu_common" -w sitl_${MODEL}_${n} -s etc/init.d-posix/rcS >out.log 2>err.log &
+	python3 ${src_path}/Tools/sitl_gazebo/scripts/xacro.py ${src_path}/Tools/sitl_gazebo/models/rotors_description/urdf/${MODEL}_base.xacro \
+		rotors_description_dir:=${src_path}/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(($mavlink_udp_port+$N)) \
+		mavlink_tcp_port:=$(($mavlink_tcp_port+$N))  -o /tmp/${MODEL}_${N}.urdf
+
+	gz sdf -p  /tmp/${MODEL}_${N}.urdf > /tmp/${MODEL}_${n}.sdf
+	echo "Spawning ${MODEL}_${N}"
+
+	gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x 0.0 -y $((3*${N})) -z 0.0
+
+	popd &>/dev/null
+
+}
+
 if [ "$1" == "-h" ] || [ "$1" == "--help" ]
 then
-	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>]"
+	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-w <world>] [-s <script>]"
+	echo "-s flag is used to script spawning vehicles e.g. $0 -s iris:3,plane:2"
 	exit 1
 fi
 
-while getopts n:m:w: option
+while getopts n:m:w:s: option
 do
 	case "${option}"
 	in
 		n) NUM_VEHICLES=${OPTARG};;
 		m) VEHICLE_MODEL=${OPTARG};;
 		w) WORLD=${OPTARG};;
+		s) SCRIPT=${OPTARG};;
 	esac
 done
 
 num_vehicles=${NUM_VEHICLES:=3}
 export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris}
 world=${WORLD:=empty}
+echo ${SCRIPT}
 
-if [ "$PX4_SIM_MODEL" != "iris" ] && [ "$PX4_SIM_MODEL" != "plane" ] && [ "$PX4_SIM_MODEL" != "standard_vtol" ]
-then
-	echo "Currently only the following vehicle models are supported! [iris, plane, standard_vtol]"
-	exit 1
-fi
-
-if [ $num_vehicles -gt 255 ]
-then
-	echo "Tried spawning $num_vehicles vehicles. The maximum number of supported vehicles is 255"
-	exit 1
-fi
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 src_path="$SCRIPT_DIR/.."
@@ -62,27 +83,39 @@ gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose &
 sleep 5
 
 n=0
-while [ $n -lt $num_vehicles ]; do
-	working_dir="$build_path/instance_$n"
-	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
-
-	pushd "$working_dir" &>/dev/null
-	echo "starting instance $n in $(pwd)"
-	../bin/px4 -i $n -d "$src_path/ROMFS/px4fmu_common" -w sitl_${PX4_SIM_MODEL}_${n} -s etc/init.d-posix/rcS >out.log 2>err.log &
-	python3 ${src_path}/Tools/sitl_gazebo/scripts/xacro.py ${src_path}/Tools/sitl_gazebo/models/rotors_description/urdf/${PX4_SIM_MODEL}_base.xacro \
-		rotors_description_dir:=${src_path}/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(($mavlink_udp_port+$n)) \
-		mavlink_tcp_port:=$(($mavlink_tcp_port+$n))  -o /tmp/${PX4_SIM_MODEL}_${n}.urdf
-
-	gz sdf -p  /tmp/${PX4_SIM_MODEL}_${n}.urdf > /tmp/${PX4_SIM_MODEL}_${n}.sdf
-	echo "Spawning ${PX4_SIM_MODEL}_${n}"
-
-	gz model --spawn-file=/tmp/${PX4_SIM_MODEL}_${n}.sdf --model-name=${PX4_SIM_MODEL}_${n} -x 0.0 -y $((3*${n})) -z 0.0
-
-	popd &>/dev/null
-
-	n=$(($n + 1))
-done
+if [ -z ${SCRIPT} ]; then
+	if [ $num_vehicles -gt 255 ]
+	then
+		echo "Tried spawning $num_vehicles vehicles. The maximum number of supported vehicles is 255"
+		exit 1
+	fi
+
+	while [ $n -lt $num_vehicles ]; do
+		spawn_model ${PX4_SIM_MODEL} $n
+		n=$(($n + 1))
+	done
+else
+	IFS=,
+	for target in ${SCRIPT}; do
+		target="$(echo "$target" | tr -d ' ')" #Remove spaces
+		target_vehicle="${target%:*}"
+		target_number="${target#*:}"
+
+		if [ $n -gt 255 ]
+		then
+			echo "Tried spawning $n vehicles. The maximum number of supported vehicles is 255"
+			exit 1
+		fi
+
+		m=0
+		while [ $m -lt ${target_number} ]; do
+			spawn_model ${target_vehicle} $n
+			m=$(($m + 1))
+			n=$(($n + 1))
+		done
+	done
 
+fi
 trap "cleanup" SIGINT SIGTERM EXIT
 
 echo "Starting gazebo client"