[MrsDroneSpawner]: fix and rework mavlink GCS connection

ctu-mrs · Jul 25, 2024 · 147607d · 147607d
1 parent 29ba363
commit 147607d
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 23 deletions.
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
@@ -2,15 +2,15 @@
 # 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
+mavlink start -x -u $MAVLINK_GCS_UDP_PORT_LOCAL -r 4000000 -o $MAVLINK_GCS_UDP_PORT_REMOTE
+mavlink stream -r 100 -s ATTITUDE -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s ATTITUDE_QUATERNION -u $MAVLINK_GCS_UDP_PORT_LOCAL # /mavros/imu/data
+mavlink stream -r 100 -s ATTITUDE_TARGET -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s HIGHRES_IMU -u $MAVLINK_GCS_UDP_PORT_LOCAL # /mavros/imu/data_raw
+mavlink stream -r 100 -s LOCAL_POSITION_NED -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s ODOMETRY -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s GLOBAL_POSITION_INT -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 10 -s RC_CHANNELS -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 10 -s SYS_STATUS -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s HEARTBEAT -u $MAVLINK_GCS_UDP_PORT_LOCAL
+mavlink stream -r 100 -s DISTANCE_SENSOR -u $MAVLINK_GCS_UDP_PORT_LOCAL
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
@@ -271,7 +271,7 @@ fi
 #user defined mavlink streams for instances can be in PATH
 . px4-rc.mavlink
 
-if [ -n "$MAVLINK_GCS_UDP_PORT" ]
+if [ -n "$MAVLINK_GCS_UDP_PORT_LOCAL" ] && [ -n "$MAVLINK_GCS_UDP_PORT_REMOTE" ]
 then
   . px4-rc.mavlink_gcs
 fi

diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -2,6 +2,8 @@ mavlink_config:
   vehicle_base_port: 14000
   mavlink_tcp_base_port: 4560
   mavlink_udp_base_port: 14560
+  mavlink_gcs_udp_base_port_local: 18000
+  mavlink_gcs_udp_base_port_remote: 18100
   qgc_udp_port: 14550
   sdk_udp_port: 14540
   send_vision_estimation: false

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
@@ -4,7 +4,8 @@
   <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="MAVLINK_GCS_UDP_PORT_LOCAL" default=""/> <!-- mavlink port for QGC -->
+  <arg name="MAVLINK_GCS_UDP_PORT_REMOTE" default=""/> <!-- mavlink port for QGC -->
   <arg name="ROMFS_PATH" default="$(find mrs_uav_gazebo_simulation)/ROMFS"/>
 
   <!-- PX4 configs -->
@@ -19,7 +20,8 @@
     <!-- 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)" />
+    <env if="$(eval MAVLINK_GCS_UDP_PORT_LOCAL != '')" name="MAVLINK_GCS_UDP_PORT_LOCAL" value="$(arg MAVLINK_GCS_UDP_PORT_LOCAL)" />
+    <env if="$(eval MAVLINK_GCS_UDP_PORT_REMOTE != '')" name="MAVLINK_GCS_UDP_PORT_REMOTE" value="$(arg MAVLINK_GCS_UDP_PORT_REMOTE)" />
 
     <!-- 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)"/>

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
@@ -142,6 +142,8 @@ def __init__(self, verbose=False):
             self.vehicle_base_port = rospy.get_param('~mavlink_config/vehicle_base_port')
             self.mavlink_tcp_base_port = rospy.get_param('~mavlink_config/mavlink_tcp_base_port')
             self.mavlink_udp_base_port = rospy.get_param('~mavlink_config/mavlink_udp_base_port')
+            self.mavlink_gcs_udp_base_port_local = rospy.get_param('~mavlink_config/mavlink_gcs_udp_base_port_local')
+            self.mavlink_gcs_udp_base_port_remote = rospy.get_param('~mavlink_config/mavlink_gcs_udp_base_port_remote')
             self.qgc_udp_port = rospy.get_param('~mavlink_config/qgc_udp_port')
             self.sdk_udp_port = rospy.get_param('~mavlink_config/sdk_udp_port')
             self.send_vision_estimation = rospy.get_param('~mavlink_config/send_vision_estimation')
@@ -999,9 +1001,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]
+        if 'enable_mavlink_gcs' in params_dict.keys():
+            robot_params['mavlink_gcs_udp_port_local'] = self.mavlink_gcs_udp_base_port_local + ID
+            robot_params['mavlink_gcs_udp_port_remote'] = self.mavlink_gcs_udp_base_port_remote + ID
+            rospy.loginfo(f'[MrsDroneSpawner]: Publishing extra mavlink messages on UDP port {robot_params["mavlink_gcs_udp_port_remote"]}')
 
         return robot_params
     # #}
@@ -1057,11 +1060,9 @@ def launch_px4_firmware(self, robot_params):
         ]
 
         # 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')
+        if 'mavlink_gcs_udp_port_local' in robot_params.keys() and 'mavlink_gcs_udp_port_remote' in robot_params.keys():
+            roslaunch_args.append('MAVLINK_GCS_UDP_PORT_LOCAL:=' + str(robot_params['mavlink_gcs_udp_port_local']))
+            roslaunch_args.append('MAVLINK_GCS_UDP_PORT_REMOTE:=' + str(robot_params['mavlink_gcs_udp_port_remote']))
 
         roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)]