multi vehicle gz sim convenience script

Tools/simulation/README_gz_multi_vehicle.md

@@ -0,0 +1,156 @@
+# Multi-Vehicle Gazebo Simulation Script
+
+## Overview
+
+`gz_multi_vehicle.sh` is a script to easily launch multiple PX4 gz_x500 drones in Gazebo simulation with configurable positioning. Each drone instance runs in its own terminal window for easy monitoring.
+
+**Platform**: Ubuntu Linux only
+
+## Prerequisites
+
+1. PX4 SITL must be built:
+   ```bash
+   make px4_sitl
+   ```
+
+2. Gazebo (gz) must be installed (Harmonic, Ionic, or Jetty)
+
+3. A terminal emulator (terminator or gnome-terminal):
+   ```bash
+   # Recommended:
+   sudo apt install terminator
+
+   # Or use gnome-terminal:
+   sudo apt install gnome-terminal
+   ```
+
+## Usage
+
+```bash
+./Tools/simulation/gz_multi_vehicle.sh [num_vehicles] [spacing]
+```
+
+### Parameters
+
+- `num_vehicles` (optional, default: 2, max: 10): Number of drones to spawn
+- `spacing` (optional, default: 0.5): Distance between drones in meters along the Y-axis
+
+### Examples
+
+Launch 3 drones with default 0.5m spacing:
+```bash
+./Tools/simulation/gz_multi_vehicle.sh 3
+```
+
+Launch 5 drones with 1.0m spacing:
+```bash
+./Tools/simulation/gz_multi_vehicle.sh 5 1.0
+```
+
+Launch 2 drones (default configuration):
+```bash
+./Tools/simulation/gz_multi_vehicle.sh
+```
+
+## What the Script Does
+
+1. **Validates** that PX4 SITL is built and terminal emulator is available
+2. **Cleans up** any existing PX4 and Gazebo instances
+3. **Launches the first vehicle in a new terminal** (instance 0):
+   - Starts Gazebo server and GUI
+   - Spawns x500_0 at position (0, 0, 0)
+   - MAV_SYS_ID = 1
+   - Terminal title: "PX4 Vehicle 0 (MAV_SYS_ID=1)"
+4. **Launches subsequent vehicles in separate terminals** (instances 1, 2, ...):
+   - Connects to running Gazebo instance
+   - Spawns x500_N at position (0, N×spacing, 0)
+   - MAV_SYS_ID = N+1
+   - Terminal title: "PX4 Vehicle N (MAV_SYS_ID=N+1)"
+
+Each vehicle runs in its own terminal window, making it easy to monitor the output and debug individual drones.
+
+## Vehicle Configuration
+
+Each vehicle gets:
+- **Unique instance number**: 0, 1, 2, ...
+- **Unique MAV_SYS_ID**: instance + 1 (1, 2, 3, ...)
+- **Unique model name**: x500_0, x500_1, x500_2, ...
+- **Unique position**: Spaced along Y-axis
+- **Unique ROS 2 namespace** (if using ROS 2): px4_1, px4_2, px4_3, ...
+- **Dedicated terminal window**: Each instance displays its output in a separate terminal
+
+## Using with QGroundControl
+
+QGroundControl should automatically detect and connect to all vehicles. You can switch between them in the vehicle dropdown menu.
+
+## Using with ROS 2
+
+To use the vehicles with ROS 2:
+
+1. Start the MicroXRCE-DDS Agent (all vehicles connect to the same agent):
+   ```bash
+   MicroXRCEAgent udp4 -p 8888
+   ```
+
+2. List topics to see all vehicles:
+   ```bash
+   ros2 topic list
+   ```
+
+You should see topics namespaced by vehicle:
+- `/px4_1/fmu/in/...`
+- `/px4_1/fmu/out/...`
+- `/px4_2/fmu/in/...`
+- `/px4_2/fmu/out/...`
+- etc.
+
+## Stopping the Simulation
+
+To stop all vehicles, you can either:
+
+1. **Close the terminal windows** - Each terminal will prompt you to press Enter before closing
+2. **Kill all PX4 processes**:
+   ```bash
+   pkill -x px4
+   ```
+
+To stop Gazebo:
+```bash
+gz sim -k
+```
+
+## Troubleshooting
+
+### Vehicles not spawning
+- Check the terminal window for the specific vehicle - all output is displayed there
+- Ensure Gazebo is installed: `gz sim --versions`
+- Make sure the first vehicle (instance 0) has fully started Gazebo before others spawn
+
+### Gazebo GUI not appearing
+- Check if running in headless mode
+- Try setting display: `export DISPLAY=:0`
+
+### Terminal emulator not found
+- Install terminator (recommended): `sudo apt install terminator`
+- Or install gnome-terminal: `sudo apt install gnome-terminal`
+
+### Port conflicts
+- The script automatically handles MAVLink ports (14540 + instance)
+- UXRCE-DDS uses unique keys for namespacing
+
+## Monitoring Output
+
+Each vehicle displays its output in its own terminal window. The terminal title shows the vehicle instance and MAV_SYS_ID for easy identification. If a vehicle crashes or exits, the terminal will remain open and prompt you to press Enter before closing, allowing you to review any error messages.
+
+## Architecture Details
+
+The script implements the multi-vehicle pattern described in the PX4 docs:
+- First instance launches Gazebo server (no PX4_GZ_STANDALONE)
+- Subsequent instances connect to running server (PX4_GZ_STANDALONE=1)
+- Each instance uses PX4_GZ_MODEL_POSE for positioning
+- Automatic MAV_SYS_ID and UXRCE_DDS_KEY assignment based on instance number
+
+## References
+
+- [PX4 Multi-Vehicle Gazebo Docs](https://docs.px4.io/main/en/sim_gazebo_gz/multi_vehicle_simulation.html)
+- [PX4 ROS 2 Multi-Vehicle](https://docs.px4.io/main/en/ros2/multi_vehicle.html)

Tools/simulation/gz_multi_vehicle.sh

@@ -0,0 +1,226 @@
+#!/bin/bash
+# Multi-vehicle Gazebo (gz) simulation launcher for PX4
+# Spawns multiple gz_x500 drones with configurable spacing
+# Each instance runs in its own terminal window
+# Usage: ./gz_multi_vehicle.sh [num_vehicles] [spacing]
+#   num_vehicles: Number of drones to spawn (default: 2, max: 10)
+#   spacing: Distance between drones in meters (default: 0.5)
+
+set -e
+
+# Configuration
+num_vehicles=${1:-2}
+spacing=${2:-1}
+MAX_VEHICLES=10
+
+# Get script directory and PX4 root
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+PX4_DIR="$SCRIPT_DIR/../.."
+
+# Build directory
+BUILD_DIR="${PX4_DIR}/build/px4_sitl_default"
+
+# Detect available terminal emulator (Ubuntu only)
+TERMINAL=""
+if command -v terminator &> /dev/null; then
+    TERMINAL="terminator"
+elif command -v gnome-terminal &> /dev/null; then
+    TERMINAL="gnome-terminal"
+else
+    echo "ERROR: No compatible terminal emulator found"
+    echo "Please install either terminator or gnome-terminal"
+    echo "  sudo apt install terminator"
+    echo "  or"
+    echo "  sudo apt install gnome-terminal"
+    exit 1
+fi
+
+echo "Using terminal: $TERMINAL"
+
+# Validate inputs
+if ! [[ "$num_vehicles" =~ ^[0-9]+$ ]] || [ "$num_vehicles" -lt 1 ]; then
+    echo "ERROR: Number of vehicles must be a positive integer"
+    echo "Usage: $0 [num_vehicles] [spacing]"
+    exit 1
+fi
+
+if [ "$num_vehicles" -gt "$MAX_VEHICLES" ]; then
+    echo "ERROR: Number of vehicles cannot exceed $MAX_VEHICLES"
+    echo "Usage: $0 [num_vehicles] [spacing]"
+    exit 1
+fi
+
+if ! [[ "$spacing" =~ ^[0-9]+\.?[0-9]*$ ]]; then
+    echo "ERROR: Spacing must be a positive number"
+    echo "Usage: $0 [num_vehicles] [spacing]"
+    exit 1
+fi
+
+# Check if PX4 is built
+if [ ! -f "${BUILD_DIR}/bin/px4" ]; then
+    echo "ERROR: PX4 SITL not built. Please run 'make px4_sitl' first"
+    exit 1
+fi
+
+echo "=================================================="
+echo "PX4 Multi-Vehicle Gazebo Launcher"
+echo "=================================================="
+echo "Number of vehicles: $num_vehicles"
+echo "Spacing: ${spacing}m"
+echo "Model: gz_x500"
+echo "PX4 Directory: $PX4_DIR"
+echo "=================================================="
+echo ""
+
+# # Kill any existing PX4 instances
+# echo "Cleaning up any existing PX4 instances..."
+# pkill -x px4 || true
+# sleep 1
+
+# # Kill any existing Gazebo instances
+# echo "Cleaning up any existing Gazebo instances..."
+# pkill -x ruby || true  # Gazebo uses ruby
+# pkill gz || true
+# sleep 2
+
+echo ""
+echo "Launching vehicles..."
+echo ""
+
+# Function to launch terminal based on type
+launch_terminal() {
+    local title="$1"
+    local instance="$2"
+    local working_dir="$3"
+    local y_pos="$4"
+
+    case "$TERMINAL" in
+        terminator)
+            terminator --title="$title" -x bash -c "
+                cd '$working_dir'
+                echo '=========================================='
+                echo '$title'
+                echo '=========================================='
+                echo 'Working directory: $working_dir'
+                echo 'Instance: $instance'
+                if [ $instance -eq 0 ]; then
+                    echo 'Mode: Primary (launching Gazebo)'
+                    export PX4_SYS_AUTOSTART=4001
+                    export PX4_SIM_MODEL=gz_x500
+                    '$BUILD_DIR/bin/px4' -i $instance -d '$BUILD_DIR/etc'
+                else
+                    echo 'Mode: Secondary (connecting to Gazebo)'
+                    echo 'Position: (0, $y_pos, 0)'
+                    export PX4_SYS_AUTOSTART=4001
+                    export PX4_SIM_MODEL=gz_x500
+                    export PX4_GZ_MODEL_POSE='0,$y_pos'
+                    export PX4_GZ_STANDALONE=1
+                    '$BUILD_DIR/bin/px4' -i $instance -d '$BUILD_DIR/etc'
+                fi
+                echo ''
+                echo 'PX4 exited. Press Enter to close this terminal...'
+                read
+            " &
+            ;;
+        gnome-terminal)
+            gnome-terminal --title="$title" -- bash -c "
+                cd '$working_dir'
+                echo '=========================================='
+                echo '$title'
+                echo '=========================================='
+                echo 'Working directory: $working_dir'
+                echo 'Instance: $instance'
+                if [ $instance -eq 0 ]; then
+                    echo 'Mode: Primary (launching Gazebo)'
+                    export PX4_SYS_AUTOSTART=4001
+                    export PX4_SIM_MODEL=gz_x500
+                    '$BUILD_DIR/bin/px4' -i $instance -d '$BUILD_DIR/etc'
+                else
+                    echo 'Mode: Secondary (connecting to Gazebo)'
+                    echo 'Position: (0, $y_pos, 0)'
+                    export PX4_SYS_AUTOSTART=4001
+                    export PX4_SIM_MODEL=gz_x500
+                    export PX4_GZ_MODEL_POSE='0,$y_pos'
+                    export PX4_GZ_STANDALONE=1
+                    '$BUILD_DIR/bin/px4' -i $instance -d '$BUILD_DIR/etc'
+                fi
+                echo ''
+                echo 'PX4 exited. Press Enter to close this terminal...'
+                read
+            " &
+            ;;
+    esac
+}
+
+# Launch each vehicle instance
+for i in $(seq 0 $((num_vehicles - 1))); do
+    # Create working directory for this instance
+    working_dir="${BUILD_DIR}/instance_${i}"
+    mkdir -p "$working_dir"
+
+    # Create symlink to gz_env.sh if it doesn't exist
+    if [ ! -f "$working_dir/gz_env.sh" ]; then
+        ln -sf "${BUILD_DIR}/rootfs/gz_env.sh" "$working_dir/gz_env.sh"
+    fi
+
+    # Calculate Y position (spacing along Y-axis)
+    y_pos=$(echo "$i * $spacing" | bc -l)
+
+    # MAV_SYS_ID will be instance + 1
+    mav_sys_id=$((i + 1))
+
+    echo "----------------------------------------"
+    echo "Launching Vehicle $i"
+    echo "  Working directory: $working_dir"
+    echo "  Position: (0, $y_pos, 0)"
+    echo "  MAV_SYS_ID: $mav_sys_id"
+    echo "  Model name: x500_${i}"
+
+    if [ $i -eq 0 ]; then
+        echo "  Mode: Primary (launching Gazebo)"
+    else
+        echo "  Mode: Secondary (connecting to Gazebo)"
+    fi
+
+    # Launch in new terminal
+    title="PX4 Vehicle $i (MAV_SYS_ID=$mav_sys_id)"
+    launch_terminal "$title" "$i" "$working_dir" "$y_pos"
+
+    echo "  Status: Terminal launched"
+    echo "----------------------------------------"
+    echo ""
+
+    # Wait for first instance to start Gazebo before launching others
+    if [ $i -eq 0 ]; then
+        echo "Waiting for Gazebo to start (15 seconds)..."
+        sleep 15
+    fi
+done
+
+echo ""
+echo "=================================================="
+echo "All vehicles launched successfully!"
+echo "=================================================="
+echo ""
+echo "Vehicle Summary:"
+echo "----------------"
+for i in $(seq 0 $((num_vehicles - 1))); do
+    y_pos=$(echo "$i * $spacing" | bc -l)
+    mav_sys_id=$((i + 1))
+    echo "Vehicle $i:"
+    echo "  - MAV_SYS_ID: $mav_sys_id"
+    echo "  - Model name: x500_${i}"
+    echo "  - Position: (0, $y_pos, 0)"
+    echo "  - ROS 2 namespace: px4_${mav_sys_id}"
+    echo "  - Terminal: PX4 Vehicle $i (MAV_SYS_ID=$mav_sys_id)"
+    echo ""
+done
+
+echo "Tips:"
+echo "-----"
+echo "- Each vehicle is running in its own terminal window"
+echo "- QGroundControl should auto-connect to all vehicles"
+echo "- For ROS 2, start MicroXRCEAgent: MicroXRCEAgent udp4 -p 8888"
+echo "- To stop all vehicles, close the terminal windows or run: pkill -x px4"
+echo ""
+echo "=================================================="