This comprehensive system extends the Orca4 AUV simulation to support multi-vehicle formation control using the advanced MADDPG+RBF (Multi-Agent Deep Deterministic Policy Gradient with Radial Basis Function) algorithm. The system features 3 autonomous underwater vehicles (AUVs) operating in coordinated formation, with AUV1 as the intelligent leader and AUV2/AUV3 as adaptive followers.

• System Architecture
• Algorithm Description
• System Components
• Installation & Setup
• Usage Guide
• Configuration & Tuning
• Monitoring & Visualization
• Advanced Features
• Troubleshooting
• Extending the System

The Multi-AUV Formation Control System is built with a layered architecture that ensures modularity, scalability, and robust performance:

┌─────────────────────────────────────────────────────────┐
│                 Multi-AUV Formation System              │
├─────────────────────────────────────────────────────────┤
│  🎯 Mission Layer                                      │
│    ├── formation_mission_runner.py (Mission control)    │
│    └── launch_formation_control.py (Auto launcher)     │
├─────────────────────────────────────────────────────────┤
│  🤖 Control Layer                                       │
│    ├── formation_controller.py (Basic MADDPG+RBF)      │
│    ├── maddpg_rbf_controller.py (Full RBF simulation)  │
│    └── leader_trajectory_controller.py (Leader paths)  │
├─────────────────────────────────────────────────────────┤
│  🚁 Vehicle Layer                                       │
│    ├── AUV1 (Leader)   - Executes trajectories         │
│    ├── AUV2 (Follower) - Δ₁₂ = (-5, 2) m              │
│    └── AUV3 (Follower) - Δ₁₃ = (-5, -2) m             │
└─────────────────────────────────────────────────────────┘

graph LR
    A[formation_mission_runner.py] --> B[AUV1 /cmd_vel]
    B --> C[formation_controller.py]
    C --> D[AUV2 /cmd_vel]
    C --> E[AUV3 /cmd_vel]
    
    A --> F[Base Controllers]
    F --> G[MAVROS Integration]
    G --> H[ArduSub instances]

This implementation is based on the Multi-Agent Deep Deterministic Policy Gradient (MADDPG) algorithm enhanced with Radial Basis Function (RBF) networks, providing intelligent, adaptive formation control:

Formation Configuration:

• AUV1 (Leader): Executes predefined trajectories (sine wave, waypoint, or custom paths)
  • Position: P₁(t) = [x₁(t), y₁(t), z₁(t)]
  • Orientation: ψ₁(t) (yaw angle)
• AUV2 (Follower 1): Δ₁₂ = (-5, 2, 0) m (5m behind, 2m to port of leader)
  • Desired position: P₂,des(t) = P₁(t) + R(ψ₁(t)) * Δ₁₂
  • Where R(ψ₁(t)) = [[cos(ψ₁), -sin(ψ₁), 0], [sin(ψ₁), cos(ψ₁), 0], [0, 0, 1]]
• AUV3 (Follower 2): Δ₁₃ = (-5, -2, 0) m (5m behind, 2m to starboard of leader)
  • Desired position: P₃,des(t) = P₁(t) + R(ψ₁(t)) * Δ₁₃

Control Algorithm:

v_j(t) = v_leader(t) + K_p * (η_j,des(t) - η_j(t)) + K_d * (η̇_j,des(t) - η̇_j(t))

Where:

• v_j(t): Velocity command for follower j at time t
• v_leader(t): Leader velocity vector (acquired from odometry) = [v_x, v_y, v_z, ω_z]
• K_p: Proportional gain matrix (tuned to prevent oscillations) = diag([k_px, k_py, k_pz, k_pω])
• K_d: Derivative gain matrix (for damping) = diag([k_dx, k_dy, k_dz, k_dω])
• η_j,des(t): Desired position of follower j in formation = [x_des, y_des, z_des, ψ_des]
• η_j(t): Current position of follower j = [x_j, y_j, z_j, ψ_j]
• η̇_j,des(t): Desired velocity of follower j in formation
• η̇_j(t): Current velocity of follower j

RBF Network Actor (Advanced Implementation):

