From 80dd841cad529193705c6c4eef0915153dbb80fc Mon Sep 17 00:00:00 2001
From: Mohamed Abdelkader Zahana <mohamedashraf123@gmail.com>
Date: Fri, 8 Jan 2021 21:29:47 +0300
Subject: [PATCH] Update launch files to enable multi-video stream in SITL
 (#16497)

* Update launch files to support multi-video stream in SITL

* Pass extra args to jinja_gen.py inside gazebo_sitl_multiple_run.sh to enable multi-video streaming
---
 Tools/gazebo_sitl_multiple_run.sh   | 2 +-
 launch/multi_uav_mavros_sitl.launch | 9 +++++++++
 launch/single_vehicle_spawn.launch  | 5 ++++-
 3 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/Tools/gazebo_sitl_multiple_run.sh b/Tools/gazebo_sitl_multiple_run.sh
index 38b6e5a5bb..14a189fe5c 100755
--- a/Tools/gazebo_sitl_multiple_run.sh
+++ b/Tools/gazebo_sitl_multiple_run.sh
@@ -31,7 +31,7 @@ function spawn_model() {
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
 	../bin/px4 -i $N -d "$build_path/etc" -w sitl_${MODEL}_${N} -s etc/init.d-posix/rcS >out.log 2>err.log &
-	python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+	python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
 
 	echo "Spawning ${MODEL}_${N}"
 
diff --git a/launch/multi_uav_mavros_sitl.launch b/launch/multi_uav_mavros_sitl.launch
index 7cb06880a8..c699d72ea6 100644
--- a/launch/multi_uav_mavros_sitl.launch
+++ b/launch/multi_uav_mavros_sitl.launch
@@ -36,6 +36,9 @@
             <arg name="mavlink_udp_port" value="14560"/>
             <arg name="mavlink_tcp_port" value="4560"/>
             <arg name="ID" value="$(arg ID)"/>
+            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
         </include>
         <!-- MAVROS -->
         <include file="$(find mavros)/launch/px4.launch">
@@ -62,6 +65,9 @@
             <arg name="mavlink_udp_port" value="14561"/>
             <arg name="mavlink_tcp_port" value="4561"/>
             <arg name="ID" value="$(arg ID)"/>
+            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
         </include>
         <!-- MAVROS -->
         <include file="$(find mavros)/launch/px4.launch">
@@ -88,6 +94,9 @@
             <arg name="mavlink_udp_port" value="14562"/>
             <arg name="mavlink_tcp_port" value="4562"/>
             <arg name="ID" value="$(arg ID)"/>
+            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
+            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
         </include>
         <!-- MAVROS -->
         <include file="$(find mavros)/launch/px4.launch">
diff --git a/launch/single_vehicle_spawn.launch b/launch/single_vehicle_spawn.launch
index ad9d9b5436..b62b89cd8a 100644
--- a/launch/single_vehicle_spawn.launch
+++ b/launch/single_vehicle_spawn.launch
@@ -17,10 +17,13 @@
     <env name="PX4_ESTIMATOR" value="$(arg est)" />
     <arg name="mavlink_udp_port" default="14560"/>
     <arg name="mavlink_tcp_port" default="4560"/>
+    <arg name="gst_udp_port" default="5600"/>
+    <arg name="video_uri" default="5600"/>
+    <arg name="mavlink_cam_udp_port" default="14530"/>
     <!-- PX4 configs -->
     <arg name="interactive" default="true"/>
     <!-- generate sdf vehicle model -->
-    <arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) $(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf.jinja $(find mavlink_sitl_gazebo)"/>
+    <arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(eval 1 + arg('ID')) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf.jinja $(find mavlink_sitl_gazebo)"/>
     <param command="$(arg cmd)" name="sdf_$(arg vehicle)$(arg ID)"/>
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>