From 9707ac531609382b6b647feec45d60b9f4168e29 Mon Sep 17 00:00:00 2001
From: SungTae Moon <munhoney@gmail.com>
Date: Sat, 8 Aug 2020 20:19:19 +0900
Subject: [PATCH] implement gazebo simulation based on RTPS for multiple UAVs
 (#15463)

- add t option for target
   - add iris_rtps model (PX4/sitl_gazebo#558)
---
 .../init.d-posix/1016_iris_rtps.post          |  4 +++-
 Tools/gazebo_sitl_multiple_run.sh             | 21 ++++++++++++-------
 Tools/sitl_gazebo                             |  2 +-
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/1016_iris_rtps.post b/ROMFS/px4fmu_common/init.d-posix/1016_iris_rtps.post
index 1b3e86297f..d3cfa4f817 100644
--- a/ROMFS/px4fmu_common/init.d-posix/1016_iris_rtps.post
+++ b/ROMFS/px4fmu_common/init.d-posix/1016_iris_rtps.post
@@ -1,2 +1,4 @@
+#!/bin/sh
 # shellcheck disable=SC2154
-micrortps_client start -t UDP
+
+micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))
diff --git a/Tools/gazebo_sitl_multiple_run.sh b/Tools/gazebo_sitl_multiple_run.sh
index f709bc5244..150da5e446 100755
--- a/Tools/gazebo_sitl_multiple_run.sh
+++ b/Tools/gazebo_sitl_multiple_run.sh
@@ -9,15 +9,19 @@
 function cleanup() {
 	pkill -x px4
 	pkill gzclient
+	pkill gzserver
 }
 
 function spawn_model() {
 	MODEL=$1
 	N=$2 #Instance Number
 
-	if [ "$MODEL" != "iris" ] && [ "$MODEL" != "plane" ] && [ "$MODEL" != "standard_vtol" ]
+	SUPPORTED_MODELS=("iris" "iris_rtps" "plane" "standard_vtol")
+	if [[ " ${SUPPORTED_MODELS[*]} " != *"$MODEL "* ]];
 	then
-		echo "Currently only the following vehicle models are supported! [iris, plane, standard_vtol]"
+		echo "ERROR: Currently only vehicle model $MODEL is not supported!"
+		echo "       Supported Models: [${SUPPORTED_MODELS[@]}]"
+		trap "cleanup" SIGINT SIGTERM EXIT
 		exit 1
 	fi
 
@@ -47,7 +51,7 @@ then
 	exit 1
 fi
 
-while getopts n:m:w:s: option
+while getopts n:m:w:s:t: option
 do
 	case "${option}"
 	in
@@ -55,19 +59,20 @@ do
 		m) VEHICLE_MODEL=${OPTARG};;
 		w) WORLD=${OPTARG};;
 		s) SCRIPT=${OPTARG};;
+		t) TARGET=${OPTARG};;
 	esac
 done
 
 num_vehicles=${NUM_VEHICLES:=3}
-export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris}
 world=${WORLD:=empty}
+target=${TARGET:=px4_sitl_default}
+export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris}
+
 echo ${SCRIPT}
-
-
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 src_path="$SCRIPT_DIR/.."
 
-build_path=${src_path}/build/px4_sitl_default
+build_path=${src_path}/build/${target}
 mavlink_udp_port=14560
 mavlink_tcp_port=4560
 
@@ -76,7 +81,7 @@ pkill -x px4 || true
 
 sleep 1
 
-source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/px4_sitl_default
+source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
 
 echo "Starting gazebo"
 gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose &
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 037ce1b3b8..0da08f0333 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 037ce1b3b88b1c25abc1f525fbbbec84589d090d
+Subproject commit 0da08f0333cd14c08a9545c6ba7ddfc18706398f