Use mavlink-router in the ground image to merge and split MAVLink telemetry

README.md

@@ -58,14 +58,14 @@ flowchart TB
         end
 
         subgraph gnd ["#nbsp;ground#nbsp;container#nbsp;(amd64)"]
-            mavproxy{{mavproxy}}:::bridge
+            mlrouter{{mavlink-router}}:::bridge
             ground_system[/ground_system\]:::algo
             qgc(QGroundControl):::resource
             zenoh_gnd{{zenoh-bridge}}:::bridge
 
             ground_system --> |"/tracks"| zenoh_gnd
-            mavproxy <--> qgc
-            mavproxy --> ground_system
+            mlrouter <--> qgc
+            mlrouter --> ground_system
         end
 
         subgraph air ["[N#nbsp;x]#nbsp;aircraft#nbsp;container(s)#nbsp;(amd64,#nbsp;arm64)"]
@@ -100,7 +100,7 @@ flowchart TB
     gz --> |"gz_gst_bridge <br/> [SIM_SUBNET]"| yolo_py
     gz --> |"/lidar_points <br/> [SIM_SUBNET]"| kiss_icp
     sitl <--> |"UDP <br/> [SIM_SUBNET]"| ap_link
-    sitl <--> |"MAVLink <br/> [SIM_SUBNET]"| mavproxy 
+    sitl <--> |"MAVLink <br/> [SIM_SUBNET]"| mlrouter 
     zenoh_gnd <-.-> |"TCP <br/> [AIR_SUBNET]"| zenoh_air
 
     classDef bridge fill:#ffebd6,stroke:#f5a623,stroke-width:2px;
@@ -483,6 +483,7 @@ Distributed under the MIT License. See `LICENSE.txt` for more information. Copyr
 
 - ArduPilot SITL for Iris uses option -f that also sets "external": True, this is not the case for the Alti Transition from ArduPilot/SITL_Models
 - QGC is started with a virtual joystick (with low throttle if using only VTOLs and centered throttle if there are quads), this is reflective of real-life but note that this counts as "RC loss" when switching focus from one autopilot instance to another
+- QGC will only connect to the first 10 ArduPilot vehicles when GND_CONTAINER=false because of settings in QGroundControl.ini
 - On non-configured real-life AP, missing topics: ros2 topic echo /mavros/local_position/odom ros2 topic echo /mavros/home_position/home
 - Gazebo WindEffects plugin is disabled/not working for PX4 standard_vtol
 - Command 178 MAV_CMD_DO_CHANGE_SPEED is accepted but not effective in changing speed for ArduPilot VTOL

ground/ground.yml.erb

@@ -15,7 +15,27 @@ windows:
       layout: even-horizontal
       panes:
         - clear && echo "So nice to have you with us, this is the ground container."
-        - ros2 run ground_system ground_system --ros-args -p num_drones:=<%= num_quads + num_vtols %> -p base_port:=14540 -p rate:=1.0 <%= simulated_time ? ' -p use_sim_time:=True' : '' %>
+        - ros2 run ground_system ground_system --ros-args -p num_drones:=<%= num_quads + num_vtols %> -p base_port:=18540 -p rate:=1.0 <%= simulated_time ? ' -p use_sim_time:=True' : '' %>
+
+  - mavlink-router:
+      layout: tiled
+      panes:
+        - >
+          cp /aas/ground_resources/comms/mavlink_router_base.conf /aas/ground_resources/comms/mavlink_router_main.conf &&
+          <% if simulated_time %>
+          <% (1..(num_quads + num_vtols)).each do |drone_id| %>
+          <% sitl_instance = drone_id - 1 %>
+          echo "[UdpEndpoint drone<%= drone_id %>]" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          echo "Mode = Eavesdropping" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          echo "Address = 0.0.0.0" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          echo "Port = <%= 16540 + sitl_instance %>" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          <% end %>
+          <% else %>
+          echo "[UartEndpoint hardware]" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          echo "Device = /dev/ttyUSB0" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          echo "Baud = 57600" >> /aas/ground_resources/comms/mavlink_router_main.conf &&
+          <% end %>
+          mavlink-routerd -c /aas/ground_resources/comms/mavlink_router_main.conf
 
   - zenoh_and_qgc:
       layout: even-horizontal

