diff --git a/controls/sae_2025_ws/src/uav/uav/vehicles/Payload.py b/controls/sae_2025_ws/src/uav/uav/vehicles/Payload.py index a04a2c7ab..f2560020e 100644 --- a/controls/sae_2025_ws/src/uav/uav/vehicles/Payload.py +++ b/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] diff --git a/controls/sae_2025_ws/src/uav/uav/vehicles/UAV.py b/controls/sae_2025_ws/src/uav/uav/vehicles/UAV.py index a5a1ba66a..980c8db33 100644 --- a/controls/sae_2025_ws/src/uav/uav/vehicles/UAV.py +++ b/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, diff --git a/controls/sae_2025_ws/src/uav/uav/vehicles/Vehicle.py b/controls/sae_2025_ws/src/uav/uav/vehicles/Vehicle.py index 2f6b783d9..defebb223 100644 --- a/controls/sae_2025_ws/src/uav/uav/vehicles/Vehicle.py +++ b/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