┌─────────────────────────────────────────────┐
│  Layer 3: Agentic Mission Planner            │
│  "Fly to third floor, inspect rooms"         │
├─────────────────────────────────────────────┤
│  Layer 2: Visual-Inertial SLAM               │
│  Camera + IMU → 6DOF Pose                    │
├─────────────────────────────────────────────┤
│  Layer 1: Nav2 Navigation Stack              │
│  Path planning + Obstacle avoidance          │
└─────────────────────────────────────────────┘

# Test the parser without a drone
parser = MissionParser()
goals = parser.parse("Fly to floor 3, inspect area")
# Returns: [{"type": "NavigateToFloor", "floor": 3}, ...]

# Test SLAM without Nav2
slam = VINSMonoPipeline(config)
slam.process_frame(image, imu_data)
pose = slam.get_pose()  # Returns 6DOF pose

# Test the bridge without hardware
bridge = PoseBridge(max_jump_m=5.0)
accepted = bridge.process(pose)  # Rejects impossible jumps

# Ubuntu 22.04 (recommended)
# ROS2 Humble
# Python 3.10+
# OpenCV 4.x

# Clone the repo
git clone https://github.com/AmSach/GhostPilot.git
cd GhostPilot

# Install Python dependencies
pip install -r requirements.txt

# Run headless simulation (no ROS2 required!)
python3 simulate.py

GhostPilot/
├── src/
│   ├── ghostpilot_core/       # SLAM + Nav2 bridge
│   │   ├── vins_mono.py       # Pure-Python VINS-Mono estimator
│   │   ├── slam_node.py       # ROS2 wrapper
│   │   └── pose_bridge.py     # SLAM → Nav2 translator
│   ├── ghostpilot_agent/      # Mission parser + executor
│   │   ├── mission_parser.py  # Natural language → goals
│   │   └── executor.py        # Goal execution engine
│   └── ghostpilot_gazebo/     # Simulation environments
├── mock_ros2/                 # Test without ROS2 install
├── tests/                     # 63 automated tests
└── simulate.py                # End-to-end demo

Camera Frames → Feature Tracking → IMU Pre-integration
       ↓                ↓                    ↓
   FAST corners    Optical Flow       Motion integration
       ↓                ↓                    ↓
       └────────────────┼────────────────────┘
                          ↓
              Sliding Window Optimization
                          ↓
                    6DOF Pose Estimate

# From vins_mono.py
class FeatureTracker:
    """
    Tracks visual features across frames using:
    - FAST corner detection
    - Pyramidal Lucas-Kanade optical flow
    - Forward-backward consistency check
    """

    def __init__(self, max_features=150):
        self.max_features = max_features
        self.feature_id = 0
        self.tracks = {}  # id → (point, age)

    def detect(self, image):
        """Detect FAST corners in the image."""
        # Convert to grayscale
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

        # Detect FAST corners
        corners = cv2.FAST_create(threshold=20).detect(gray)

        # Limit to max_features
        corners = sorted(corners, key=lambda x: -x.response)[:self.max_features]

        return np.array([[c.pt] for c in corners], dtype=np.float32)

    def track(self, prev_img, curr_img, prev_points):
        """Track points using Lucas-Kanade optical flow."""
        # Forward tracking
        next_points, status, _ = cv2.calcOpticalFlowPyrLK(
            prev_img, curr_img, prev_points, None
        )

        # Backward tracking (consistency check)
        back_points, back_status, _ = cv2.calcOpticalFlowPyrLK(
            curr_img, prev_img, next_points, None
        )

        # Filter: only keep consistent tracks
        fb_error = np.linalg.norm(back_points - prev_points, axis=1)
        valid = (status.flatten() == 1) & (back_status.flatten() == 1) & (fb_error < 1.0)

        return next_points[valid], prev_points[valid], valid

class IMUPreintegration:
    """
    Integrates IMU measurements between keyframes.

    The key insight: instead of storing every IMU sample,
    we pre-integrate them into a single motion constraint.
    """

    def __init__(self, gravity=9.81, gyro_noise=0.1, accel_noise=0.1):
        self.gravity = np.array([0, 0, gravity])
        self.gyro_noise = gyro_noise
        self.accel_noise = accel_noise

        # Pre-integrated state
        self.delta_R = np.eye(3)  # Rotation
        self.delta_v = np.zeros(3)  # Velocity
        self.delta_p = np.zeros(3)  # Position
        self.dt_sum = 0.0

    def integrate(self, accel, gyro, dt):
        """
        Integrate one IMU measurement.

        Args:
            accel: Acceleration [ax, ay, az] in m/s²
            gyro: Angular velocity [wx, wy, wz] in rad/s
            dt: Time since last measurement
        """
        # Mid-point integration for rotation
        gyro_mid = 0.5 * (gyro + gyro)  # Simplified
        dR = self._angle_to_rotation(gyro * dt)

        # Update rotation
        self.delta_R = self.delta_R @ dR

        # Update velocity and position
        # Note: This is simplified; full VINS uses Jacobians
        self.delta_v += self.delta_R @ accel * dt
        self.delta_p += self.delta_v * dt + 0.5 * (self.delta_R @ accel) * dt**2

        self.dt_sum += dt

    def _angle_to_rotation(self, angle_axis):
        """Convert angle-axis to rotation matrix."""
        angle = np.linalg.norm(angle_axis)
        if angle < 1e-10:
            return np.eye(3)
        axis = angle_axis / angle
        return cv2.Rodrigues(angle_axis)[0]

