From f1a064093df212bc4bfec521becad1d7778b3548 Mon Sep 17 00:00:00 2001
From: stibipet <stibinger@fly4future.com>
Date: Thu, 4 Jul 2024 21:24:26 +0200
Subject: [PATCH] add --mavlink_gcs_udp_port param to the drone spawner

---
 .../init.d-posix/px4-rc.mavlink_gcs              | 16 ++++++++++++++++
 .../ROMFS/px4fmu_common/init.d-posix/rcS         |  5 +++++
 .../launch/run_simulation_firmware.launch        |  6 ++++--
 .../mrs_drone_spawner/mrs_drone_spawner.py       | 14 +++++++++++++-
 4 files changed, 38 insertions(+), 3 deletions(-)
 create mode 100644 ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs

diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs
new file mode 100644
index 0000000..2ca2b25
--- /dev/null
+++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink_gcs
@@ -0,0 +1,16 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+# GCS link
+mavlink start -x -u $MAVLINK_GCS_UDP_PORT -r 4000000 -f
+mavlink stream -r 100 -s ATTITUDE -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s ATTITUDE_QUATERNION -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data
+mavlink stream -r 100 -s ATTITUDE_TARGET -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s HIGHRES_IMU -u $MAVLINK_GCS_UDP_PORT # /mavros/imu/data_raw
+mavlink stream -r 100 -s LOCAL_POSITION_NED -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s ODOMETRY -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s GLOBAL_POSITION_INT -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 10 -s RC_CHANNELS -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 10 -s SYS_STATUS -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s HEARTBEAT -u $MAVLINK_GCS_UDP_PORT
+mavlink stream -r 100 -s DISTANCE_SENSOR -u $MAVLINK_GCS_UDP_PORT
diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
index 55f1c75..2e819d3 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -271,6 +271,11 @@ fi
 #user defined mavlink streams for instances can be in PATH
 . px4-rc.mavlink
 
+if [ -n "$MAVLINK_GCS_UDP_PORT" ]
+then
+  . px4-rc.mavlink_gcs
+fi
+
 # execute autostart post script if any
 [ -e "$autostart_file".post ] && . "$autostart_file".post
 
diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
index 9661dd6..45b00ff 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
+++ b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch
@@ -4,8 +4,9 @@
   <arg name="ID"/>
   <arg name="PX4_SIM_MODEL"/>
   <arg name="PX4_ESTIMATOR" default="ekf2"/>
+  <arg name="MAVLINK_GCS_UDP_PORT" default=""/> <!-- mavlink port for QGC -->
   <arg name="ROMFS_PATH" default="$(find mrs_uav_gazebo_simulation)/ROMFS"/>
-  
+
   <!-- PX4 configs -->
   <arg name="interactive" default="false"/>
   <!-- PX4 SITL -->
@@ -18,9 +19,10 @@
     <!-- PX4 params -->
     <env name="PX4_SIM_MODEL" value="$(arg PX4_SIM_MODEL)" />
     <env name="PX4_ESTIMATOR" value="$(arg PX4_ESTIMATOR)" />
+    <env if="$(eval MAVLINK_GCS_UDP_PORT != '')" name="MAVLINK_GCS_UDP_PORT" value="$(arg MAVLINK_GCS_UDP_PORT)" />
 
     <!-- PX4 SITL -->
     <node name="sitl_uav$(arg ID)" pkg="px4" type="px4" output="screen" args="$(arg ROMFS_PATH)/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_interactive_mode) -i $(arg ID) -w sitl_uav$(arg ID)"/>
-  
+
   </group>
 </launch>
diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
index 0fd409b..68dae53 100755
--- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
+++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py
@@ -471,7 +471,7 @@ def parse_user_input(self, input_str):
             'model': None,
             'ids': [],
             'names': [],
-            'spawn_poses': {},
+            'spawn_poses': {}
         }
 
         # parse out the keywords starting with '--'
@@ -999,6 +999,10 @@ def get_jinja_params_for_one_robot(self, params_dict, index, ID):
 
         robot_params['mavlink_config'] = self.get_mavlink_config_for_robot(ID)
 
+        if 'mavlink_gcs_udp_port' in params_dict.keys():
+            if len(params_dict['mavlink_gcs_udp_port']) > index:
+                robot_params['mavlink_gcs_udp_port'] = params_dict['mavlink_gcs_udp_port'][index]
+
         return robot_params
     # #}
 
@@ -1051,6 +1055,14 @@ def launch_px4_firmware(self, robot_params):
             'PX4_SIM_MODEL:=' + str(robot_params['model']),
             'ROMFS_PATH:=' + str(romfs_path)
         ]
+
+        # do we want to send raw mavlink data to a specific port? (this will also auto connect to QGroundControl)
+        if 'mavlink_gcs_udp_port' in robot_params.keys():
+            if isinstance(robot_params['mavlink_gcs_udp_port'], int):
+                roslaunch_args.append('MAVLINK_GCS_UDP_PORT:=' + str(robot_params["mavlink_gcs_udp_port"]))
+            else:
+                raise CouldNotLaunch(f'\'{robot_params["mavlink_gcs_udp_port"]}\' is not a valid port number')
+
         roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)]
 
         firmware_process = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence)