simulation/docs/architecture.md

System Architecture

Overview

┌─────────────────────────────────────────────────────────────────┐
│                        Simulation                                │
├───────────────────┬─────────────────────┬───────────────────────┤
│   Gazebo Harmonic │   ArduPilot SITL    │    ROS 2 Nodes        │
│   (Physics/3D)    │   (Flight Control)  │    (Perception/Nav)   │
└───────────────────┴─────────────────────┴───────────────────────┘
         │                    │                      │
         └────────────────────┼──────────────────────┘
                              │
                    ┌─────────┴─────────┐
                    │   MAVLink/MAVROS  │
                    └───────────────────┘

Components

1. Gazebo Harmonic

Role: 3D simulation, physics, sensors

• World simulation: Ground plane, lighting, physics
• UAV model: Iris quadcopter with cameras
• UGV model: Differential drive rover
• Sensors: Cameras, IMU, rangefinder

2. ArduPilot SITL

Role: Flight controller simulation

• EKF3: State estimation using external vision
• Flight modes: GUIDED, LOITER, RTL, LAND
• Motor mixing: Quadcopter dynamics
• Failsafe: Battery, geofence, communication

Key Parameters (GPS-Denied):

GPS_TYPE 0              # GPS disabled
EK3_SRC1_POSXY 6        # External nav for position
EK3_SRC1_VELXY 6        # External nav for velocity
VISO_TYPE 1             # MAVLink vision input
ARMING_CHECK 0          # Disabled for simulation

3. ROS 2 Nodes

Vision Pipeline

Camera → Visual Odometry → Position Estimator → Controller
           ↓
       Optical Flow ─────────────┘

Node	Function
visual_odom_node	ORB feature tracking, pose estimation
optical_flow_node	Lucas-Kanade velocity estimation
position_estimator	Weighted average sensor fusion
ekf_fusion_node	Extended Kalman Filter fusion

Control Pipeline

Search Algorithm → UAV Controller → MAVROS → ArduPilot
                 → UGV Controller → cmd_vel

Node	Function
uav_controller	GUIDED mode control, auto-arm
ugv_controller	Differential drive control
search	Spiral, lawnmower, Lévy walk algorithms

Safety Pipeline

GPS → Geofence Monitor → Failsafe Handler → Emergency Action

Node	Function
geofence_node	GPS-based boundary monitoring
failsafe_handler	Vision loss, battery, emergency

Data Flow

Position Estimation

1. Camera captures frames (30 Hz)
2. ORB detects features
3. Essential matrix computed
4. Relative motion estimated
5. Position integrated
6. Published to /uav/visual_odometry/pose

Control Loop

1. Target received (/uav/setpoint_position)
2. Current position from VO or MAVROS
3. Error computed
4. Velocity command generated
5. Sent via MAVROS to ArduPilot
6. ArduPilot executes motor commands

Geofencing (GPS Only)

1. GPS fix received
2. Check against polygon boundary
3. Check altitude limits
4. If breach: trigger RTL/LAND
5. Navigation continues using VO (not GPS)

Coordinate Frames

Frame	Description
odom	Local origin (takeoff point)
base_link	Vehicle body frame
map	World frame (aligned with odom)

NED Convention:

• X = North (forward)
• Y = East (right)
• Z = Down (negative altitude)

File Structure

src/
├── main.py                      # Entry point
├── vision/
│   ├── object_detector.py       # ArUco marker detection
│   └── camera_processor.py      # Gazebo camera feeds
├── navigation/
│   ├── search.py                # Search algorithm controller
│   ├── flight_tracker.py        # Live flight visualization
│   └── patterns/
│       ├── spiral.py            # Expanding square spiral
│       ├── lawnmower.py         # Back-and-forth lanes
│       └── levy.py              # Lévy walk random search
├── control/
│   ├── uav_controller.py        # UAV flight control (pymavlink)
│   └── ugv_controller.py        # UGV drive control (gz.transport)
├── safety/
│   └── geofence.py              # GPS boundary enforcement
└── utils/
    ├── config.py                # YAML config loader
    ├── convert.py               # Unit/coordinate conversions
    ├── helpers.py               # Distance utilities
    └── recorder.py              # Video recording & cloud upload

Configuration

ArduPilot EKF Sources

EK3_SRC1_POSXY: 6  # External Nav
EK3_SRC1_POSZ: 1   # Barometer
EK3_SRC1_VELXY: 6  # External Nav
EK3_SRC1_YAW: 6    # External Nav

Sensor Weights

vo_weight: 0.6      # Visual odometry
optical_flow: 0.3   # Optical flow
imu: 0.1            # IMU integration

Speed Limits

WPNAV_SPEED: 150      # Horizontal speed (cm/s)
WPNAV_ACCEL: 100      # Horizontal acceleration (cm/s/s)
WPNAV_SPEED_UP: 100   # Climb speed (cm/s)
WPNAV_SPEED_DN: 75    # Descent speed (cm/s)
WPNAV_ACCEL_Z: 75     # Vertical acceleration (cm/s/s)
LOIT_SPEED: 150       # Loiter speed (cm/s)