Type UAV vehicle state

controls/sae_2025_ws/src/uav/uav/vehicles/Payload.py

@@ -1,3 +1,5 @@
+from typing import Any, Protocol, cast
+
 from rclpy.node import Node
 
 from payload_interfaces.msg import (
@@ -13,6 +15,23 @@
 _DEFAULT_UDP_HEARTBEAT_HZ = 10.0
 
 
+class _RawNodeApi(Protocol):
+    def create_publisher(self, *args: Any, **kwargs: Any) -> Any: ...
+    def create_subscription(self, *args: Any, **kwargs: Any) -> Any: ...
+    def create_timer(self, *args: Any, **kwargs: Any) -> Any: ...
+
+
+class _PeerConnections(Protocol):
+    def status(self) -> dict[str, bool]: ...
+
+
+class _PayloadRuntimeNode(Protocol):
+    _raw_node_api: _RawNodeApi
+    _peer_connections: _PeerConnections
+
+    def _now_seconds(self) -> float: ...
+
+
 class Payload(Vehicle):
     """Mission-side adapter for one payload node namespace."""
 
@@ -49,12 +68,13 @@ def __init__(
         # UDP heartbeat.  Supported keys:
         #   tag_visible (bool), tag_distance_m (float), tag_bearing_deg (float),
         #   is_error (bool)
-        self.heartbeat_state: dict = {}
+        self.heartbeat_state: dict[str, bool | float] = {}
 
         self._udp_heartbeat_seq = 0
         self._latest_motor_state: MotorState | None = None
 
-        raw = node._raw_node_api
+        self._runtime_node = cast(_PayloadRuntimeNode, node)
+        raw = self._runtime_node._raw_node_api
         self._udp_heartbeat_pub = raw.create_publisher(
             PayloadHeartbeat, "udp_bridge/out/heartbeat", 10
         )
@@ -75,7 +95,7 @@ def _on_motor_state(self, msg: MotorState) -> None:
     def _publish_udp_heartbeat(self) -> None:
         msg = PayloadHeartbeat()
 
-        now = self.node._now_seconds()
+        now = self._runtime_node._now_seconds()
         msg.stamp.sec = int(now)
         msg.stamp.nanosec = int((now % 1.0) * 1e9)
 
@@ -87,7 +107,7 @@ def _publish_udp_heartbeat(self) -> None:
         msg.mission_started = getattr(self.node, "timer", None) is not None
         msg.is_error = bool(self.heartbeat_state.get("is_error", False))
 
-        peer_status: dict[str, bool] = self.node._peer_connections.status()
+        peer_status = self._runtime_node._peer_connections.status()
         msg.connected_peers = [p for p, alive in peer_status.items() if alive]
         msg.disconnected_peers = [p for p, alive in peer_status.items() if not alive]
 

controls/sae_2025_ws/src/uav/uav/vehicles/UAV.py

@@ -69,10 +69,10 @@ def __init__(
         self.failsafe = False
         self.failsafe_px4 = False
         self.failsafe_trigger = False
-        self.vehicle_status = None
-        self.vehicle_attitude = None
-        self.nav_state = None
-        self.arm_state = None
+        self.vehicle_status: VehicleStatus | None = None
+        self.vehicle_attitude: VehicleAttitude | None = None
+        self.nav_state: int | None = None
+        self.arm_state: int | None = None
 
         self.system_id = 1
         self.component_id = 1
@@ -87,19 +87,19 @@ def __init__(
 
         # set takeoff parameters
         self.origin_set = False
-        self.roll = None
-        self.pitch = None
-        self.yaw = None
+        self.roll: float | None = None
+        self.pitch: float | None = None
+        self.yaw: float | None = None
         self.takeoff_amount = takeoff_amount
         self.attempted_takeoff = False
 
         # Initialize drone position
-        self.local_origin = None
-        self.gps_origin = None
+        self.local_origin: tuple[float, float, float] | None = None
+        self.gps_origin: tuple[float, float, float] | None = None
 
         # Store current drone position
-        self.global_position = None
-        self.local_position = None
+        self.global_position: VehicleGlobalPosition | None = None
+        self.local_position: VehicleLocalPosition | None = None
 
     # -------------------------
     # Public commands
@@ -112,11 +112,39 @@ def arm(self):
         )
         self.node.get_logger().info("Sent Arm Command")
 
+    def _require_global_position(self) -> VehicleGlobalPosition:
+        if self.global_position is None:
+            raise RuntimeError("No GPS data available.")
+        return self.global_position
+
+    def _require_local_position(self) -> VehicleLocalPosition:
+        if self.local_position is None:
+            raise RuntimeError("No local position data available.")
+        return self.local_position
+
+    def _require_gps(self) -> tuple[float, float, float]:
+        gps = self.get_gps()
+        if gps is None:
+            raise RuntimeError("No GPS data available.")
+        return gps
+
+    def _require_local_position_tuple(self) -> tuple[float, float, float]:
+        position = self.get_local_position()
+        if position is None:
+            raise RuntimeError("No local position data available.")
+        return position
+
+    def _require_yaw(self) -> float:
+        if self.yaw is None:
+            raise RuntimeError("No yaw data available.")
+        return self.yaw
+
     def set_origin(self):
-        if self.global_position and self.yaw:
-            lat = self.global_position.lat
-            lon = self.global_position.lon
-            alt = self.global_position.alt
+        if self.global_position is not None and self.yaw is not None:
+            global_position = self.global_position
+            lat = global_position.lat
+            lon = global_position.lon
+            alt = global_position.alt
             self.node.get_logger().info(f"Setting origin to {lat}, {lon}, {alt}")
             self._send_vehicle_command(
                 VehicleCommand.VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
@@ -136,7 +164,7 @@ def set_origin(self):
     def distance_to_waypoint(self, coordinate_system, waypoint) -> float:
         """Calculate the distance to the current waypoint."""
         if coordinate_system == "GPS":
-            curr_gps = self.get_gps()
+            curr_gps = self._require_gps()
             return self.gps_distance_3d(
                 waypoint[0],
                 waypoint[1],
@@ -146,11 +174,15 @@ def distance_to_waypoint(self, coordinate_system, waypoint) -> float:
                 curr_gps[2],
             )
         elif coordinate_system == "LOCAL":
-            return np.sqrt(
-                (self.local_position.x - waypoint[0]) ** 2
-                + (self.local_position.y - waypoint[1]) ** 2
-                + (self.local_position.z - waypoint[2]) ** 2
+            local_position = self._require_local_position()
+            return float(
+                np.sqrt(
+                    (local_position.x - waypoint[0]) ** 2
+                    + (local_position.y - waypoint[1]) ** 2
+                    + (local_position.z - waypoint[2]) ** 2
+                )
             )
+        raise ValueError(f"Unsupported coordinate system: {coordinate_system}")
 
     def hover(self):
         self._send_vehicle_command(
@@ -189,9 +221,10 @@ def takeoff(self):
         """
         if not self.attempted_takeoff:
             self.attempted_takeoff = True
-            lat = self.global_position.lat
-            lon = self.global_position.lon
-            alt = self.global_position.alt
+            global_position = self._require_global_position()
+            lat = global_position.lat
+            lon = global_position.lon
+            alt = global_position.alt
             takeoff_gps = (lat, lon, alt + self.takeoff_amount)
             self._send_vehicle_command(
                 VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF,
@@ -237,9 +270,10 @@ def publish_position_setpoint(self, coordinate, relative=False, lock_yaw=False):
         """
         x, y, z = coordinate
         if relative:
-            x += self.local_position.x
-            y += self.local_position.y
-            z += self.local_position.z
+            local_position = self._require_local_position()
+            x += local_position.x
+            y += local_position.y
+            z += local_position.z
 
         msg = TrajectorySetpoint()
         msg.position = [float(x), float(y), float(z)]
@@ -259,8 +293,9 @@ def publish_position_setpoint(self, coordinate, relative=False, lock_yaw=False):
     def calculate_yaw(self, x: float, y: float) -> float:
         """Calculate the yaw angle to point towards the next waypoint."""
         # Calculate relative position
-        dx = x - self.local_position.x
-        dy = y - self.local_position.y
+        local_position = self._require_local_position()
+        dx = x - local_position.x
+        dy = y - local_position.y
 
         # If very close to target (hovering), maintain current yaw to prevent spinning
         # caused by noisy position estimates when dx/dy are near zero
@@ -312,11 +347,16 @@ def gps_distance_3d(self, lat1, lon1, alt1, lat2, lon2, alt2):
         Returns:
             float: The 3D distance between the two points in feet.
         """
-        # Earth's radius in feet (using an average value)
-        curr_x, curr_y, curr_z = self.gps_to_local((lat1, lon1, alt1))
-        tar_x, tar_y, tar_z = self.gps_to_local((lat2, lon2, alt2))
-        return np.sqrt(
-            (curr_x - tar_x) ** 2 + (curr_y - tar_y) ** 2 + (curr_z - tar_z) ** 2
+        current_local = self.gps_to_local((lat1, lon1, alt1))
+        target_local = self.gps_to_local((lat2, lon2, alt2))
+        if current_local is None or target_local is None:
+            raise RuntimeError("Unable to convert GPS coordinates to local frame.")
+        curr_x, curr_y, curr_z = current_local
+        tar_x, tar_y, tar_z = target_local
+        return float(
+            np.sqrt(
+                (curr_x - tar_x) ** 2 + (curr_y - tar_y) ** 2 + (curr_z - tar_z) ** 2
+            )
         )
 
     def gps_to_local(self, target):
@@ -361,15 +401,16 @@ def uav_to_local(self, point, relative=False):
         :param relative: If True, the point is relative to the current local position.
         :return: A tuple (goal_x, goal_y, goal_z) representing the point in the global frame.
         """
-        current_pos = self.get_local_position()
         point_x, point_y, point_z = point
+        yaw = self._require_yaw()
 
         # Rotate the x and y points according to the UAV's yaw angle.
-        rotated_point_x = point_x * math.cos(self.yaw) - point_y * math.sin(self.yaw)
-        rotated_point_y = point_x * math.sin(self.yaw) + point_y * math.cos(self.yaw)
+        rotated_point_x = point_x * math.cos(yaw) - point_y * math.sin(yaw)
+        rotated_point_y = point_x * math.sin(yaw) + point_y * math.cos(yaw)
 
         # The z-point remains unchanged.
         if relative:
+            current_pos = self._require_local_position_tuple()
             return (
                 current_pos[0] + rotated_point_x,
                 current_pos[1] + rotated_point_y,

controls/sae_2025_ws/src/uav/uav/vehicles/Vehicle.py

@@ -44,10 +44,12 @@ def _normalize_namespace(namespace: str | None) -> str | None:
 
     @staticmethod
     def _vision_node_name(vision_node: object) -> str:
-        if hasattr(vision_node, "node_name"):
-            return vision_node.node_name()
-        if hasattr(vision_node, "__name__"):
-            return vision_node.__name__
+        node_name = getattr(vision_node, "node_name", None)
+        if callable(node_name):
+            return str(node_name())
+        class_name = getattr(vision_node, "__name__", None)
+        if isinstance(class_name, str):
+            return class_name
         return str(vision_node)
 
     def _default_camera_path(self, suffix: str) -> str | None:
@@ -79,8 +81,9 @@ def vision_service_name(self, vision_node: object) -> str:
             raise RuntimeError(
                 f"Vehicle '{self.name}' does not expose a namespaced camera contract."
             )
-        if hasattr(vision_node, "service_name"):
-            return vision_node.service_name()
+        service_name = getattr(vision_node, "service_name", None)
+        if callable(service_name):
+            return str(service_name())
         return self.namespaced_path(f"vision/{self._vision_node_name(vision_node)}")
 
     @abstractmethod