ground/ground_resources/comms/mavlink_router_base.conf

@@ -0,0 +1,11 @@
+[General]
+
+[UdpEndpoint qgc]
+Mode = Normal
+Address = 127.0.0.1
+Port = 14550
+
+[UdpEndpoint ros2]
+Mode = Normal
+Address = 127.0.0.1
+Port = 18540

ground/ground_ws/src/ground_system/src/ground_system.cpp

@@ -5,7 +5,7 @@ GroundSystem::GroundSystem() : Node("ground_system"), keep_running_(true)
     // Declare Parameters
     this->declare_parameter("num_drones", 1);
     this->declare_parameter("ip", "0.0.0.0");
-    this->declare_parameter("base_port", 14540);
+    this->declare_parameter("base_port", 18540);
     this->declare_parameter("rate", 10.0);
 
     // Get Parameters
@@ -24,13 +24,11 @@ GroundSystem::GroundSystem() : Node("ground_system"), keep_running_(true)
     auto timer_period = std::chrono::duration<double>(1.0 / publish_rate_);
     timer_ = this->create_wall_timer(timer_period, std::bind(&GroundSystem::publish_swarm_obs, this));
 
-    // Start Listener Threads
-    for (int i = 0; i < num_drones_; ++i) {
-        int drone_id = i + 1;
-        int port = base_port_ + i;
-        listener_threads_.emplace_back(&GroundSystem::mavlink_listener, this, drone_id, port);
-        RCLCPP_INFO(this->get_logger(), "Listening for drone %d on port %d", drone_id, port);
-    }
+    // Single listener thread, use base_port_ and pass drone_id = -1 to signal "auto-detect ID from message"
+    listener_threads_.emplace_back(&GroundSystem::mavlink_listener, this, -1, base_port_);
+    RCLCPP_INFO(this->get_logger(), "Listening to the streams from %d drones on single port %d", num_drones_, base_port_);
+    // To listen to separate UDP streams on separate ports, create multiple threads using:
+    // listener_threads_.emplace_back(&GroundSystem::mavlink_listener, this, drone_id, port);
 }
 
 GroundSystem::~GroundSystem()