class SlidingWindowOptimizer:
    """
    Optimizes a sliding window of poses and landmarks.

    Key features:
    - Bounded computation (fixed window size)
    - Marginalization of old frames
    - Visual + IMU residuals
    """

    def __init__(self, window_size=10):
        self.window_size = window_size
        self.frames = []
        self.landmarks = {}

    def add_frame(self, frame):
        """Add a new frame to the window."""
        self.frames.append(frame)

        # If window too large, marginalize oldest frame
        if len(self.frames) > self.window_size:
            self._marginalize_oldest()

    def _marginalize_oldest(self):
        """
        Schur complement marginalization.

        Instead of discarding old frames, we compress their
        information into a prior on remaining frames.
        """
        oldest = self.frames.pop(0)

        # Build Schur complement (simplified)
        # In practice, this involves sparse matrix operations
        prior = self._compute_prior(oldest)

        # Add prior to remaining optimization
        self.prior_information = prior

    def optimize(self):
        """
        Run one optimization step.

        Minimizes:
        - Visual reprojection errors
        - IMU pre-integration residuals
        - Prior information (from marginalization)
        """
        # Build system matrix
        H = self._build_hessian()
        b = self._build_gradient()

        # Solve using Cholesky decomposition
        dx = np.linalg.solve(H, b)

        # Update poses
        for i, frame in enumerate(self.frames):
            frame.pose = self._update_pose(frame.pose, dx[i*7:(i+1)*7])

        return self.frames[0].pose if self.frames else None

# Run the tests
# tests/test_core.py

def test_quaternion_normalised():
    """Verify that SLAM output quaternion has unit norm."""
    pipeline = VINSMonoPipeline()

    for i in range(30):
        frame = generate_synthetic_frame(i)
        imu = generate_synthetic_imu(i)
        pipeline.process_frame(frame, imu)

    pose = pipeline.get_pose()
    q = pose[3:7]  # Quaternion part

    assert np.isclose(np.linalg.norm(q), 1.0), f"Quaternion norm: {np.linalg.norm(q)}"

def test_slam_initialises():
    """SLAM should initialise within first 5 frames."""
    pipeline = VINSMonoPipeline()

    for i in range(5):
        pipeline.process_frame(*generate_synthetic_data(i))

    assert pipeline.is_initialised(), "SLAM failed to initialise"

Input:  "Fly to the 2nd floor, inspect the area, avoid personnel, report anomaly"
Output: [
    {"type": "NavigateToFloor", "floor": 2},
    {"type": "InspectArea", "area": "current"},
    {"type": "AvoidObstacle", "obstacle_type": "personnel"},
    {"type": "Report", "data": "anomaly"}
]