• Input state: s_j = [x_j, y_j, z_j, vx_j, vy_j, vz_j, x_1-x_j, y_1-y_j, z_1-z_j, ψ_j, ψ_1-ψ_j]
• RBF activation: φ_k(s) = exp(-γ_k * ||s - c_k||²)
• Weight vector: W = [w_1, w_2, ..., w_n]^T
• Bias vector: b = [b_x, b_y, b_z, b_ψ]^T
• RBF layer output: Φ(s_j) = [φ_1(s_j), φ_2(s_j), ..., φ_n(s_j)]^T
• Output action: a_j = W^T * Φ(s_j) + b = Σ(i=1 to n) w_i * φ_i(s_j) + b

Reward Function:

r_j(t) = -α * ||e_j(t)||² - β * C_j(t) - γ * ||a_j(t)||² + δ * R_formation(t) + ε * R_smooth(t)

Where:

• r_j(t): Reward for agent j at time t
• α: Formation error penalty weight (typically α = 1.0)
• e_j(t): Formation error vector = η_j,des(t) - η_j(t)
• ||e_j(t)||²: Squared formation error = (x_error² + y_error² + z_error²)
• β: Collision penalty weight (typically β = 10.0)
• C_j(t): Collision penalty = max(0, d_safe - d_min(t))
• d_safe: Safe distance threshold (typically 2.0m)
• d_min(t): Minimum distance to other AUVs
• γ: Action penalty weight (typically γ = 0.1)
• ||a_j(t)||²: Squared action magnitude for energy efficiency
• δ: Formation bonus weight (typically δ = 0.5)
• R_formation(t): Formation maintenance bonus = exp(-||e_j(t)||)
• ε: Smoothness bonus weight (typically ε = 0.2)
• R_smooth(t): Action smoothness bonus = -||a_j(t) - a_j(t-1)||²

    AUV2 (Port Follower)
        *
         \
          \
           * AUV1 (Leader)
          /
         /
        *
    AUV3 (Starboard Follower)

• AUV1: Formation leader with autonomous trajectory execution
• AUV2: Port follower, maintaining 5m behind and 2m to port
• AUV3: Starboard follower, maintaining 5m behind and 2m to starboard

1. formation_controller.py ⭐ PRIMARY CONTROLLER

   • Simplified MADDPG+RBF implementation
   • Real-time formation maintenance
   • Optimized for performance and stability

2. maddpg_rbf_controller.py 🧠 ADVANCED CONTROLLER

   • Full RBF network simulation
   • Complete MADDPG algorithm implementation
   • Research and development platform

3. leader_trajectory_controller.py 🎯 TRAJECTORY CONTROLLER

   • Leader-specific path control
   • Supports sine wave and waypoint trajectories
   • Independent leader navigation

4. formation_mission_runner.py 🎮 MISSION COORDINATOR
   • Essential Component - System initialization and coordination
   • Leader trajectory execution
   • Multi-trajectory support (waypoint, sine wave, basic maneuvers)
   • MAVROS integration and health monitoring
   • Base controller initialization for all AUVs

5. launch_formation_control.py 🚀 AUTO LAUNCHER
   • Interactive system launcher
   • Automated component initialization
   • User-friendly mission selection

The Formation Mission Runner serves multiple critical functions:

1. 🔧 System Bootstrap: Enables base controllers for all AUVs
2. 🎮 Leader Control: Provides mission commands to AUV1
3. 🔗 Integration Point: Connects mission planning with formation control
4. 📊 Status Monitoring: Checks MAVROS connectivity and health
5. 🎯 Mission Flexibility: Supports multiple trajectory types
6. ⚡ Real-time Coordination: Synchronizes leader movement with follower control

Without this component: Formation controllers would work, but there would be no automated leader trajectory or system initialization.

• Docker installed and running
• ROS 2 Humble or later
• Gazebo Garden or later
• ArduPilot/ArduSub
• MAVROS

cd /path/to/orca4/docker
./build.sh

./run.sh
ros2 launch orca_bringup multi_auv_sim_launch.py

This comprehensive launch will initialize:

• Gazebo with 3 AUVs in formation starting positions
• ArduSub instances for each AUV (-I0, -I1, -I2)
• MAVROS nodes with proper port configuration
• Base controllers and SLAM for each AUV
• Camera image bridges for all stereo cameras
• Formation controller (followers automatically follow leader)
• Path visualization publishers
• RViz with multi-AUV configuration

The simplest way to start the formation control system:

cd /path/to/orca4
python3 launch_formation_control.py

Interactive menu options:

1. Basic Formation Control - Simplified MADDPG+RBF
2. Full MADDPG+RBF Simulation - Complete RBF network
3. Leader-Only Control - Trajectory control without followers

Mission types:

1. Basic Maneuvers - Simple formation testing
2. Waypoint Trajectory - Algorithm validation path
3. Sine Wave Trajectory - Smooth sinusoidal motion

For fine control over individual components:

# Terminal 1: Start formation controller
ros2 run orca_base formation_controller.py

# Terminal 2: Start mission runner
ros2 run orca_base formation_mission_runner.py

# Optional Terminal 3: Advanced controllers
ros2 run orca_base maddpg_rbf_controller.py
# OR
ros2 run orca_base leader_trajectory_controller.py

Manual control for testing and debugging:

# Move formation forward
ros2 topic pub /auv1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.0}}" --once

# Turn formation left
ros2 topic pub /auv1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.3}}" --once

# Stop formation
ros2 topic pub /auv1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0}, angular: {z: 0.0}}" --once

# Control AUV2 directly (bypass formation control)
ros2 topic pub /auv2/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1}, angular: {z: 0.0}}" --once

# Control AUV3 directly (bypass formation control)
ros2 topic pub /auv3/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1}, angular: {z: 0.0}}" --once

Sine Wave Parameters:

• v_forward: Forward velocity (typically 0.5 m/s)
• A: Amplitude of sine wave (typically 5.0 m)
• ω: Angular frequency = 2π/T, where T is period (typically 20s)
• φ: Phase offset (typically 0)
• z_const: Constant depth (typically -5.0 m)

• AUV1 (Leader, I0):
  • FDM ports: 9002/9003
  • GCS connection: UDP:14550
  • MAVROS TCP: localhost:5760
• AUV2 (Follower, I1):
  • FDM ports: 9012/9013
  • GCS connection: UDP:14560
  • MAVROS TCP: localhost:5761
• AUV3 (Follower, I2):
  • FDM ports: 9022/9023
  • GCS connection: UDP:14570
  • MAVROS TCP: localhost:5762

• K_p < 0.5: Prevents oscillations and overshooting
• Control frequency = 20Hz: Matches base_controller update rate
• Formation distance > 3m: Ensures collision avoidance
• Speed limits < 2.5 m/s: Maintains system stability

• Formation error target: < 1m RMS for good performance
• Response time: < 2 seconds to formation commands
• Stability requirement: No oscillations during steady-state
• Safety margin: > 2m minimum separation between AUVs

RBF_CENTERS = 5          # Number of RBF centers
RBF_GAMMA = 1.0          # RBF width parameter
LEARNING_RATE = 0.001    # Weight update rate
EXPLORATION_NOISE = 0.1  # Action exploration noise

WAYPOINT_TOLERANCE = 1.0    # Distance to consider waypoint reached
MISSION_TIMEOUT = 300       # Maximum mission duration (seconds)
HEALTH_CHECK_INTERVAL = 5   # MAVROS health check frequency

The custom RViz configuration provides comprehensive system monitoring:

• AUV Poses: All vehicle positions and orientations (color-coded)
• Camera Feeds: Live video streams from each AUV's stereo cameras
• Real-time Trajectories: Continuous path visualization for all AUVs
• TF Frames: Complete transform tree for each AUV namespace
• Formation Status: Visual indicators of formation maintenance

Individual camera streams for each AUV:

• AUV1: /auv1/stereo_left, /auv1/stereo_right
• AUV2: /auv2/stereo_left, /auv2/stereo_right
• AUV3: /auv3/stereo_left, /auv3/stereo_right

Real-time path tracking with color coding:

• AUV1: /auv1/path (Magenta - Leader)
• AUV2: /auv2/path (Green - Port Follower)
• AUV3: /auv3/path (Blue - Starboard Follower)

# Verify formation controller is running
ros2 node list | grep formation

# Monitor formation error
ros2 topic echo /formation_status

# Check control commands
ros2 topic echo /auv2/cmd_vel
ros2 topic echo /auv3/cmd_vel

# Check all MAVROS connections
ros2 topic list | grep mavros

# Monitor AUV states
ros2 topic echo /auv1/mavros/state
ros2 topic echo /auv2/mavros/state
ros2 topic echo /auv3/mavros/state

# Check odometry data
ros2 topic hz /model/auv1/odometry
ros2 topic hz /model/auv2/odometry
ros2 topic hz /model/auv3/odometry

# Monitor formation control frequency
ros2 topic hz /auv2/cmd_vel
ros2 topic hz /auv3/cmd_vel

The system supports comprehensive testing scenarios for validation and research:

python3 launch_formation_control.py
# Select: 1 (Basic Formation), 1 (Basic Maneuvers)

• Purpose: Verify formation maintenance during simple maneuvers
• Duration: 2-5 minutes
• Metrics: Formation error, response time, stability

python3 launch_formation_control.py  
# Select: 1 (Basic Formation), 2 (Waypoint Trajectory)

• Purpose: Test waypoint trajectory from research algorithm
• Path: (0,0)→(20,-13)→(10,-23)→(-10,-8)→(0,0) × 2 cycles
• Metrics: Path following accuracy, formation maintenance

python3 launch_formation_control.py
# Select: 2 (Full MADDPG+RBF), 3 (Sine Wave)

• Purpose: Full MADDPG+RBF algorithm with continuous trajectories
• Features: Real-time learning, adaptive control, smooth tracking

formation_mission_runner.py
├── Requires: Base controllers enabled for all AUVs
├── Publishes: /auv1/cmd_vel (Leader trajectory commands)
├── Services: /auv1/conn, /auv2/conn, /auv3/conn (MAVROS connectivity)
├── Monitors: MAVROS states and health for all vehicles
└── Coordinates: Mission execution and system synchronization

formation_controller.py  
├── Subscribes: /model/auv1/odometry (Leader state information)
├── Subscribes: /model/auv2/odometry, /model/auv3/odometry (Follower states)
├── Publishes: /auv2/cmd_vel, /auv3/cmd_vel (Follower control commands)
├── Algorithm: MADDPG+RBF simplified implementation
└── Features: Real-time formation control, collision avoidance

orca_base/scripts/
├── formation_controller.py         # Primary formation control (MADDPG+RBF)
├── formation_mission_runner.py     # Mission coordination and leader control
├── maddpg_rbf_controller.py       # Advanced RBF network implementation
├── leader_trajectory_controller.py # Independent leader path control
├── multi_auv_path_publisher.py    # Trajectory visualization
└── auto_connector.py              # System connectivity management

orca_bringup/
├── launch/
│   ├── multi_auv_sim_launch.py    # Main multi-AUV simulation launcher
│   ├── multi_auv_bringup.py       # AUV node coordination
│   └── sim_launch.py              # Single AUV simulation (legacy)
├── params/
│   ├── auv1_mavros_params.yaml    # MAVROS configuration for AUV1
│   ├── auv2_mavros_params.yaml    # MAVROS configuration for AUV2
│   ├── auv3_mavros_params.yaml    # MAVROS configuration for AUV3
│   ├── auv1_orca_params.yaml      # Orca-specific parameters for AUV1
│   ├── auv2_orca_params.yaml      # Orca-specific parameters for AUV2
│   └── auv3_orca_params.yaml      # Orca-specific parameters for AUV3
└── cfg/
    ├── multi_auv_sim_launch.rviz   # RViz configuration for multi-AUV
    ├── auv1_sim_left.ini           # Camera configuration AUV1
    ├── auv2_sim_left.ini           # Camera configuration AUV2
    └── auv3_sim_right.ini          # Camera configuration AUV3

orca_description/
├── models/
│   ├── auv1/                      # AUV1 model and configuration (I0, port 9002)
│   ├── auv2/                      # AUV2 model and configuration (I1, port 9012)
│   └── auv3/                      # AUV3 model and configuration (I2, port 9022)
└── worlds/
    └── multi_auv_sand.world        # Gazebo world with 3 AUVs in formation

/
├── launch_formation_control.py     # Interactive auto-launcher
├── quick_tf_check.sh              # TF tree validation script
├── check_tf_tree.sh               # Comprehensive TF diagnostics
├── validate_multi_auv_setup.sh    # System validation script
└── mophong.txt                     # Original algorithm specification

• Formation Error: Target < 1.0m RMS during steady-state
  • RMS Error: E_RMS = √(1/N * Σ(i=1 to N) ||e_i||²)
  • Where e_i = P_i,des - P_i,actual for each follower AUV
• Response Time: < 2.0 seconds to leader commands
  • Response time: t_response = min{t : ||e(t)|| < 0.1 * ||e(0)||}
• Tracking Accuracy: < 0.5m deviation from desired trajectory
  • Tracking error: E_track = max{||P_leader,actual(t) - P_leader,desired(t)||} over mission
• Collision Avoidance: Maintains > 2.0m minimum inter-vehicle distance
  • Safety metric: d_min(t) = min{||P_i(t) - P_j(t)||} for all i≠j

• Control Loop Frequency: 20Hz (consistent with base_controller)
• Communication Latency: < 50ms for command propagation
• Processing Load: < 30% CPU utilization per AUV controller
• Memory Usage: < 500MB per AUV simulation instance

• Waypoint Accuracy: < 1.0m arrival tolerance
• Mission Completion: > 95% successful completion rate
• Formation Maintenance: < 5% deviation during complex maneuvers
• System Stability: Zero crashes during 30+ minute missions

AUVs Not Following Formation

• Check formation controller status: ros2 node list | grep formation
• Verify odometry topics: ros2 topic list | grep odometry
• Monitor formation controller logs: ros2 topic echo /rosout | grep formation
• Inspect formation error: Formation error > 2m indicates potential issues

Solution Steps:

# 1. Restart formation controller
ros2 run orca_base formation_controller.py

# 2. Check odometry data quality
ros2 topic hz /model/auv1/odometry  # Should be ~20Hz
ros2 topic hz /model/auv2/odometry
ros2 topic hz /model/auv3/odometry

# 3. Verify control commands are being sent
ros2 topic echo /auv2/cmd_vel
ros2 topic echo /auv3/cmd_vel

ArduSub Connection Failures

• Verify ArduSub instances: Check that all 3 instances (-I0, -I1, -I2) are running
• Check port availability: Ensure ports 5760, 5761, 5762 are not in use
• Monitor MAVROS status: All should show "CONNECTED"

Diagnostic Commands:

# Check MAVROS connections
ros2 topic list | grep mavros

# Monitor connection status
ros2 topic echo /auv1/mavros/state
ros2 topic echo /auv2/mavros/state  
ros2 topic echo /auv3/mavros/state

# Verify ArduSub processes
ps aux | grep ArduSub

Solution:

# If connections fail, restart in order:
# 1. Stop all ArduSub instances
pkill -f ArduSub

# 2. Restart the simulation
ros2 launch orca_bringup multi_auv_sim_launch.py

# 3. Wait 10-15 seconds for full initialization

Missing Transform Warnings Common warning: "No transform from [auvX/base_link] to [map]" in RViz

Understanding:

• These warnings are normal during first 10-15 seconds of initialization
• The system uses a dual-transform strategy: static transforms provide initial connectivity, then dynamic transforms take over

Diagnostic Tools:

# Quick TF validation
./quick_tf_check.sh

# Comprehensive TF tree analysis  
./check_tf_tree.sh

# Monitor transform availability
ros2 run tf2_tools view_frames

Expected TF Tree Structure:

map -> auvX/map -> auvX/slam -> auvX/down
               \-> auvX/odom -> auvX/base_link -> auvX/left_camera_link
                                              \-> auvX/right_camera_link

Solution Steps:

# 1. Wait for system initialization (10-15 seconds)
# 2. Verify static transforms are published
ros2 node list | grep static_transform_publisher

# 3. Check base controller status
ros2 topic echo /auv1/rosout | grep base_controller

# 4. If issues persist, restart base controllers
ros2 run orca_base formation_mission_runner.py  # This re-enables base controllers

Missing Camera Feeds in RViz

• Verify image bridge: ros2 node list | grep image_bridge
• Check camera topics: ros2 topic list | grep stereo
• Ensure Gazebo camera plugins: Camera data should be published from Gazebo

Solution:

# Check image topics are active
ros2 topic hz /auv1/stereo_left
ros2 topic hz /auv2/stereo_left
ros2 topic hz /auv3/stereo_left

# If missing, restart image bridges
ros2 run ros_gz_image image_bridge /auv1/stereo_left
ros2 run ros_gz_image image_bridge /auv2/stereo_left
ros2 run ros_gz_image image_bridge /auv3/stereo_left

System Lag or High CPU Usage

• Reduce camera update rates: Modify model.sdf files to lower FPS
• Disable unnecessary RViz displays: Turn off unused visualizations
• Limit path history: Reduce trajectory visualization length

Optimization Settings:

# Monitor system resources
top -p $(pgrep -d',' gazebo)

# Check ROS 2 node performance
ros2 topic hz /model/auv1/odometry  # Should maintain ~20Hz

Formation Controller Oscillations

• Reduce proportional gain: Lower K_p value (try 0.2 instead of 0.3)
• Check formation distance: Ensure > 3m to avoid collision avoidance conflicts
• Verify control frequency: Should match base_controller at 20Hz

Complete System Health Check

# Run comprehensive validation
./validate_multi_auv_setup.sh

# Check all critical components
ros2 node list | grep -E "(formation|mavros|base_controller)"

# Verify all topics are publishing
ros2 topic list | wc -l  # Should show 50+ topics for 3-AUV system

# 1. Stop all processes
pkill -f ros2
pkill -f gazebo
pkill -f ArduSub

