gazebo multiple sim script: cleanup

- make sure to kill all px4 instances and gazebo client when exiting
- add arguments to specify number of vehicles and vehicle model
- exit for now if vehicle model is not iris, since needs changes in
sitl_gazebo submodule

Signed-off-by: RomanBapst <bapstroman@gmail.com>

Tools/gazebo_sitl_multiple_run.sh

@@ -4,10 +4,36 @@
 
 # The simulator is expected to send to TCP port 4560+i for i in [0, N-1]
 # For example gazebo can be run like this:
-#./Tools/gazebo_sitl_multiple_run.sh <number_of_vehicles>
+#./Tools/gazebo_sitl_multiple_run.sh -n 10 -m iris
 
-sitl_num=2
-[ -n "$1" ] && sitl_num="$1"
+function cleanup() {
+	pkill -x px4
+	pkill gzclient
+}
+
+if [ "$1" == "-h" ] || [ "$1" == "--help" ]
+then
+	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>]"
+	exit 1
+fi
+
+while getopts n:m: option
+do
+	case "${option}"
+	in
+		n) NUM_VEHICLES=${OPTARG};;
+		m) VEHICLE_MODEL=${OPTARG};;
+	esac
+done
+
+num_vehicles=${NUM_VEHICLES:=3}
+export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris}
+
+if [ "$PX4_SIM_MODEL" != "iris" ]
+then
+	echo "Currently only iris vehicle model is supported!"
+	exit 1
+fi
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 src_path="$SCRIPT_DIR/.."
@@ -24,22 +50,24 @@ sleep 1
 
 source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/px4_sitl_default
 
-export PX4_SIM_MODEL=iris
-
 echo "Starting gazebo"
 gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose &
 sleep 5
 
 n=0
-while [ $n -lt $sitl_num ]; do
+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_iris_${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/iris_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
+	../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
+
 	echo "Spawning ${PX4_SIM_MODEL}_${n}"
+
 	gz model --spawn-file=/tmp/${PX4_SIM_MODEL}_${n}.urdf --model-name=${PX4_SIM_MODEL}_${n} -x 0.0 -y ${n} -z 0.0
 
 	popd &>/dev/null
@@ -47,5 +75,7 @@ while [ $n -lt $sitl_num ]; do
 	n=$(($n + 1))
 done
 
+trap "cleanup" SIGINT SIGTERM EXIT
+
 echo "Starting gazebo client"
 gzclient