class MissionParser:
    """
    Natural language mission parser with dual-mode fallback.

    Mode 1: LLM-assisted (when available)
    Mode 2: Regex fallback (always available, deterministic)
    """

    def __init__(self, llm_available=False):
        self.llm_available = llm_available
        self.patterns = self._build_regex_patterns()

    def _build_regex_patterns(self):
        """Build deterministic regex patterns for common commands."""
        return {
            "floor": re.compile(
                r'(?:fly\s+to\s+)?(?:the\s+)?(\d+)(?:st|nd|rd|th)\s+floor|'
                r'(?:fly\s+to\s+)?(?:the\s+)?(first|second|third|fourth|fifth)\s+floor',
                re.IGNORECASE
            ),
            "inspect": re.compile(
                r'(?:inspect|check|scan)\s+(?:the\s+)?(?:area|room|building)',
                re.IGNORECASE
            ),
            "avoid": re.compile(
                r'(?:avoid|stay\s+away\s+from)\s+(personnel|people|obstacles|machinery)',
                re.IGNORECASE
            ),
            "land": re.compile(
                r'(?:land\s+at|return\s+to)\s+(?:the\s+)?(\w+)',
                re.IGNORECASE
            ),
            "report": re.compile(
                r'(?:report|notify)\s+(\w+)',
                re.IGNORECASE
            )
        }

    def parse(self, command: str) -> List[Dict]:
        """
        Parse a natural language command into structured goals.

        Args:
            command: Natural language mission command

        Returns:
            List of goal dictionaries
        """
        # Try LLM first if available
        if self.llm_available:
            try:
                return self._parse_with_llm(command)
            except Exception as e:
                print(f"LLM parsing failed: {e}, falling back to regex")

        # Always have regex fallback
        return self._parse_with_regex(command)

    def _parse_with_regex(self, command: str) -> List[Dict]:
        """Deterministic regex-based parsing."""
        goals = []

        # Floor navigation
        floor_match = self.patterns["floor"].search(command)
        if floor_match:
            floor = self._extract_floor(floor_match)
            goals.append({"type": "NavigateToFloor", "floor": floor})

        # Inspection
        if self.patterns["inspect"].search(command):
            goals.append({"type": "InspectArea", "area": "current"})

        # Avoidance
        avoid_match = self.patterns["avoid"].search(command)
        if avoid_match:
            goals.append({
                "type": "AvoidObstacle",
                "obstacle_type": avoid_match.group(1)
            })

        # Landing
        land_match = self.patterns["land"].search(command)
        if land_match:
            goals.append({
                "type": "LandAt",
                "location": land_match.group(1)
            })

        # Reporting
        report_match = self.patterns["report"].search(command)
        if report_match:
            goals.append({
                "type": "Report",
                "data": report_match.group(1)
            })

        return goals

    def _extract_floor(self, match) -> int:
        """Convert floor text to integer."""
        # Check for numeric ordinal
        if match.group(1):
            return int(match.group(1))

        # Check for word ordinal
        word_map = {
            "first": 1, "second": 2, "third": 3,
            "fourth": 4, "fifth": 5
        }
        return word_map.get(match.group(2).lower(), 1)

# Scenario: LLM is unavailable (offline deployment, API down)
# Regex still works!

parser = MissionParser(llm_available=False)

# These all work:
parser.parse("Fly to 3rd floor")
parser.parse("Go to the second floor and check the rooms")
parser.parse("Avoid personnel, inspect area, report damage")

class MissionExecutor:
    """
    Executes parsed mission goals with:
    - Sequential goal processing
    - Success/failure tracking
    - Nav2 integration
    """

    def __init__(self, nav2_client=None):
        self.nav2 = nav2_client
        self.mission_log = []

    def execute(self, goals: List[Dict]) -> Dict:
        """
        Execute a list of goals.

        Returns mission report with success/failure status.
        """
        results = []

        for goal in goals:
            result = self._execute_goal(goal)
            results.append({
                "goal": goal,
                "success": result["success"],
                "message": result.get("message", "")
            })

            # Log each goal result
            self.mission_log.append(result)

        return {
            "completed": all(r["success"] for r in results),
            "results": results
        }

    def _execute_goal(self, goal: Dict) -> Dict:
        """Execute a single goal."""
        goal_type = goal["type"]

        handlers = {
            "NavigateToFloor": self._navigate_to_floor,
            "InspectArea": self._inspect_area,
            "AvoidObstacle": self._avoid_obstacle,
            "LandAt": self._land_at,
            "Report": self._send_report
        }

        handler = handlers.get(goal_type)
        if not handler:
            return {"success": False, "message": f"Unknown goal type: {goal_type}"}

        return handler(goal)

    def _navigate_to_floor(self, goal: Dict) -> Dict:
        """Navigate to a specific floor (converts to altitude)."""
        floor = goal["floor"]
        altitude = floor * 3.0  # 3m per floor (configurable)

        # Send to Nav2
        if self.nav2:
            success = self.nav2.go_to_altitude(altitude)
            return {"success": success, "altitude": altitude}

        # Mock mode
        return {"success": True, "altitude": altitude, "mock": True}

    def _avoid_obstacle(self, goal: Dict) -> Dict:
        """Configure obstacle avoidance."""
        obstacle_type = goal.get("obstacle_type", "unknown")

        # Map obstacle types to inflation radii
        inflation_map = {
            "personnel": 2.0,   # 2m safety buffer for people
            "people": 2.0,
            "machinery": 3.0,  # 3m for equipment
            "obstacles": 1.5,  # Default
            "unknown": 1.0
        }

        radius = inflation_map.get(obstacle_type, 1.0)

        # Update Nav2 costmap
        if self.nav2:
            self.nav2.set_inflation_radius(radius)

        return {"success": True, "inflation_radius": radius}