# 2. Clean ROS environment
rm -rf ~/.ros/log/*

# 3. Restart from beginning
cd /path/to/orca4/docker
./run.sh
ros2 launch orca_bringup multi_auv_sim_launch.py

# 4. Wait for complete initialization (30+ seconds)
# 5. Launch formation control
python3 launch_formation_control.py

# For formation control issues only
ros2 run orca_base formation_controller.py

# For mission runner issues only  
ros2 run orca_base formation_mission_runner.py

# For visualization issues only
rviz2 -d orca_bringup/cfg/multi_auv_sim_launch.rviz

The system is designed for scalability. To add more AUVs (e.g., AUV4, AUV5):

# Create new model directory
mkdir orca_description/models/auv4
cp -r orca_description/models/auv1/* orca_description/models/auv4/

# Update model.sdf with new port assignment
# I3 instance uses FDM ports 9032/9033
# GCS connection: UDP:14580
# MAVROS TCP: localhost:5763

# Create AUV4 parameter files
cp orca_bringup/params/auv1_mavros_params.yaml orca_bringup/params/auv4_mavros_params.yaml
cp orca_bringup/params/auv1_orca_params.yaml orca_bringup/params/auv4_orca_params.yaml

# Update port configurations in new files

# Update multi_auv_sim_launch.py to include AUV4
# Add new spawn entity with proper namespace and positioning

# Update formation_controller.py for new formation patterns:
# Example: Diamond formation with 4 AUVs
formation_offsets = {
    'auv2': (-5, 2),    # Port follower
    'auv3': (-5, -2),   # Starboard follower  
    'auv4': (-8, 0)     # Rear follower
}

# Modify formation_controller.py
def calculate_line_formation(leader_pos, leader_yaw):
    """Create single-file line formation
    Mathematical model: P_i = P_leader + R(ψ_leader) * [-(i*spacing), 0, 0]
    """
    spacing = 6.0  # Distance between AUVs
    formations = {}
    for i, auv_id in enumerate(['auv2', 'auv3', 'auv4']):
        # Position each AUV directly behind the previous one
        offset_x = -(i + 1) * spacing * math.cos(leader_yaw)
        offset_y = -(i + 1) * spacing * math.sin(leader_yaw)
        formations[auv_id] = (offset_x, offset_y)
    return formations

def calculate_diamond_formation(leader_pos, leader_yaw):
    """Create diamond formation with leader at front
    Mathematical model: 
    - P_left = P_leader + R(ψ) * [-d*cos(θ), d*sin(θ), 0]
    - P_right = P_leader + R(ψ) * [-d*cos(θ), -d*sin(θ), 0]  
    - P_tail = P_leader + R(ψ) * [-2d, 0, 0]
    Where d = formation_distance, θ = formation_angle
    """
    formations = {
        'auv2': (-4, 3),    # Left wing
        'auv3': (-4, -3),   # Right wing
        'auv4': (-8, 0)     # Tail
    }
    return formations

def calculate_v_formation(leader_pos, leader_yaw):
    """Create V-formation for efficient movement
    Mathematical model: V-shape with angle α between arms
    - Left arm: P_i = P_leader + R(ψ) * [-i*d*cos(α/2), i*d*sin(α/2), 0]
    - Right arm: P_j = P_leader + R(ψ) * [-j*d*cos(α/2), -j*d*sin(α/2), 0]
    Where α = V_angle (typically 60°), d = spacing between AUVs
    """
    v_angle = math.pi/3  # 60 degrees
    spacing = 3.0
    formations = {
        'auv2': (-3, 4),    # Left arm of V
        'auv3': (-3, -4),   # Right arm of V
        'auv4': (-6, 6),    # Extended left
        'auv5': (-6, -6)    # Extended right
    }
    return formations

# Extend formation_mission_runner.py
class WaypointMission:
    def __init__(self):
        self.waypoints = [
            (0, 0, 0),      # Start position
            (20, 10, -5),   # Waypoint 1
            (40, -5, -10),  # Waypoint 2
            (20, -20, -5),  # Waypoint 3
            (0, 0, 0)       # Return home
        ]
        self.current_waypoint = 0
        self.waypoint_tolerance = 2.0
    
    def get_next_waypoint(self, current_pos):
        """Return next waypoint or None if mission complete"""
        if self.current_waypoint >= len(self.waypoints):
            return None
            
        target = self.waypoints[self.current_waypoint]
        distance = math.sqrt(
            (current_pos[0] - target[0])**2 + 
            (current_pos[1] - target[1])**2
        )
        
        if distance < self.waypoint_tolerance:
            self.current_waypoint += 1
            
        return target if self.current_waypoint < len(self.waypoints) else None

class GPSMission:
    def __init__(self):
        # Define mission in real GPS coordinates
        self.gps_waypoints = [
            (37.7749, -122.4194, -5),  # San Francisco Bay
            (37.7849, -122.4094, -10), # Alcatraz Island area
            (37.7649, -122.4294, -5)   # Return point
        ]
    
    def convert_gps_to_local(self, gps_coord, origin):
        """Convert GPS coordinates to local simulation coordinates"""
        # Implementation for GPS to local coordinate transformation
        pass

class SearchPatternMission:
    def __init__(self, search_area, pattern_type='lawnmower'):
        self.search_area = search_area  # (x_min, y_min, x_max, y_max)
        self.pattern_type = pattern_type
        self.spacing = 10.0  # Distance between search lines
    
    def generate_lawnmower_pattern(self):
        """Generate lawnmower search pattern waypoints"""
        waypoints = []
        x_min, y_min, x_max, y_max = self.search_area
        
        y = y_min
        going_right = True
        
        while y <= y_max:
            if going_right:
                waypoints.append((x_min, y, -5))
                waypoints.append((x_max, y, -5))
            else:
                waypoints.append((x_max, y, -5))
                waypoints.append((x_min, y, -5))
            
            y += self.spacing
            going_right = not going_right
            
        return waypoints

# Enhance formation_controller.py with obstacle avoidance
class ObstacleAvoidanceFormation:
    def __init__(self):
        self.obstacle_detection_range = 10.0  # meters
        self.avoidance_gain = 2.0
        
    def calculate_avoidance_force(self, auv_pos, obstacles):
        """Calculate repulsive force from nearby obstacles
        Mathematical model: F_avoid = Σ(k_avoid * (1/d² - 1/d_max²) * û_i)
        Where:
        - F_avoid: Total avoidance force vector
        - k_avoid: Avoidance gain constant
        - d: Distance to obstacle i
        - d_max: Maximum detection range
        - û_i: Unit vector pointing away from obstacle i
        """
        avoidance_force = np.array([0.0, 0.0])
        
        for obstacle in obstacles:
            distance = np.linalg.norm(auv_pos - obstacle)
            if distance < self.obstacle_detection_range and distance > 0:
                # Repulsive force inversely proportional to distance squared
                direction = (auv_pos - obstacle) / distance  # Unit vector away from obstacle
                force_magnitude = self.avoidance_gain / (distance**2)
                # Add exponential decay for smoother transitions
                decay_factor = np.exp(-(distance / self.obstacle_detection_range))
                avoidance_force += direction * force_magnitude * decay_factor
                
        return avoidance_force
    
    def modify_formation_with_avoidance(self, formation_command, avoidance_force):
        """Combine formation control with obstacle avoidance
        Final command: u_total = u_formation + α * u_avoidance
        Where α is blending factor (typically 0.3-0.7)
        """
        alpha = 0.5  # Blending factor
        return formation_command + alpha * avoidance_force

# Create mission_manager.py for high-level coordination
class MissionManager:
    def __init__(self):
        self.current_mission = None
        self.mission_queue = []
        self.formation_controller = FormationController()
        
    def add_mission(self, mission):
        """Add mission to execution queue"""
        self.mission_queue.append(mission)
        
    def execute_next_mission(self):
        """Execute next mission in queue"""
        if self.mission_queue:
            self.current_mission = self.mission_queue.pop(0)
            self.current_mission.start()
            
    def update(self):
        """Main update loop for mission execution"""
        if self.current_mission:
            if self.current_mission.is_complete():
                self.execute_next_mission()
            else:
                self.current_mission.update()

# Add service interfaces for external mission control
from example_interfaces.srv import AddTwoInts
from geometry_msgs.msg import Point

class FormationMissionService:
    def __init__(self):
        self.node = rclpy.create_node('formation_mission_service')
        
        # Service for adding waypoints
        self.add_waypoint_srv = self.node.create_service(
            AddWaypoint, 'add_waypoint', self.add_waypoint_callback
        )
        
        # Service for changing formation pattern
        self.change_formation_srv = self.node.create_service(
            ChangeFormation, 'change_formation', self.change_formation_callback
        )
        
    def add_waypoint_callback(self, request, response):
        """Add waypoint to current mission"""
        waypoint = (request.x, request.y, request.z)
        self.mission_manager.add_waypoint(waypoint)
        response.success = True
        return response

• Online Learning: Implement adaptive formation parameters based on performance
• Behavior Cloning: Learn from human operator demonstrations
• Reinforcement Learning: Advanced reward shaping for complex scenarios

• Underwater Currents: Add current simulation for realistic underwater dynamics
• Acoustic Communication: Simulate realistic communication delays and dropouts
• Sensor Noise: Add realistic sensor noise models for robust controller testing

This multi-AUV formation control system provides a comprehensive, extensible platform for research and development in cooperative autonomous vehicle systems, underwater robotics, and multi-agent control algorithms.

The Multi-AUV Formation Control System with MADDPG+RBF algorithm represents a state-of-the-art implementation of cooperative autonomous underwater vehicle control. With its modular architecture, comprehensive testing capabilities, and extensive documentation, it serves as both a practical deployment platform and a research development environment.

Key Features:

• Advanced Algorithm: MADDPG+RBF implementation with multiple controller options
• Scalable Architecture: Easy addition of more AUVs and formation patterns
• Comprehensive Testing: Multiple test scenarios and validation tools
• Real-time Visualization: Complete monitoring and debugging capabilities
• Production Ready: Robust error handling and recovery procedures

Use Cases:

• Research: Algorithm development and validation
• Education: Multi-agent systems and robotics training
• Industry: Underwater inspection, monitoring, and exploration missions
• Defense: Coordinated surveillance and reconnaissance operations

For questions, contributions, or support, please refer to the issue tracker or contact the development team.