From 6b6688841eb05941279ce07b82b17574dbe13171 Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Wed, 25 Mar 2026 20:56:48 -0400 Subject: [PATCH] vehicle class fields and methods --- .../sae_2025_ws/src/uav/uav/ModeManager.py | 20 +- .../sae_2025_ws/src/uav/uav/Multicopter.py | 9 +- controls/sae_2025_ws/src/uav/uav/Payload.py | 65 ++++++ controls/sae_2025_ws/src/uav/uav/UAV.py | 188 +----------------- controls/sae_2025_ws/src/uav/uav/VTOL.py | 22 +- controls/sae_2025_ws/src/uav/uav/Vehicle.py | 150 ++++++++++++++ controls/sae_2025_ws/src/uav/uav/__init__.py | 6 +- controls/sae_2025_ws/src/uav/uav/utils.py | 21 +- 8 files changed, 265 insertions(+), 216 deletions(-) create mode 100644 controls/sae_2025_ws/src/uav/uav/Payload.py create mode 100644 controls/sae_2025_ws/src/uav/uav/Vehicle.py diff --git a/controls/sae_2025_ws/src/uav/uav/ModeManager.py b/controls/sae_2025_ws/src/uav/uav/ModeManager.py index 12c1b270..b69c6027 100644 --- a/controls/sae_2025_ws/src/uav/uav/ModeManager.py +++ b/controls/sae_2025_ws/src/uav/uav/ModeManager.py @@ -8,9 +8,9 @@ from rclpy.node import Node from px4_msgs.msg import VehicleStatus from std_srvs.srv import Trigger -from uav import VTOL, Multicopter +from uav import VTOL, Multicopter, Payload from uav.autonomous_modes import Mode, LandingMode -from uav.utils import Vehicle +from uav.utils import VehicleType import yaml VISION_NODE_PATH = "uav.vision_nodes" @@ -31,7 +31,7 @@ def __init__(self) -> None: self.declare_parameter("camera_offsets", [0.0, 0.0, 0.0]) self.declare_parameter("debug", False) self.declare_parameter("servo_only", False) - self.declare_parameter("vehicle_class", Vehicle.MULTICOPTER.name) + self.declare_parameter("vehicle_class", VehicleType.MULTICOPTER.name) mode_map = self.get_parameter("mode_map").value if not mode_map: @@ -64,9 +64,11 @@ def __init__(self) -> None: self.active_mode = None self.last_update_time = time() self.start_time = self.last_update_time - # Instantiate appropriate UAV subclass based on vehicle type - if vehicle_class == Vehicle.VTOL: + # Instantiate appropriate Vehicle subclass based on vehicle type + if vehicle_class == VehicleType.VTOL: self.uav = VTOL(self, DEBUG=debug, camera_offsets=camera_offsets) + elif vehicle_class == VehicleType.PAYLOAD: + self.uav = Payload(self, DEBUG=debug) else: self.uav = Multicopter(self, DEBUG=debug, camera_offsets=camera_offsets) self.get_logger().info("Mission Node has started!") @@ -74,14 +76,14 @@ def __init__(self) -> None: self.setup_modes(mode_map) self.servo_only = servo_only - def _parse_vehicle_class(self, vehicle_class) -> Vehicle: - if isinstance(vehicle_class, Vehicle): + def _parse_vehicle_class(self, vehicle_class) -> VehicleType: + if isinstance(vehicle_class, VehicleType): return vehicle_class if isinstance(vehicle_class, str): try: - return Vehicle[vehicle_class.upper()] + return VehicleType[vehicle_class.upper()] except KeyError as exc: - valid = ", ".join(v.name for v in Vehicle) + valid = ", ".join(v.name for v in VehicleType) raise ValueError( f"Invalid vehicle_class '{vehicle_class}'. Expected one of: {valid}" ) from exc diff --git a/controls/sae_2025_ws/src/uav/uav/Multicopter.py b/controls/sae_2025_ws/src/uav/uav/Multicopter.py index 94043e45..10be5ce8 100644 --- a/controls/sae_2025_ws/src/uav/uav/Multicopter.py +++ b/controls/sae_2025_ws/src/uav/uav/Multicopter.py @@ -13,16 +13,15 @@ def __init__( ): super().__init__(node, takeoff_amount, DEBUG, camera_offsets) + @property + def vehicle_type(self) -> str: + return "MULTICOPTER" + @property def is_vtol(self) -> bool: """Multicopters are not VTOL.""" return False - @property - def vehicle_type(self) -> None: - """Multicopters don't have vehicle_type (not VTOL).""" - return None - def vtol_transition_to(self, vtol_state, immediate=False): """Not available on multicopters.""" self.node.get_logger().warn( diff --git a/controls/sae_2025_ws/src/uav/uav/Payload.py b/controls/sae_2025_ws/src/uav/uav/Payload.py new file mode 100644 index 00000000..74062609 --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/Payload.py @@ -0,0 +1,65 @@ +from rclpy.node import Node +from payload_interfaces.msg import DriveCommand, MotorState +from payload_interfaces.srv import TimedDrive, ComputePidZieglerNichols +from uav.Vehicle import Vehicle + + +class Payload(Vehicle): + """ + Python ROS 2 client wrapper for the C++ payload node (ground car). + Communicates with the payload node via topics and services defined in payload_interfaces. + """ + + def __init__(self, node: Node, payload_name: str = "payload", DEBUG: bool = False): + super().__init__(node, DEBUG) + self.payload_name = payload_name + self.motor_state: MotorState | None = None + + self._drive_pub = node.create_publisher( + DriveCommand, f"/{payload_name}/cmd_drive", 10 + ) + self._timed_drive_client = node.create_client( + TimedDrive, f"/{payload_name}/timed_drive" + ) + self._compute_pid_client = node.create_client( + ComputePidZieglerNichols, f"/{payload_name}/compute_pid" + ) + self._motor_state_sub = node.create_subscription( + MotorState, + f"/{payload_name}/motor_state", + self._motor_state_callback, + 10, + ) + + @property + def vehicle_type(self) -> str: + return "PAYLOAD" + + def drive(self, linear: float, angular: float): + """Publish a continuous drive command. linear: m/s, angular: rad/s (+left, -right).""" + msg = DriveCommand() + msg.linear = linear + msg.angular = angular + self._drive_pub.publish(msg) + + def timed_drive(self, linear: float, angular: float, duration_sec: float): + """Call the timed_drive service — drives for duration_sec then auto-stops. Returns a Future.""" + req = TimedDrive.Request() + req.linear = linear + req.angular = angular + req.duration_sec = duration_sec + return self._timed_drive_client.call_async(req) + + def stop(self): + """Stop the ground vehicle.""" + self.drive(0.0, 0.0) + + def compute_pid(self, ku: float, pu: float): + """Call the PID tuning service (Ziegler-Nichols). Returns a Future.""" + req = ComputePidZieglerNichols.Request() + req.ku = ku + req.pu = pu + return self._compute_pid_client.call_async(req) + + def _motor_state_callback(self, msg: MotorState): + self.motor_state = msg diff --git a/controls/sae_2025_ws/src/uav/uav/UAV.py b/controls/sae_2025_ws/src/uav/uav/UAV.py index 698b407f..4013e7f4 100755 --- a/controls/sae_2025_ws/src/uav/uav/UAV.py +++ b/controls/sae_2025_ws/src/uav/uav/UAV.py @@ -1,5 +1,6 @@ -from abc import ABC, abstractmethod +from abc import abstractmethod from rclpy.node import Node +from uav.Vehicle import Vehicle from px4_msgs.msg import ( OffboardControlMode, TrajectorySetpoint, @@ -18,9 +19,7 @@ QoSDurabilityPolicy, ) import numpy as np -import math from uav.px4_modes import PX4CustomMainMode, PX4CustomSubModeAuto -from uav.utils import R_earth # Map nav_state value -> name for readable logging _NAV_STATE_NAMES = { @@ -35,7 +34,7 @@ def get_nav_state_str(val): return _NAV_STATE_NAMES.get(val, str(val)) -class UAV(ABC): +class UAV(Vehicle): """ Abstract base class for UAV control and interfacing with PX4 via ROS 2. Subclasses: VTOL, Multicopter @@ -44,21 +43,16 @@ class UAV(ABC): def __init__( self, node: Node, takeoff_amount=5.0, DEBUG=False, camera_offsets=[0, 0, 0] ): - self.node = node - self.DEBUG = DEBUG + super().__init__(node, DEBUG) self.node.get_logger().info(f"Initializing UAV with DEBUG={DEBUG}") - self.vision_clients = {} - # Initialize necessary parameters to handle PX4 flight failures + # PX4/flight-specific state self.flight_check = False self.emergency_landing = False - 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.system_id = 1 self.component_id = 1 @@ -75,17 +69,12 @@ def __init__( self.origin_set = False self.roll = None self.pitch = None - self.yaw = None self.takeoff_amount = takeoff_amount self.attempted_takeoff = False # Initialize drone position self.local_origin = None - self.gps_origin = None - # Store current drone position - self.global_position = None - self.local_position = None # ------------------------- # Public commands @@ -119,25 +108,6 @@ def set_origin(self): self.camera_offsets = self.uav_to_local(self.camera_offsets) self.origin_set = True - 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() - return self.gps_distance_3d( - waypoint[0], - waypoint[1], - waypoint[2], - curr_gps[0], - curr_gps[1], - 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 - ) - def hover(self): self._send_vehicle_command( VehicleCommand.VEHICLE_CMD_DO_SET_MODE, @@ -239,20 +209,6 @@ def publish_position_setpoint(self, coordinate, relative=False, lock_yaw=False): self.trajectory_publisher.publish(msg) - 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 - - # If very close to target (hovering), maintain current yaw to prevent spinning - # caused by noisy position estimates when dx/dy are near zero - if np.linalg.norm([dx, dy]) < 3.0 and self.yaw is not None: - return self.yaw - - # Calculate yaw angle - yaw = np.arctan2(dy, dx) - return yaw @abstractmethod def _calculate_velocity(self, target_pos: tuple, lock_yaw: bool) -> list: @@ -280,139 +236,9 @@ def publish_offboard_control_heartbeat_signal(self): msg.timestamp = int(self.node.get_clock().now().nanoseconds / 1000) self.offboard_mode_publisher.publish(msg) - def gps_distance_3d(self, lat1, lon1, alt1, lat2, lon2, alt2): - """ - Calculate the 3D distance in feet between two GPS points, including altitude. - - Parameters: - lat1 (float): Latitude of the first point in decimal degrees. - lon1 (float): Longitude of the first point in decimal degrees. - alt1 (float): Altitude of the first point in feet above sea level. - lat2 (float): Latitude of the second point in decimal degrees. - lon2 (float): Longitude of the second point in decimal degrees. - alt2 (float): Altitude of the second point in feet above sea level. - - 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 - ) - - def gps_to_local(self, target): - """ - Convert target GPS coordinates to local NED coordinates. - - Args: - target (tuple): (target_lat, target_lon, target_alt) - ref (tuple): (ref_lat, ref_lon, ref_alt) from the local position message - - Returns: - tuple: (x, y, z) in the local frame where: - x is North (meters), - y is East (meters), - z is Down (meters) - """ - if self.gps_origin is None: - self.node.get_logger().error( - "gps_origin not set. Cannot convert GPS to local coordinates." - ) - return None - - target_lat, target_lon, target_alt = target - ref_lat, ref_lon, ref_alt = self.gps_origin - - # Convert differences in latitude and longitude from degrees to radians - d_lat = math.radians(target_lat - ref_lat) - d_lon = math.radians(target_lon - ref_lon) - - # Compute local displacements - x = d_lat * R_earth # North displacement - y = d_lon * R_earth * math.cos(math.radians(ref_lat)) # East displacement - z = -(target_alt - ref_alt) # Down displacement - - return (x, y, z) - def uav_to_local(self, point, relative=False): - """ - Converts a point in the UAV's local frame to the global frame. - - :param point: A tuple (point_x, point_y, point_z) in the UAV's local frame. - :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 - - # 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) - - # The z-point remains unchanged. - if relative: - return ( - current_pos[0] + rotated_point_x, - current_pos[1] + rotated_point_y, - current_pos[2] + point_z, - ) - else: - return (rotated_point_x, rotated_point_y, point_z) - - def local_to_gps(self, local_pos): - """ - Convert a local NED coordinate to a GPS coordinate. - - Args: - local_pos (tuple): (x, y, z) in meters, where: - x: North displacement, - y: East displacement, - z: Down displacement. - ref_gps (tuple): (lat, lon, alt) of the reference point (takeoff) in degrees and meters. - - Returns: - tuple: (lat, lon, alt) GPS coordinate corresponding to local_pos. - """ - if self.gps_origin is None: - self.node.get_logger().error( - "gps_origin not set. Cannot convert local to GPS coordinates." - ) - return None - else: - x, y, z = local_pos - lat0, lon0, alt0 = self.gps_origin - - # Convert displacements from meters to degrees - dlat = (x / R_earth) * (180.0 / math.pi) - dlon = (y / (R_earth * math.cos(math.radians(lat0)))) * (180.0 / math.pi) - - lat = lat0 + dlat - lon = lon0 + dlon - alt = alt0 - z # because z is down in NED - return (lat, lon, alt) - - # ------------------------- - # Getters / data access - # ------------------------- - def get_gps(self): - if self.global_position: - return ( - self.global_position.lat, - self.global_position.lon, - self.global_position.alt, - ) - else: - self.node.get_logger().warn("No GPS data available.") - return None - - def get_local_position(self): - if self.local_position: - return (self.local_position.x, self.local_position.y, self.local_position.z) - else: - self.node.get_logger().warn("No local position data available.") - return None + """Alias for vehicle_to_local — kept for backwards compatibility.""" + return self.vehicle_to_local(point, relative) def _calculate_proportional_velocity( self, direction: np.ndarray, distance: float diff --git a/controls/sae_2025_ws/src/uav/uav/VTOL.py b/controls/sae_2025_ws/src/uav/uav/VTOL.py index 59a01732..7035e141 100644 --- a/controls/sae_2025_ws/src/uav/uav/VTOL.py +++ b/controls/sae_2025_ws/src/uav/uav/VTOL.py @@ -23,13 +23,17 @@ def __init__( self, node: Node, takeoff_amount=5.0, DEBUG=False, camera_offsets=[0, 0, 0] ): # Initialize VTOL-specific attributes before calling super().__init__ - self.vehicle_type = None # 'MC' or 'FW' from VtolVehicleStatus + self.flight_mode = None # 'MC' or 'FW' from VtolVehicleStatus self.vtol_vehicle_status = None self._fw_takeoff_phase = 0 # state machine phase for FW takeoff self.attempted_takeoff = False super().__init__(node, takeoff_amount, DEBUG, camera_offsets) + @property + def vehicle_type(self) -> str: + return "VTOL" + @property def is_vtol(self) -> bool: """VTOL aircraft can transition between MC and FW modes.""" @@ -103,7 +107,7 @@ def _calculate_velocity(self, target_pos: tuple, lock_yaw: bool) -> list: Returns: [vx, vy, vz] velocity list in m/s """ - is_fw_mode = self.vehicle_type == "FW" + is_fw_mode = self.flight_mode == "FW" if is_fw_mode: # Fixed-wing mode: always maintain forward velocity for lift @@ -152,10 +156,10 @@ def _vtol_vehicle_status_callback(self, msg: VtolVehicleStatus): """ self.vtol_vehicle_status = msg if msg.vehicle_vtol_state == VtolVehicleStatus.VEHICLE_VTOL_STATE_MC: - self.vehicle_type = "MC" + self.flight_mode = "MC" elif msg.vehicle_vtol_state == VtolVehicleStatus.VEHICLE_VTOL_STATE_FW: - self.vehicle_type = "FW" - # During transitions, maintain the current state (don't change vehicle_type) + self.flight_mode = "FW" + # During transitions, maintain the current state (don't change flight_mode) # VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 (still in MC mode) # VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 (still in FW mode) elif ( @@ -163,15 +167,15 @@ def _vtol_vehicle_status_callback(self, msg: VtolVehicleStatus): == VtolVehicleStatus.VEHICLE_VTOL_STATE_TRANSITION_TO_FW ): # Transitioning to FW, but still in MC mode - if self.vehicle_type is None: - self.vehicle_type = "MC" + if self.flight_mode is None: + self.flight_mode = "MC" elif ( msg.vehicle_vtol_state == VtolVehicleStatus.VEHICLE_VTOL_STATE_TRANSITION_TO_MC ): # Transitioning to MC, but still in FW mode - if self.vehicle_type is None: - self.vehicle_type = "FW" + if self.flight_mode is None: + self.flight_mode = "FW" def _initialize_publishers_and_subscribers(self): """ diff --git a/controls/sae_2025_ws/src/uav/uav/Vehicle.py b/controls/sae_2025_ws/src/uav/uav/Vehicle.py new file mode 100644 index 00000000..0f0a857d --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/Vehicle.py @@ -0,0 +1,150 @@ +from abc import ABC, abstractmethod +import math +import numpy as np +from rclpy.node import Node +from uav.utils import R_earth + + +class Vehicle(ABC): + """ + Abstract base class for all vehicles (UAVs, ground vehicles, etc.). + """ + + def __init__(self, node: Node, DEBUG: bool = False): + self.node = node + self.DEBUG = DEBUG + + # Shared state — present on all vehicle types + self.failsafe = False + self.failsafe_trigger = False + self.arm_state = None + + self.local_position = None + self.global_position = None + self.gps_origin = None + self.yaw = None + + self.vision_clients = {} + + @property + @abstractmethod + def vehicle_type(self) -> str: + """Return a string identifying the vehicle type (e.g. 'MULTICOPTER', 'VTOL', 'PAYLOAD').""" + pass + + # ------------------------- + # Getters / data access + # ------------------------- + def get_gps(self): + if self.global_position: + return ( + self.global_position.lat, + self.global_position.lon, + self.global_position.alt, + ) + else: + self.node.get_logger().warn("No GPS data available.") + return None + + def get_local_position(self): + if self.local_position: + return (self.local_position.x, self.local_position.y, self.local_position.z) + else: + self.node.get_logger().warn("No local position data available.") + return None + + # ------------------------- + # Navigation helpers + # ------------------------- + def calculate_yaw(self, x: float, y: float) -> float: + """Calculate the yaw angle to point towards a target (x, y) in the local frame.""" + dx = x - self.local_position.x + dy = y - self.local_position.y + + # If very close to target, maintain current yaw to prevent spinning + if np.linalg.norm([dx, dy]) < 3.0 and self.yaw is not None: + return self.yaw + + return np.arctan2(dy, dx) + + def vehicle_to_local(self, point, relative=False): + """ + Converts a point in the vehicle's body frame to the local world frame. + + :param point: A tuple (x, y, z) in the vehicle's body frame. + :param relative: If True, the result is offset from the current local position. + :return: (x, y, z) in the local world frame. + """ + current_pos = self.get_local_position() + point_x, point_y, point_z = point + + rotated_x = point_x * math.cos(self.yaw) - point_y * math.sin(self.yaw) + rotated_y = point_x * math.sin(self.yaw) + point_y * math.cos(self.yaw) + + if relative: + return ( + current_pos[0] + rotated_x, + current_pos[1] + rotated_y, + current_pos[2] + point_z, + ) + else: + return (rotated_x, rotated_y, point_z) + + def distance_to_waypoint(self, coordinate_system, waypoint) -> float: + """Calculate the distance to a waypoint. coordinate_system: 'GPS' or 'LOCAL'.""" + if coordinate_system == "GPS": + curr_gps = self.get_gps() + return self.gps_distance_3d( + waypoint[0], waypoint[1], waypoint[2], + curr_gps[0], curr_gps[1], 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 + ) + + def gps_distance_3d(self, lat1, lon1, alt1, lat2, lon2, alt2) -> float: + """Calculate the 3D distance in meters between two GPS points.""" + 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 + ) + + def gps_to_local(self, target): + """Convert GPS coordinates (lat, lon, alt) to local NED (x, y, z) in meters.""" + if self.gps_origin is None: + self.node.get_logger().error( + "gps_origin not set. Cannot convert GPS to local coordinates." + ) + return None + + target_lat, target_lon, target_alt = target + ref_lat, ref_lon, ref_alt = self.gps_origin + + d_lat = math.radians(target_lat - ref_lat) + d_lon = math.radians(target_lon - ref_lon) + + x = d_lat * R_earth + y = d_lon * R_earth * math.cos(math.radians(ref_lat)) + z = -(target_alt - ref_alt) + + return (x, y, z) + + def local_to_gps(self, local_pos): + """Convert local NED (x, y, z) in meters to GPS (lat, lon, alt).""" + if self.gps_origin is None: + self.node.get_logger().error( + "gps_origin not set. Cannot convert local to GPS coordinates." + ) + return None + + x, y, z = local_pos + lat0, lon0, alt0 = self.gps_origin + + dlat = (x / R_earth) * (180.0 / math.pi) + dlon = (y / (R_earth * math.cos(math.radians(lat0)))) * (180.0 / math.pi) + + return (lat0 + dlat, lon0 + dlon, alt0 - z) diff --git a/controls/sae_2025_ws/src/uav/uav/__init__.py b/controls/sae_2025_ws/src/uav/uav/__init__.py index 01f52296..496aa6b3 100644 --- a/controls/sae_2025_ws/src/uav/uav/__init__.py +++ b/controls/sae_2025_ws/src/uav/uav/__init__.py @@ -1,5 +1,7 @@ -from .UAV import UAV # Abstract base class +from .Vehicle import Vehicle # Abstract base class for all vehicles +from .UAV import UAV # Abstract base class for UAVs from .VTOL import VTOL # Concrete VTOL implementation from .Multicopter import Multicopter # Concrete multicopter implementation +from .Payload import Payload # Ground vehicle (payload car) -__all__ = ["UAV", "VTOL", "Multicopter"] +__all__ = ["Vehicle", "UAV", "VTOL", "Multicopter", "Payload"] diff --git a/controls/sae_2025_ws/src/uav/uav/utils.py b/controls/sae_2025_ws/src/uav/uav/utils.py index f61aac3f..43fd13ac 100644 --- a/controls/sae_2025_ws/src/uav/uav/utils.py +++ b/controls/sae_2025_ws/src/uav/uav/utils.py @@ -37,14 +37,15 @@ } -class Vehicle(IntEnum): - """Vehicle class enumeration.""" +class VehicleType(IntEnum): + """Vehicle type enumeration.""" MULTICOPTER = 0 PLANE = 1 VTOL = 2 - OTHER = 3 - UNKNOWN = 4 + PAYLOAD = 3 + OTHER = 4 + UNKNOWN = 5 def camel_to_snake(name): @@ -137,7 +138,7 @@ def get_airframe_details(px4_path, airframe_id): """ Parses PX4 airframe files to find vehicle type and model name from an ID. Returns: (vehicle_class, model_name) - Example: (4001) -> (Vehicle.MULTICOPTER, 'x500') + Example: (4001) -> (VehicleType.MULTICOPTER, 'x500') """ # 1. Locate the Airframe File # PX4 stores these in ROMFS/px4fmu_common/init.d-posix/airframes @@ -151,7 +152,7 @@ def get_airframe_details(px4_path, airframe_id): if not matches: print(f"Warning: Airframe ID {airframe_id} not found in {airframes_dir}") - return Vehicle.UNKNOWN, "gz_ERROR" + return VehicleType.UNKNOWN, "gz_ERROR" # 2. Extract Model Name from Filename filename = os.path.basename(matches[0]) @@ -163,13 +164,13 @@ def get_airframe_details(px4_path, airframe_id): content = f.read() if "rc.mc_defaults" in content: - vehicle_class = Vehicle.MULTICOPTER + vehicle_class = VehicleType.MULTICOPTER elif "rc.fw_defaults" in content: - vehicle_class = Vehicle.PLANE + vehicle_class = VehicleType.PLANE elif "rc.vtol_defaults" in content: - vehicle_class = Vehicle.VTOL + vehicle_class = VehicleType.VTOL else: - vehicle_class = Vehicle.OTHER + vehicle_class = VehicleType.OTHER return vehicle_class, model_name