class PoseBridge:
    """
    Converts SLAM pose output to Nav2-compatible localization.

    Key features:
    - Frame transformation
    - Velocity estimation
    - Jump rejection (safety!)
    - Odometry publishing
    """

    def __init__(self, max_jump_meters=5.0, frame_map="map", frame_base="base_link"):
        self.max_jump = max_jump_meters
        self.frame_map = frame_map
        self.frame_base = frame_base

        self.prev_pose = None
        self.prev_time = None

    def process(self, pose: np.ndarray, timestamp: float = None) -> Dict:
        """
        Process a SLAM pose estimate.

        Args:
            pose: 7D pose [x, y, z, qw, qx, qy, qz]
            timestamp: Current time (for velocity estimation)

        Returns:
            Processed localization data, or None if rejected
        """
        # Check for reasonable pose
        if not self._is_valid_pose(pose):
            return {"accepted": False, "reason": "invalid_pose"}

        # Jump rejection
        if self.prev_pose is not None:
            jump = np.linalg.norm(pose[:3] - self.prev_pose[:3])
            if jump > self.max_jump:
                print(f"⚠️ Rejected pose jump: {jump:.1f}m (max: {self.max_jump}m)")
                return {"accepted": False, "reason": "jump_too_large", "jump": jump}

        # Compute velocity
        velocity = self._estimate_velocity(pose, timestamp)

        # Store for next iteration
        self.prev_pose = pose.copy()
        self.prev_time = timestamp

        return {
            "accepted": True,
            "pose": pose,
            "velocity": velocity,
            "frame_map": self.frame_map,
            "frame_base": self.frame_base
        }

    def _is_valid_pose(self, pose: np.ndarray) -> bool:
        """Check if pose is numerically valid."""
        # Check for NaN/Inf
        if not np.all(np.isfinite(pose)):
            return False

        # Check quaternion normalization
        q_norm = np.linalg.norm(pose[3:7])
        if not np.isclose(q_norm, 1.0, atol=0.01):
            print(f"⚠️ Quaternion not normalized: {q_norm}")
            return False

        return True

    def _estimate_velocity(self, pose: np.ndarray, timestamp: float) -> np.ndarray:
        """Estimate velocity from successive poses."""
        if self.prev_pose is None or self.prev_time is None:
            return np.zeros(6)

        dt = timestamp - self.prev_time
        if dt <= 0:
            return np.zeros(6)

        # Linear velocity
        v_linear = (pose[:3] - self.prev_pose[:3]) / dt

        # Angular velocity (simplified)
        v_angular = np.zeros(3)  # Would compute from quaternion derivative

        return np.concatenate([v_linear, v_angular])

# Scenario: SLAM glitch causes 19.8m jump in 1 second
bridge = PoseBridge(max_jump_meters=5.0)

# Normal pose
result1 = bridge.process(np.array([0, 0, 0, 1, 0, 0, 0]), t=0.0)
# → accepted: True

# Glitch: 19.8m jump!
result2 = bridge.process(np.array([19.8, 0, 0, 1, 0, 0, 0]), t=1.0)
# → accepted: False, reason: "jump_too_large", jump: 19.8

# Nav2 never sees the bad estimate — system stays stable

# Headless simulation (no ROS2 required!)
python3 simulate.py

# simulate.py - simplified

def run_simulation():
    # 1. Initialize components
    parser = MissionParser()
    executor = MissionExecutor()
    slam = VINSMonoPipeline()
    bridge = PoseBridge()

    # 2. Parse a mission
    command = "Fly to 2nd floor, inspect area, avoid personnel, report anomaly"
    goals = parser.parse(command)
    print(f"Parsed goals: {goals}")

    # 3. Simulate SLAM convergence
    for i in range(30):
        frame, imu = generate_synthetic_data(i)
        slam.process_frame(frame, imu)

        if slam.is_initialised():
            pose = slam.get_pose()
            result = bridge.process(pose, timestamp=i*0.1)

            if result["accepted"]:
                executor.update_localization(result)

    # 4. Execute mission
    report = executor.execute(goals)

    # 5. Output results
    print(f"\n{'='*50}")
    print(f"Mission completed: {report['completed']}")
    print(f"Final altitude: {slam.get_pose()[2]:.1f}m (expected: {2*3.0}m)")
    print(f"{'='*50}")

    return report

Parsed goals: [
    {'type': 'NavigateToFloor', 'floor': 2},
    {'type': 'InspectArea', 'area': 'current'},
    {'type': 'AvoidObstacle', 'obstacle_type': 'personnel'},
    {'type': 'Report', 'data': 'anomaly'}
]

[SLAM] Initialised at frame 4
[POSE] Accepted pose: [0.0, 0.0, 0.1]
[POSE] Rejected pose jump: 19.8m (max: 5.0m)
[EXEC] Navigating to floor 2 (altitude: 6.0m)
[EXEC] Inspecting area
[EXEC] Avoiding personnel (radius: 2.0m)
[EXEC] Reporting anomaly

==================================================
Mission completed: True
Final altitude: 6.0m (expected: 6.0m)
==================================================