mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 14:41:13 +00:00
feat(stage2-phase2): structlog hot-path, pytest markers, obs package
Phase 2 deliverables not yet committed from plan execution: - structlog wired to 10 hot-path files (orchestrator, eskf, components) - bind_contextvars(correlation_id=frame_id) in process_frame - obs/logging_config.py: configure_logging(env) JSON/console renderer - pyproject.toml: structlog>=25.1, --strict-markers, 6 markers registered - tests/conftest.py: ac(id) validator plugin + pytest_collection hooks Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Yuzviak
@@ -7,12 +7,12 @@ The legacy import path (gps_denied.core.mavlink) re-exports everything here.
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from typing import Callable, Optional
|
||||
|
||||
import numpy as np
|
||||
import structlog
|
||||
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.eskf import ConfidenceTier, ESKFState, IMUMeasurement
|
||||
@@ -22,7 +22,7 @@ from gps_denied.schemas.mavlink import (
|
||||
TelemetryMessage,
|
||||
)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger = structlog.get_logger(__name__)
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# pymavlink conditional import
|
||||
@@ -30,11 +30,11 @@ logger = logging.getLogger(__name__)
|
||||
try:
|
||||
from pymavlink import mavutil as _mavutil # type: ignore
|
||||
_PYMAVLINK_AVAILABLE = True
|
||||
logger.info("pymavlink available — real MAVLink connection enabled")
|
||||
logger.info("pymavlink_available", mode="real_mavlink")
|
||||
except ImportError:
|
||||
_mavutil = None # type: ignore
|
||||
_PYMAVLINK_AVAILABLE = False
|
||||
logger.info("pymavlink not available — using MockMAVConnection (dev/CI mode)")
|
||||
logger.info("pymavlink_unavailable", fallback="MockMAVConnection")
|
||||
|
||||
# GPS epoch offset from Unix epoch (seconds)
|
||||
_GPS_EPOCH_OFFSET = 315_964_800
|
||||
@@ -211,7 +211,7 @@ class MAVLinkBridge:
|
||||
asyncio.create_task(self._imu_receive_loop(), name="mav_imu_input"),
|
||||
asyncio.create_task(self._telemetry_loop(), name="mav_telemetry"),
|
||||
]
|
||||
logger.info("MAVLinkBridge started (conn=%s, %g Hz)", self.connection_string, self.output_hz)
|
||||
logger.info("mavlink_bridge_started", conn=self.connection_string, output_hz=self.output_hz)
|
||||
|
||||
async def stop(self) -> None:
|
||||
"""Cancel background tasks and close connection."""
|
||||
@@ -223,8 +223,7 @@ class MAVLinkBridge:
|
||||
if self._conn:
|
||||
self._conn.close()
|
||||
self._conn = None
|
||||
logger.info("MAVLinkBridge stopped. sent=%d imu_rx=%d",
|
||||
self._sent_count, self._recv_imu_count)
|
||||
logger.info("mavlink_bridge_stopped", sent=self._sent_count, imu_rx=self._recv_imu_count)
|
||||
|
||||
def build_gps_input(self) -> Optional[GPSInputMessage]:
|
||||
"""Build GPSInputMessage from current ESKF state (public, for testing)."""
|
||||
@@ -238,6 +237,7 @@ class MAVLinkBridge:
|
||||
|
||||
async def _gps_output_loop(self) -> None:
|
||||
"""Send GPS_INPUT at output_hz. MAV-01 / MAV-02."""
|
||||
log = logger.bind(task="gps_output_loop")
|
||||
interval = 1.0 / self.output_hz
|
||||
while self._running:
|
||||
try:
|
||||
@@ -250,7 +250,7 @@ class MAVLinkBridge:
|
||||
if self._consecutive_failures >= self.max_consecutive_failures:
|
||||
self._send_reloc_request()
|
||||
except Exception as exc:
|
||||
logger.warning("GPS output loop error: %s", exc)
|
||||
log.warning("gps_output_loop_error", error=str(exc))
|
||||
await asyncio.sleep(interval)
|
||||
|
||||
def _send_gps_input(self, msg: GPSInputMessage) -> None:
|
||||
@@ -289,7 +289,7 @@ class MAVLinkBridge:
|
||||
lon=msg.lon,
|
||||
)
|
||||
except Exception as exc:
|
||||
logger.error("Failed to send GPS_INPUT: %s", exc)
|
||||
logger.error("gps_input_send_failed", error=str(exc))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# MAV-03: IMU receive loop
|
||||
@@ -297,6 +297,7 @@ class MAVLinkBridge:
|
||||
|
||||
async def _imu_receive_loop(self) -> None:
|
||||
"""Receive ATTITUDE/RAW_IMU and invoke ESKF callback. MAV-03."""
|
||||
log = logger.bind(task="imu_receive_loop")
|
||||
while self._running:
|
||||
try:
|
||||
raw = self._recv_imu()
|
||||
@@ -305,7 +306,7 @@ class MAVLinkBridge:
|
||||
if self._on_imu:
|
||||
self._on_imu(raw)
|
||||
except Exception as exc:
|
||||
logger.warning("IMU receive loop error: %s", exc)
|
||||
log.warning("imu_receive_loop_error", error=str(exc))
|
||||
await asyncio.sleep(0.01) # poll at ~100 Hz; blocks throttled by recv_match timeout
|
||||
|
||||
def _recv_imu(self) -> Optional[IMUMeasurement]:
|
||||
@@ -334,7 +335,7 @@ class MAVLinkBridge:
|
||||
timestamp=t,
|
||||
)
|
||||
except Exception as exc:
|
||||
logger.debug("IMU recv error: %s", exc)
|
||||
logger.debug("imu_recv_error", error=str(exc))
|
||||
return None
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
@@ -364,9 +365,9 @@ class MAVLinkBridge:
|
||||
)
|
||||
else:
|
||||
self._conn.named_value_float_send(time=t_boot_ms, name=name, value=value)
|
||||
logger.warning("Re-localisation request sent (failures=%d)", self._consecutive_failures)
|
||||
logger.warning("reloc_request_sent", consecutive_failures=self._consecutive_failures)
|
||||
except Exception as exc:
|
||||
logger.error("Failed to send reloc request: %s", exc)
|
||||
logger.error("reloc_request_send_failed", error=str(exc))
|
||||
|
||||
def _build_reloc_request(self) -> RelocalizationRequest:
|
||||
last_lat, last_lon = None, None
|
||||
@@ -393,13 +394,14 @@ class MAVLinkBridge:
|
||||
|
||||
async def _telemetry_loop(self) -> None:
|
||||
"""Send confidence + drift at 1 Hz. MAV-05."""
|
||||
log = logger.bind(task="telemetry_loop")
|
||||
interval = 1.0 / self.telemetry_hz
|
||||
while self._running:
|
||||
try:
|
||||
self._send_telemetry()
|
||||
self._frames_since_sat += 1
|
||||
except Exception as exc:
|
||||
logger.warning("Telemetry loop error: %s", exc)
|
||||
log.warning("telemetry_loop_error", error=str(exc))
|
||||
await asyncio.sleep(interval)
|
||||
|
||||
def _send_telemetry(self) -> None:
|
||||
@@ -437,7 +439,7 @@ class MAVLinkBridge:
|
||||
else:
|
||||
self._conn.named_value_float_send(time=t_boot_ms, name=name, value=float(value))
|
||||
except Exception as exc:
|
||||
logger.debug("Telemetry send error: %s", exc)
|
||||
logger.debug("telemetry_send_error", error=str(exc))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Connection factory
|
||||
@@ -448,8 +450,8 @@ class MAVLinkBridge:
|
||||
if _PYMAVLINK_AVAILABLE:
|
||||
try:
|
||||
conn = _mavutil.mavlink_connection(self.connection_string)
|
||||
logger.info("MAVLink connection opened: %s", self.connection_string)
|
||||
logger.info("mavlink_connection_opened", conn=self.connection_string)
|
||||
return conn
|
||||
except Exception as exc:
|
||||
logger.warning("Cannot open MAVLink connection (%s) — using mock", exc)
|
||||
logger.warning("mavlink_connection_failed", error=str(exc), fallback="mock")
|
||||
return MockMAVConnection()
|
||||
|
||||
@@ -1,18 +1,20 @@
|
||||
"""Local-disk tile loader (SAT-01/02). Phase 1 home of the existing SatelliteDataManager impl."""
|
||||
|
||||
import hashlib
|
||||
import logging
|
||||
import math
|
||||
import os
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import structlog
|
||||
|
||||
from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.satellite import TileBounds, TileCoords
|
||||
from gps_denied.utils import mercator
|
||||
|
||||
logger = structlog.get_logger(__name__)
|
||||
|
||||
|
||||
class SatelliteDataManager:
|
||||
"""Manages satellite tiles from a local pre-loaded directory.
|
||||
@@ -24,8 +26,6 @@ class SatelliteDataManager:
|
||||
downloads and stores tiles before the mission.
|
||||
"""
|
||||
|
||||
_logger = logging.getLogger(__name__)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
tile_dir: str = ".satellite_tiles",
|
||||
@@ -73,8 +73,10 @@ class SatelliteDataManager:
|
||||
sha.update(chunk)
|
||||
actual = sha.hexdigest()
|
||||
if actual != expected:
|
||||
self._logger.warning("Tile integrity failed: %s (exp %s, got %s)",
|
||||
rel_path, expected[:12], actual[:12])
|
||||
logger.warning("tile_integrity_failed",
|
||||
rel_path=rel_path,
|
||||
expected_prefix=expected[:12],
|
||||
actual_prefix=actual[:12])
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
@@ -4,11 +4,11 @@ SAT-03: GSD normalization — downsample camera frame to satellite resolution.
|
||||
SAT-04: RANSAC homography → WGS84 position; confidence = inlier_ratio.
|
||||
"""
|
||||
|
||||
import logging
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import structlog
|
||||
|
||||
from gps_denied.components.satellite_matcher.protocol import IMetricRefinement
|
||||
from gps_denied.core.models import IModelManager
|
||||
@@ -16,7 +16,7 @@ from gps_denied.schemas import GPSPoint
|
||||
from gps_denied.schemas.metric import AlignmentResult, ChunkAlignmentResult, Sim3Transform
|
||||
from gps_denied.schemas.satellite import TileBounds
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger = structlog.get_logger(__name__)
|
||||
|
||||
|
||||
class MetricRefinement(IMetricRefinement):
|
||||
|
||||
@@ -19,17 +19,17 @@ hardware; Inertial mode is retained for sprint-1 reversibility.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import structlog
|
||||
|
||||
from gps_denied.components.vio.orbslam_backend import ORBVisualOdometry
|
||||
from gps_denied.components.vio.protocol import ISequentialVisualOdometry
|
||||
from gps_denied.schemas import CameraParameters
|
||||
from gps_denied.schemas.vo import Features, Matches, Motion, RelativePose
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger = structlog.get_logger(__name__)
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Optional cuVSLAM SDK import (aarch64 Jetson only — x86 dev/CI must still pass)
|
||||
@@ -70,9 +70,9 @@ class CuVSLAMVisualOdometry(ISequentialVisualOdometry):
|
||||
import cuvslam # type: ignore # only available on Jetson
|
||||
self._cuvslam = cuvslam
|
||||
self._init_tracker()
|
||||
logger.info("CuVSLAMVisualOdometry: cuVSLAM SDK loaded (Jetson mode)")
|
||||
logger.info("cuvslam_sdk_loaded", backend="CuVSLAMVisualOdometry", mode="jetson")
|
||||
except ImportError:
|
||||
logger.info("CuVSLAMVisualOdometry: cuVSLAM not available — using ORB fallback (dev/CI mode)")
|
||||
logger.info("cuvslam_sdk_unavailable", backend="CuVSLAMVisualOdometry", fallback="ORB")
|
||||
|
||||
def _init_tracker(self):
|
||||
"""Initialise cuVSLAM tracker in Inertial mode."""
|
||||
@@ -96,9 +96,9 @@ class CuVSLAMVisualOdometry(ISequentialVisualOdometry):
|
||||
gyro_noise=self._imu_params.get("gyro_noise", 0.001),
|
||||
)
|
||||
self._tracker = self._cuvslam.Tracker(rig_params, tracker_params)
|
||||
logger.info("cuVSLAM tracker initialised in Inertial mode")
|
||||
logger.info("cuvslam_tracker_initialised", mode="inertial")
|
||||
except Exception as exc:
|
||||
logger.error("cuVSLAM tracker init failed: %s", exc)
|
||||
logger.error("cuvslam_tracker_init_failed", error=str(exc))
|
||||
self._cuvslam = None
|
||||
|
||||
@property
|
||||
@@ -156,7 +156,7 @@ class CuVSLAMVisualOdometry(ISequentialVisualOdometry):
|
||||
scale_ambiguous=False, # VO-04: cuVSLAM Inertial mode = metric NED
|
||||
)
|
||||
except Exception as exc:
|
||||
logger.error("cuVSLAM tracking step failed: %s", exc)
|
||||
logger.error("cuvslam_tracking_step_failed", error=str(exc))
|
||||
return None
|
||||
|
||||
|
||||
@@ -205,9 +205,9 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
import cuvslam # type: ignore
|
||||
self._cuvslam = cuvslam
|
||||
self._init_tracker()
|
||||
logger.info("CuVSLAMMonoDepthVisualOdometry: cuVSLAM SDK loaded (Jetson Mono-Depth mode)")
|
||||
logger.info("cuvslam_sdk_loaded", backend="CuVSLAMMonoDepthVisualOdometry", mode="mono_depth")
|
||||
except ImportError:
|
||||
logger.info("CuVSLAMMonoDepthVisualOdometry: cuVSLAM not available — using scaled ORB fallback")
|
||||
logger.info("cuvslam_sdk_unavailable", backend="CuVSLAMMonoDepthVisualOdometry", fallback="scaled_ORB")
|
||||
|
||||
def update_depth_hint(self, depth_hint_m: float) -> None:
|
||||
"""Update barometric altitude used for scale recovery. Call each frame."""
|
||||
@@ -231,11 +231,11 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
tracker_params.use_imu = False
|
||||
tracker_params.odometry_mode = self._cuvslam.OdometryMode.MONO_DEPTH
|
||||
self._tracker = self._cuvslam.Tracker(rig_params, tracker_params)
|
||||
logger.info("cuVSLAM tracker initialised in Mono-Depth mode")
|
||||
logger.info("cuvslam_tracker_initialised", mode="mono_depth")
|
||||
except Exception:
|
||||
logger.exception(
|
||||
"cuVSLAM Mono-Depth tracker init FAILED — falling back to ORB. "
|
||||
"Production Jetson path is DISABLED until this is fixed."
|
||||
"cuvslam_mono_depth_init_failed",
|
||||
note="Production Jetson path is DISABLED until this is fixed.",
|
||||
)
|
||||
self._cuvslam = None
|
||||
|
||||
@@ -277,7 +277,7 @@ class CuVSLAMMonoDepthVisualOdometry(ISequentialVisualOdometry):
|
||||
scale_ambiguous=False,
|
||||
)
|
||||
except Exception:
|
||||
logger.exception("cuVSLAM Mono-Depth tracking step failed — frame dropped")
|
||||
logger.exception("cuvslam_mono_depth_tracking_failed", note="frame dropped")
|
||||
return None
|
||||
|
||||
def _compute_via_orb_scaled(
|
||||
|
||||
@@ -14,18 +14,18 @@ optional-import block isolated.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import Optional
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import structlog
|
||||
|
||||
from gps_denied.components.vio.protocol import ISequentialVisualOdometry
|
||||
from gps_denied.core.models import IModelManager
|
||||
from gps_denied.schemas import CameraParameters
|
||||
from gps_denied.schemas.vo import Features, Matches, Motion, RelativePose
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger = structlog.get_logger(__name__)
|
||||
|
||||
|
||||
class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
@@ -88,7 +88,7 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
pts1, pts2, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=1.0
|
||||
)
|
||||
except Exception as e:
|
||||
logger.error(f"Error finding essential matrix: {e}")
|
||||
logger.error("essential_matrix_failed", error=str(e))
|
||||
return None
|
||||
|
||||
if E is None or E.shape != (3, 3):
|
||||
@@ -98,14 +98,14 @@ class SequentialVisualOdometry(ISequentialVisualOdometry):
|
||||
inlier_count = np.sum(inliers_mask)
|
||||
|
||||
if inlier_count < inlier_threshold:
|
||||
logger.warning(f"Insufficient inliers: {inlier_count} < {inlier_threshold}")
|
||||
logger.warning("insufficient_inliers", n_inliers=int(inlier_count), threshold=inlier_threshold)
|
||||
return None
|
||||
|
||||
# Recover pose
|
||||
try:
|
||||
_, R, t, mask = cv2.recoverPose(E, pts1, pts2, cameraMatrix=K, mask=inliers)
|
||||
except Exception as e:
|
||||
logger.error(f"Error recovering pose: {e}")
|
||||
logger.error("recover_pose_failed", error=str(e))
|
||||
return None
|
||||
|
||||
return Motion(
|
||||
@@ -224,7 +224,7 @@ class ORBVisualOdometry(ISequentialVisualOdometry):
|
||||
try:
|
||||
E, inliers = cv2.findEssentialMat(pts1, pts2, cameraMatrix=K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
|
||||
except Exception as exc:
|
||||
logger.warning("ORB findEssentialMat failed: %s", exc)
|
||||
logger.warning("orb_essential_matrix_failed", error=str(exc))
|
||||
return None
|
||||
if E is None or E.shape != (3, 3) or inliers is None:
|
||||
return None
|
||||
@@ -235,7 +235,7 @@ class ORBVisualOdometry(ISequentialVisualOdometry):
|
||||
try:
|
||||
_, R, t, mask = cv2.recoverPose(E, pts1, pts2, cameraMatrix=K, mask=inliers)
|
||||
except Exception as exc:
|
||||
logger.warning("ORB recoverPose failed: %s", exc)
|
||||
logger.warning("orb_recover_pose_failed", error=str(exc))
|
||||
return None
|
||||
return Motion(translation=t.flatten(), rotation=R, inliers=inlier_mask, inlier_count=inlier_count)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user