@@ -82,6 +80,15 @@ void GroundSystem::mavlink_listener(int drone_id, int port)
             // Parse bytes
             for (ssize_t i = 0; i < len; ++i) {
                 if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {
+
+                    // In single-port/single-thread mode (drone_id == -1), detect ID from the message
+                    int current_id = drone_id;
+                    if (current_id == -1) {
+                        current_id = msg.sysid;
+                        if (current_id < 1 || current_id > num_drones_) {
+                            continue; // Ignore out-of-bounds IDs
+                        }
+                    }
 
                     if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { // Handle GLOBAL_POSITION_INT message
                         mavlink_global_position_int_t pos;
@@ -95,7 +102,7 @@ void GroundSystem::mavlink_listener(int drone_id, int port)
                         obs.vz = pos.vz / 100.0;
                         {
                             std::lock_guard<std::mutex> lock(data_mutex_);
-                            drone_obs_[drone_id] = obs;
+                            drone_obs_[current_id] = obs;
                         }
                     }
                 }

scripts/docker/Dockerfile.ground

@@ -88,16 +88,29 @@ RUN apt update \
     && apt clean \
     && rm -rf /var/lib/apt/lists/*
 
-# Add pymavlink to emulate the ground system using telemetry data
+# Add pymavlink and mavproxy to quickly inspect MAVLink streams
 RUN pip3 install --no-cache-dir --upgrade pip \
-    && pip3 install --no-cache-dir --resume-retries 5 pymavlink pyserial
+    && pip3 install --no-cache-dir --resume-retries 5 pymavlink pyserial mavproxy future
 # Check with $ python3 -c "import pymavlink; print(pymavlink.__version__)"
 
 ################################################################################
 # Stage 4 - 8+GB ###############################################################
 ################################################################################
 FROM ros2-qgc-zenoh-gst-pymavlink-image AS ground-dev-image
 
+# Install mavlink-router
+RUN apt update \
+    && apt install -y --no-install-recommends \
+        git meson ninja-build pkg-config gcc g++ systemd \
+    && apt clean \
+    && rm -rf /var/lib/apt/lists/*
+COPY /github_clones/mavlink-router /aas/github_apps/mavlink-router
+WORKDIR /aas/github_apps/mavlink-router
+RUN meson setup build . --buildtype=release \
+    && ninja -C build \
+    && ninja -C build install
+# Check with $ mavlink-routerd --version
+
 # Install wmctrl and xrandr to resize QGC window
 RUN apt update \
     && apt install -y --no-install-recommends \

scripts/sim_build.sh

@@ -29,6 +29,7 @@ REPOS=( # Format: "URL;BRANCH;LOCAL_DIR_NAME"
   "https://github.com/PX4/flight_review.git;main;flight_review"
   # Ground image
   "https://github.com/mavlink/c_library_v2;master;c_library_v2"
+  "https://github.com/mavlink-router/mavlink-router;master;mavlink-router"
   # Aircraft image
   "https://github.com/PX4/px4_msgs.git;release/1.16;px4_msgs"
   "https://github.com/eProsima/Micro-XRCE-DDS-Agent.git;master;Micro-XRCE-DDS-Agent"

simulation/simulation.yml.erb

@@ -105,7 +105,7 @@ windows:
           -l $(python3 /aas/simulation_resources/patches/extract_spherical_coords.py /aas/simulation_resources/simulation_worlds/<%= world %>.sdf)
           --out=udp:<%= "#{sim_subnet}.90.#{drone_id}" %>:8888
           <% if gnd_container %>
-          --out=udp:<%= "#{sim_subnet}.90.#{ground_id}" %>:<%= 14550 + sitl_instance %> --out=udp:<%= "#{sim_subnet}.90.#{ground_id}" %>:<%= 14540 + sitl_instance %>
+          --out=udp:<%= "#{sim_subnet}.90.#{ground_id}" %>:<%= 16540 + sitl_instance %>
           <% else %>
           --out=udp:127.0.0.1:<%= 14550 + sitl_instance %>
           <% end %>

simulation/simulation_resources/patches/px4-v1.16.0.patch

@@ -1,25 +1,26 @@
 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
-index 17ef44521b..da88a4e9f9 100644
+index 17ef44521b..ccf6907a1b 100644
 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
 +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
-@@ -11,7 +11,7 @@ udp_onboard_gimbal_port_remote=$((13280+px4_instance))
- udp_gcs_port_local=$((18570+px4_instance))
-
- # GCS link
--mavlink start -x -u $udp_gcs_port_local -r 4000000 -f
-+mavlink start -x -u $udp_gcs_port_local -r 4000000 -f -t "${AAS_GROUND_IP:-127.0.0.1}"
- mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
- mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
- mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
-@@ -23,7 +23,7 @@ mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
- mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
-
+@@ -4,6 +4,8 @@
+ udp_offboard_port_local=$((14580+px4_instance))
+ udp_offboard_port_remote=$((14540+px4_instance))
+ [ "$px4_instance" -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
++udp_aas_port_local=$((16580+px4_instance))
++udp_aas_port_remote=$((16540+px4_instance))
+ udp_onboard_payload_port_local=$((14280+px4_instance))
+ udp_onboard_payload_port_remote=$((14030+px4_instance))
+ udp_onboard_gimbal_port_local=$((13030+px4_instance))
+@@ -25,6 +27,9 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
  # API/Offboard link
--mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
-+mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -t "${AAS_GROUND_IP:-127.0.0.1}" -o $udp_offboard_port_remote
+ mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
 
++# AAS link
++mavlink start -x -u $udp_aas_port_local -r 4000000 -f -t "${AAS_GROUND_IP:-127.0.0.1}" -o $udp_aas_port_remote
++
  # Onboard link to camera
  mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
+
 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
 index ddb9703828..59be1ee21e 100644
 --- a/ROMFS/px4fmu_common/init.d-posix/rcS