From 7f016b5fd4ec93d428d0c2f434c02a7665449b21 Mon Sep 17 00:00:00 2001
From: Kunal Shah <kshah.kunal@gmail.com>
Date: Mon, 10 Sep 2018 13:05:02 -0700
Subject: [PATCH] ROS multi-vehicle simulation: fixed udp port overlap issues

---
 ROMFS/px4fmu_common/init.d-posix/rcS |  6 ++---
 launch/multi_uav_mavros_sitl.launch  | 39 +++++++++++++++++++++++++---
 2 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 9c0923a7e4..62c05d7a8f 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -81,11 +81,11 @@ fi
 
 # multi-instance setup
 # shellcheck disable=SC2154
-param set MAV_SYS_ID $((1+px4_instance))
+param set MAV_SYS_ID $((px4_instance+1))
 simulator_udp_port=$((14560+px4_instance))
-udp_offboard_port_local=$((14557+px4_instance))
+udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
-udp_gcs_port_local=$((14556+px4_instance))
+udp_gcs_port_local=$((14570+px4_instance))
 
 if [ $AUTOCNF = yes ]
 then
diff --git a/launch/multi_uav_mavros_sitl.launch b/launch/multi_uav_mavros_sitl.launch
index f973dd2962..1baa7c233d 100644
--- a/launch/multi_uav_mavros_sitl.launch
+++ b/launch/multi_uav_mavros_sitl.launch
@@ -23,7 +23,7 @@
     <group ns="uav1">
         <!-- MAVROS and vehicle configs -->        
         <arg name="ID" value="1"/>
-        <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
+        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
         <!-- PX4 SITL and vehicle spawn -->
         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
             <arg name="x" value="0"/>
@@ -40,7 +40,7 @@
         <include file="$(find mavros)/launch/px4.launch">
             <arg name="fcu_url" value="$(arg fcu_url)"/>
             <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(arg ID)"/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
             <arg name="tgt_component" value="1"/>
         </include>
     </group>
@@ -48,7 +48,7 @@
     <group ns="uav2">
         <!-- MAVROS and vehicle configs -->
         <arg name="ID" value="2"/>
-        <arg name="fcu_url" default="udp://:14541@localhost:14558"/>
+        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>
         <!-- PX4 SITL and vehicle spawn -->
         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
             <arg name="x" value="1"/>
@@ -65,8 +65,39 @@
         <include file="$(find mavros)/launch/px4.launch">
             <arg name="fcu_url" value="$(arg fcu_url)"/>
             <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(arg ID)"/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+            <arg name="tgt_component" value="1"/>
+        </include>
+    </group>
+
+    <!-- UAV3 -->
+    <group ns="uav3">
+        <!-- MAVROS and vehicle configs -->
+        <arg name="ID" value="3"/>
+        <arg name="fcu_url" default="udp://:14543@localhost:14583"/>
+        <!-- PX4 SITL and vehicle spawn -->
+        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
+            <arg name="x" value="0"/>
+            <arg name="y" value="1"/>
+            <arg name="z" value="0"/>
+            <arg name="R" value="0"/>
+            <arg name="P" value="0"/>
+            <arg name="Y" value="0"/>
+            <arg name="vehicle" value="$(arg vehicle)"/>
+            <arg name="mavlink_udp_port" value="14563"/>
+            <arg name="ID" value="$(arg ID)"/>
+        </include>
+        <!-- MAVROS -->
+        <include file="$(find mavros)/launch/px4.launch">
+            <arg name="fcu_url" value="$(arg fcu_url)"/>
+            <arg name="gcs_url" value=""/>
+            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
             <arg name="tgt_component" value="1"/>
         </include>
     </group>
 </launch>
+<!-- to add more UAVs (up to 10):
+Increase the id
+Change the name space
+Set the FCU to default="udp://:14540+id@localhost:14550+id"
+Set the malink_udp_port to 14560+id) -->