From de78515909787b491c99a9639c5d4663eab00c9f Mon Sep 17 00:00:00 2001
From: Arturo Torres <aurican12@gmail.com>
Date: Wed, 29 Aug 2018 16:56:30 +0200
Subject: [PATCH] New mode in run_mavros.py. Fix #42

---
 px4_bringup/launch/spawn_robot.launch           | 4 +++-
 px4_bringup/scripts/run_mavros.py               | 9 +++++++++
 uav_abstraction_layer/launch/test_server.launch | 1 +
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/px4_bringup/launch/spawn_robot.launch b/px4_bringup/launch/spawn_robot.launch
index 58c4f03..e07f4ec 100644
--- a/px4_bringup/launch/spawn_robot.launch
+++ b/px4_bringup/launch/spawn_robot.launch
@@ -17,6 +17,8 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
     <arg name="material" default="Orange"/>
     <arg name="target_ip" default="localhost"/>
     <arg name="own_ip" default="localhost"/>
+    <arg name="fcu_url" default="udp://:14550@localhost:14556"/>
+    <arg name="gcs_url" default="udp://@localhost"/>
     <arg name="x" default="0"/>
     <arg name="y" default="0"/>
     <arg name="z" default="0"/>
@@ -35,6 +37,6 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
     
     <!-- Run mavros node -->
     <node pkg="px4_bringup" type="run_mavros.py" name="run_mavros_$(arg id)" output="screen"
-    args="-id=$(arg id) -mode=$(arg mode) -target_ip=$(arg target_ip) -own_ip=$(arg own_ip)" />
+    args="-id=$(arg id) -mode=$(arg mode) -target_ip=$(arg target_ip) -own_ip=$(arg own_ip) -fcu_url=$(arg fcu_url) -gcs_url=$(arg gcs_url)" />
 
 </launch>
diff --git a/px4_bringup/scripts/run_mavros.py b/px4_bringup/scripts/run_mavros.py
index dc15ecd..0cc1341 100755
--- a/px4_bringup/scripts/run_mavros.py
+++ b/px4_bringup/scripts/run_mavros.py
@@ -18,6 +18,10 @@ def main():
                         help='IP address of the device running px4, used to set proper fcu_url')
     parser.add_argument('-own_ip', type=str, default="localhost",
                         help='IP address of this device, used to set proper fcu_url')
+    parser.add_argument('-fcu_url', type=str, default="udp://:14550@localhost:14556",
+                        help='set fcu_url manually in custom mode')
+    parser.add_argument('-gcs_url', type=str, default="udp://@localhost",
+                        help='set gcs_url manually in custom mode')
     args, unknown = parser.parse_known_args()
     utils.check_unknown_args(unknown)
 
@@ -54,6 +58,11 @@ def main():
         fcu_url = "udp://:14550@{}:14556".format(args.target_ip)
         subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True)
         subprocess.call("rosparam set " + node_name + "/gcs_url " + "udp://@{}".format(args.own_ip), shell=True)
+    elif args.mode =="custom":
+        fcu_url = "{}".format(args.fcu_url)
+        gcs_url = "{}".format(args.gcs_url)
+        subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True)
+        subprocess.call("rosparam set " + node_name + "/gcs_url " + gcs_url, shell=True)
 
     # ...and load blacklist, config (as seen in mavros node.launch)
     yaml_path = rospack.get_path("px4_bringup") + "/config/"
diff --git a/uav_abstraction_layer/launch/test_server.launch b/uav_abstraction_layer/launch/test_server.launch
index 9561ee7..4d835f8 100644
--- a/uav_abstraction_layer/launch/test_server.launch
+++ b/uav_abstraction_layer/launch/test_server.launch
@@ -30,6 +30,7 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI
         - sitl: for software in the loop simulation
         - serial: for serial connection to px4
         - udp: for udp connection to px4
+        - custom: to fully customize fcu and gcs urls
     -->
     <!-- Launch robot id=1 -->
     <include file="$(find px4_bringup)/launch/spawn_robot.launch" ns="$(arg ns_prefix)1">