From b0805ef05bee026c8f158c74b8fd075ac12e55c1 Mon Sep 17 00:00:00 2001 From: Vyomm Khanna Date: Wed, 4 Dec 2024 02:12:26 -0500 Subject: [PATCH 1/6] Fixed many files to work with new common hopefully --- modules/data_merge/data_merge_worker.py | 4 +- modules/decision/decision.py | 16 +-- modules/detections_and_odometry.py | 6 +- modules/flight_interface/conversions.py | 18 ++-- modules/flight_interface/flight_interface.py | 35 ++++--- .../flight_interface_worker.py | 3 +- modules/odometry_and_waypoint.py | 98 +++++++++++++++++++ tests/integration/test_data_merge_worker.py | 29 +++--- tests/integration/test_decision_worker.py | 17 ++-- .../test_flight_interface_worker.py | 7 +- tests/unit/test_decision.py | 59 ++++++----- 11 files changed, 210 insertions(+), 82 deletions(-) create mode 100644 modules/odometry_and_waypoint.py diff --git a/modules/data_merge/data_merge_worker.py b/modules/data_merge/data_merge_worker.py index 00e1990..e675e25 100644 --- a/modules/data_merge/data_merge_worker.py +++ b/modules/data_merge/data_merge_worker.py @@ -9,7 +9,7 @@ from worker import worker_controller from modules import detections_and_odometry -from modules import drone_odometry_local +from modules import odometry_and_waypoint from modules import lidar_detection @@ -38,7 +38,7 @@ def data_merge_worker( time.sleep(delay) try: - odometry: drone_odometry_local.DroneOdometryLocal = ( + odometry: odometry_and_waypoint.OdometryAndWaypoint = ( odometry_input_queue.queue.get_nowait() ) diff --git a/modules/decision/decision.py b/modules/decision/decision.py index 3d87b81..66c65ee 100644 --- a/modules/decision/decision.py +++ b/modules/decision/decision.py @@ -7,7 +7,7 @@ from .. import decision_command from .. import detections_and_odometry -from .. import drone_odometry_local +from .. import odometry_and_waypoint class Decision: @@ -29,7 +29,7 @@ def run_simple_decision( self, detections_and_odometries: "deque[detections_and_odometry.DetectionsAndOdometry]", proximity_limit: float, - current_flight_mode: drone_odometry_local.FlightMode, + current_flight_mode: odometry_and_waypoint.FlightMode, ) -> "tuple[bool, decision_command.DecisionCommand | None]": """ Runs simple collision avoidance where drone will stop within a set distance of an object. @@ -43,29 +43,29 @@ def run_simple_decision( if self.__command_requested: if start_time - time.time() > self.command_timeout: - if self.__last_command_sent == drone_odometry_local.FlightMode.STOPPED: + if self.__last_command_sent == odometry_and_waypoint.FlightMode.LOITER: return ( decision_command.DecisionCommand.create_stop_mission_and_halt_command() ) - if self.__last_command_sent == drone_odometry_local.FlightMode.MOVING: + if self.__last_command_sent == odometry_and_waypoint.FlightMode.AUTO: return decision_command.DecisionCommand.create_resume_mission_command() continue - if current_flight_mode == drone_odometry_local.FlightMode.STOPPED: + if current_flight_mode == odometry_and_waypoint.FlightMode.LOITER: for detection in detections: if detection.distance < proximity_limit: return False, None self.__command_requested = True - self.__last_command_sent = drone_odometry_local.FlightMode.MOVING + self.__last_command_sent = odometry_and_waypoint.FlightMode.AUTO self.detections_and_odometries.clear() start_time = time.time() return decision_command.DecisionCommand.create_resume_mission_command() - if current_flight_mode == drone_odometry_local.FlightMode.MOVING: + if current_flight_mode == odometry_and_waypoint.FlightMode.AUTO: for detection in detections: if detection.distance < proximity_limit: self.__command_requested = True - self.__last_command_sent = drone_odometry_local.FlightMode.STOPPED + self.__last_command_sent = odometry_and_waypoint.FlightMode.LOITER self.detections_and_odometries.clear() start_time = time.time() return ( diff --git a/modules/detections_and_odometry.py b/modules/detections_and_odometry.py index 1ef2144..d992e17 100644 --- a/modules/detections_and_odometry.py +++ b/modules/detections_and_odometry.py @@ -2,8 +2,8 @@ LiDAR detection and local odometry data structure. """ -from . import drone_odometry_local from . import lidar_detection +from . import odometry_and_waypoint class DetectionsAndOdometry: @@ -17,7 +17,7 @@ class DetectionsAndOdometry: def create( cls, detections: "list[lidar_detection.LidarDetection]", - local_odometry: drone_odometry_local.DroneOdometryLocal, + local_odometry: odometry_and_waypoint.OdometryAndWaypoint, ) -> "tuple[bool, DetectionsAndOdometry | None]": """ Combines lidar readings with local odometry @@ -34,7 +34,7 @@ def __init__( self, create_key: object, detections: "list[lidar_detection.LidarDetection]", - local_odometry: drone_odometry_local.DroneOdometryLocal, + local_odometry: odometry_and_waypoint.OdometryAndWaypoint, ) -> None: """ Private constructor, use create() method. diff --git a/modules/flight_interface/conversions.py b/modules/flight_interface/conversions.py index 92fba79..7c0fb02 100644 --- a/modules/flight_interface/conversions.py +++ b/modules/flight_interface/conversions.py @@ -4,13 +4,13 @@ import pymap3d as pymap -from .. import drone_odometry_local -from ..common.mavlink.modules import drone_odometry +from ..common.modules import position_local +from ..common.modules import position_global def position_global_to_local( - global_position: drone_odometry.DronePosition, home_location: drone_odometry.DronePosition -) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]": + global_position: position_global.PositionGlobal, home_location: position_global.PositionGlobal +) -> "tuple[bool, position_local.PositionLocal | None]": """ Converts global position (geodetic) to local position (NED). """ @@ -23,7 +23,7 @@ def position_global_to_local( home_location.altitude, ) - result, local_position = drone_odometry_local.DronePositionLocal.create(north, east, down) + result, local_position = position_local.PositionLocal.create(north, east, down) if not result: return False, None @@ -31,9 +31,9 @@ def position_global_to_local( def position_local_to_global( - local_position: drone_odometry_local.DronePositionLocal, - home_location: drone_odometry.DronePosition, -) -> "tuple[bool, drone_odometry.DronePosition | None]": + local_position: position_local.PositionLocal, + home_location: position_global.PositionGlobal, +) -> "tuple[bool, position_global.PositionGlobal| None]": """ Converts local position (NED) to global position (geodetic). """ @@ -46,7 +46,7 @@ def position_local_to_global( home_location.altitude, ) - result, global_position = drone_odometry.DronePosition.create(latitude, longitude, altitude) + result, global_position = position_global.PositionGlobal.create(latitude, longitude, altitude) if not result: return False, None diff --git a/modules/flight_interface/flight_interface.py b/modules/flight_interface/flight_interface.py index f29b5c0..f000913 100644 --- a/modules/flight_interface/flight_interface.py +++ b/modules/flight_interface/flight_interface.py @@ -5,9 +5,10 @@ import math from modules import decision_command -from modules import drone_odometry_local -from ..common.mavlink.modules import drone_odometry -from ..common.mavlink.modules import flight_controller +from modules import odometry_and_waypoint +from modules.common.modules.mavlink import flight_controller +from modules.common.modules import position_global +from modules.common.modules import position_local from . import conversions @@ -33,7 +34,7 @@ def create( if not result: return False, None - result, home_location = controller.get_home_location(timeout_home) + result, home_location = controller.get_home_position(timeout_home) if not result: return False, None @@ -67,8 +68,8 @@ def __init__( self, create_key: object, controller: flight_controller.FlightController, - home_location: drone_odometry.DronePosition, - first_waypoint: drone_odometry_local.DronePositionLocal, + home_location: position_global.PositionGlobal, + first_waypoint: position_local.PositionLocal, first_waypoint_distance_tolerance: float, ) -> None: """ @@ -84,7 +85,7 @@ def __init__( self.__run = False def __distance_to_first_waypoint_squared( - self, local_position: drone_odometry_local.DronePositionLocal + self, local_position: position_local.PositionLocal ) -> float: """ Calculates the current distance in metres to the first waypoint (value is squared). @@ -93,7 +94,7 @@ def __distance_to_first_waypoint_squared( delta_y = local_position.east - self.first_waypoint.east return delta_x**2 + delta_y**2 - def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]": + def run(self) -> "tuple[bool, odometry_and_waypoint.OdometryAndWaypoint | None]": """ Returns local drone odometry with timestamp. """ @@ -115,7 +116,7 @@ def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]": if not result: return False, None - flight_mode = drone_odometry_local.FlightMode(flight_mode.value) + flight_mode = odometry_and_waypoint.FlightMode(flight_mode.value) if not self.__run: distance_to_first_waypoint_squared = self.__distance_to_first_waypoint_squared( @@ -126,8 +127,20 @@ def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]": self.__run = True print("Obstacle avoidance started!") - return drone_odometry_local.DroneOdometryLocal.create( - local_position, drone_orientation, flight_mode + result, next_waypoint = self.controller.get_next_waypoint() + if not result: + print("Error initializing flight interface: check waypoints are loaded.") + return False, None + + result, next_waypoint_local = conversions.position_global_to_local( + next_waypoint, self.home_location + ) + + if not result: + return False, None + + return odometry_and_waypoint.OdometryAndWaypoint.create( + local_position, drone_orientation, flight_mode, next_waypoint_local ) def run_decision_handler(self, command: decision_command.DecisionCommand) -> bool: diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index c121fd7..482b447 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -7,6 +7,7 @@ from modules import decision_command from modules import drone_odometry_local +from modules import odometry_and_waypoint from worker import queue_wrapper from worker import worker_controller from . import flight_interface @@ -43,7 +44,7 @@ def flight_interface_worker( result, value = interface.run() if result: - if value.flight_mode == drone_odometry_local.FlightMode.MANUAL: + if value.flight_mode == odometry_and_waypoint.FlightMode.MANUAL: print("Obstacle avoidance killed. Check flight mode.") controller.request_exit() break diff --git a/modules/odometry_and_waypoint.py b/modules/odometry_and_waypoint.py new file mode 100644 index 0000000..a13032f --- /dev/null +++ b/modules/odometry_and_waypoint.py @@ -0,0 +1,98 @@ +""" +Data structure for local odometry data (local position, orientation, +flight mode, and timestamp) combined with next_waypoint for obstacle avoidance. +""" + +import time +import enum +from modules.common.modules import position_local +from modules.common.modules import orientation + + +class FlightMode(enum.Enum): + """ + Possible drone flight modes. + """ + + AUTO = "AUTO" + LOITER = "LOITER" + GUIDED = "GUIDED" + MANUAL = "MANUAL" + + +class OdometryAndWaypoint: + """ + Data structure combining odometry (local position, orientation, and flight mode), + the next waypoint (local position), and a timestamp. + """ + + __create_key = object() + + @classmethod + def create( + cls, + local_position: position_local.PositionLocal, + drone_orientation: orientation.Orientation, + flight_mode: FlightMode, + next_waypoint: position_local.PositionLocal, + ) -> "tuple[bool, OdometryAndWaypoint | None]": + """ + Combines odometry data, next waypoint, and timestamp. + """ + if local_position is None: + return False, None + + if drone_orientation is None: + return False, None + + if flight_mode is None: + return False, None + + if next_waypoint is None: + return False, None + + timestamp = time.time() + + return True, OdometryAndWaypoint( + cls.__create_key, + local_position, + drone_orientation, + flight_mode, + next_waypoint, + timestamp, + ) + + def __init__( + self, + create_key: object, + local_position: position_local.PositionLocal, + drone_orientation: orientation.Orientation, + flight_mode: FlightMode, + next_waypoint: position_local.PositionLocal, + timestamp: float, + ) -> None: + """ + Private constructor, use create() method. + """ + assert create_key is OdometryAndWaypoint.__create_key, "Use create() method" + + self.local_position = local_position + self.drone_orientation = drone_orientation + self.flight_mode = flight_mode + self.next_waypoint = next_waypoint + self.timestamp = timestamp + + def __str__(self) -> str: + """ + String representation. + """ + return ( + f"{self.__class__}, " + f"Local Position: {self.local_position}, " + f"Orientation: Yaw: {self.drone_orientation.yaw}, " + f"Pitch: {self.drone_orientation.pitch}, " + f"Roll: {self.drone_orientation.roll}, " + f"Flight Mode: {self.flight_mode.name}, " + f"Next Waypoint: {self.next_waypoint}, " + f"Timestamp: {self.timestamp}" + ) diff --git a/tests/integration/test_data_merge_worker.py b/tests/integration/test_data_merge_worker.py index b1a2f0a..575acd0 100644 --- a/tests/integration/test_data_merge_worker.py +++ b/tests/integration/test_data_merge_worker.py @@ -7,9 +7,10 @@ import time from modules import detections_and_odometry -from modules import drone_odometry_local +from modules import odometry_and_waypoint from modules import lidar_detection -from modules.common.mavlink.modules import drone_odometry +from modules.common.modules import position_local +from modules.common.modules import orientation from modules.data_merge import data_merge_worker from worker import queue_wrapper from worker import worker_controller @@ -36,18 +37,20 @@ def simulate_flight_interface_worker(in_queue: queue_wrapper.QueueWrapper, ident """ Place example odometry into the queue. """ - result, position = drone_odometry_local.DronePositionLocal.create(float(identifier), 0.0, 0.0) + result, position = position_local.PositionLocal.create(float(identifier), 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result assert orientation is not None - flight_mode = drone_odometry_local.FlightMode.MOVING + flight_mode = odometry_and_waypoint.OdometryAndWaypoint.FlightMode.AUTO - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) + + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation, flight_mode, next_waypoint_local ) assert result assert odometry is not None @@ -123,15 +126,15 @@ def main() -> int: print("queue is empty") time.sleep(DELAY) - # Teardown - controller.request_exit() + # Teardown + controller.request_exit() - detection_in_queue.fill_and_drain_queue() - odometry_in_queue.fill_and_drain_queue() + detection_in_queue.fill_and_drain_queue() + odometry_in_queue.fill_and_drain_queue() - worker.join() + worker.join() - return 0 + return 0 if __name__ == "__main__": diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 55ecc02..8e99823 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -8,8 +8,11 @@ from modules import decision_command from modules import detections_and_odometry from modules import drone_odometry_local +from modules import odometry_and_waypoint from modules import lidar_detection -from modules.common.mavlink.modules import drone_odometry +from modules.common.modules import position_local +from modules.common.modules import orientation + from modules.decision import decision_worker from worker import queue_wrapper from worker import worker_controller @@ -36,18 +39,20 @@ def simulate_data_merge_worker(in_queue: queue_wrapper.QueueWrapper) -> None: detections.append(detection) - result, position = drone_odometry_local.DronePositionLocal.create(0.0, 0.0, 0.0) + result, position = position_local.PositionLocal.create(0.0, 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation = orientation.Orientation.create(0.0, 0.0, 0.0) assert result assert orientation is not None - flight_mode = drone_odometry_local.FlightMode.MOVING + flight_mode = odometry_and_waypoint.FlightMode.AUTO + + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation, flight_mode, next_waypoint_local ) assert result assert odometry is not None diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index 6fc241a..cb06b45 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -7,8 +7,10 @@ import time from modules import decision_command -from modules import drone_odometry_local +from modules import odometry_and_waypoint from modules.flight_interface import flight_interface_worker +from modules.common.modules import position_local +from modules.common.modules import orientation from worker import worker_controller from worker import queue_wrapper @@ -72,7 +74,7 @@ def main() -> int: # Test while True: try: - input_data: drone_odometry_local.DroneOdometryLocal = ( + input_data: odometry_and_waypoint.OdometryAndWaypoint = ( odometry_out_queue.queue.get_nowait() ) assert ( @@ -91,6 +93,7 @@ def main() -> int: print(f"yaw: {str(input_data.drone_orientation.yaw)}") print(f"flight mode: {str(input_data.flight_mode)}") print(f"timestamp: {str(input_data.timestamp)}") + print(f"waypoint: {str(input_data.next_waypoint)}") print("") except queue.Empty: diff --git a/tests/unit/test_decision.py b/tests/unit/test_decision.py index 1a8865a..5500ab4 100644 --- a/tests/unit/test_decision.py +++ b/tests/unit/test_decision.py @@ -6,9 +6,10 @@ from modules import decision_command from modules import detections_and_odometry -from modules import drone_odometry_local +from modules import odometry_and_waypoint from modules import lidar_detection -from modules.common.mavlink.modules import drone_odometry +from modules.common.modules import orientation +from modules.common.modules import position_local from modules.decision import decision @@ -46,18 +47,19 @@ def object_within_proximity_limit_while_moving() -> detections_and_odometry.Dete assert detection is not None detections.append(detection) - result, position = drone_odometry_local.DronePositionLocal.create(0.0, 0.0, 0.0) + result, position = position_local.PositionLocal.create(0.0, 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert orientation_instance is not None - flight_mode = drone_odometry_local.FlightMode.MOVING + flight_mode = odometry_and_waypoint.FlightMode.AUTO + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None @@ -87,18 +89,20 @@ def object_within_proximity_limit_while_stopped() -> detections_and_odometry.Det assert detection is not None detections.append(detection) - result, position = drone_odometry_local.DronePositionLocal.create(0.0, 0.0, 0.0) + result, position = position_local.PositionLocal.create(0.0, 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert orientation_instance is not None - flight_mode = drone_odometry_local.FlightMode.STOPPED + flight_mode = odometry_and_waypoint.FlightMode.LOITER - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) + + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None @@ -122,18 +126,18 @@ def object_outside_proximity_limit_while_moving() -> detections_and_odometry.Det assert detection is not None detections.append(detection) - result, position = drone_odometry_local.DronePositionLocal.create(0.0, 0.0, 0.0) + result, position = position_local.PositionLocal.create(0.0, 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None - - flight_mode = drone_odometry_local.FlightMode.MOVING + assert orientation_instance is not None - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + flight_mode = odometry_and_waypoint.FlightMode.AUTO + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None @@ -157,18 +161,19 @@ def object_outside_proximity_limit_while_stopped() -> detections_and_odometry.De assert detection is not None detections.append(detection) - result, position = drone_odometry_local.DronePositionLocal.create(0.0, 0.0, 0.0) + result, position = position_local.PositionLocal.create(0.0, 0.0, 0.0) assert result assert position is not None - result, orientation = drone_odometry.DroneOrientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert orientation_instance is not None - flight_mode = drone_odometry_local.FlightMode.STOPPED + flight_mode = odometry_and_waypoint.FlightMode.LOITER + next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) - result, odometry = drone_odometry_local.DroneOdometryLocal.create( - position, orientation, flight_mode + result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None From 0d7109bdaf87dd4aabda112f58ecdd916f718ee7 Mon Sep 17 00:00:00 2001 From: Vyomm Khanna Date: Wed, 4 Dec 2024 02:22:27 -0500 Subject: [PATCH 2/6] added time contraint to exit test_decision_worker infinite while loop so teardown accessed --- modules/drone_odometry_local.py | 115 ------------------ .../flight_interface_worker.py | 1 - tests/integration/test_data_merge_worker.py | 6 +- tests/integration/test_decision_worker.py | 13 +- .../test_flight_interface_worker.py | 2 - 5 files changed, 11 insertions(+), 126 deletions(-) delete mode 100644 modules/drone_odometry_local.py diff --git a/modules/drone_odometry_local.py b/modules/drone_odometry_local.py deleted file mode 100644 index bef8931..0000000 --- a/modules/drone_odometry_local.py +++ /dev/null @@ -1,115 +0,0 @@ -""" -Data structure for local odometry data (local position, orientation, -flight mode, and timestamp). -""" - -import enum -import time - -from .common.mavlink.modules import drone_odometry - - -class DronePositionLocal: - """ - Drone position using NED. - """ - - __create_key = object() - - @classmethod - def create( - cls, north: float, east: float, down: float - ) -> "tuple[bool, DronePositionLocal | None]": - """ - Local position (NED) - """ - return True, DronePositionLocal(cls.__create_key, north, east, down) - - def __init__(self, create_key: object, north: float, east: float, down: float) -> None: - """ - Private constructor, use create() method. - """ - - assert create_key is DronePositionLocal.__create_key, "Use create() method" - - self.north = north - self.east = east - self.down = down - - def __str__(self) -> str: - """ - String representation - """ - return f"{self.__class__}: North: {self.north}, East: {self.east}, Down: {self.down}." - - -class FlightMode(enum.Enum): - """ - Possible drone flight modes. - """ - - STOPPED = drone_odometry.FlightMode.STOPPED.value - MOVING = drone_odometry.FlightMode.MOVING.value - MANUAL = drone_odometry.FlightMode.MANUAL.value - - -class DroneOdometryLocal: - """ - Data structure combining drone's local position, local orientation, - current flight mode, and timestamp. - """ - - __create_key = object() - - @classmethod - def create( - cls, - local_position: DronePositionLocal, - drone_orientation: drone_odometry.DroneOrientation, - flight_mode: FlightMode, - ) -> "tuple[bool, DroneOdometryLocal | None]": - """ - Combines local odometry data with timestamp - """ - if local_position is None: - return False, None - - if drone_orientation is None: - return False, None - - if flight_mode is None: - return False, None - - timestamp = time.time() - - return True, DroneOdometryLocal( - cls.__create_key, local_position, drone_orientation, flight_mode, timestamp - ) - - def __init__( - self, - create_key: object, - local_position: DronePositionLocal, - drone_orientation: drone_odometry.DroneOrientation, - flight_mode: FlightMode, - timestamp: float, - ) -> None: - """ - Private constructor, use create() method. - """ - - assert create_key is DroneOdometryLocal.__create_key, "Use create() method" - - self.local_position = local_position - self.drone_orientation = drone_orientation - self.flight_mode = flight_mode - self.timestamp = timestamp - - def __str__(self) -> str: - """ - String representation - """ - return f"{self.__class__},\ - {self.local_position}, \ - DroneOrientation: Roll: {self.drone_orientation.roll}, Pitch: {self.drone_orientation.pitch}, Yaw: {self.drone_orientation.yaw}.\ - Flight mode: {self.flight_mode}. Time: {self.timestamp}." diff --git a/modules/flight_interface/flight_interface_worker.py b/modules/flight_interface/flight_interface_worker.py index 482b447..25581fa 100644 --- a/modules/flight_interface/flight_interface_worker.py +++ b/modules/flight_interface/flight_interface_worker.py @@ -6,7 +6,6 @@ import queue from modules import decision_command -from modules import drone_odometry_local from modules import odometry_and_waypoint from worker import queue_wrapper from worker import worker_controller diff --git a/tests/integration/test_data_merge_worker.py b/tests/integration/test_data_merge_worker.py index 575acd0..82a3e7c 100644 --- a/tests/integration/test_data_merge_worker.py +++ b/tests/integration/test_data_merge_worker.py @@ -41,16 +41,16 @@ def simulate_flight_interface_worker(in_queue: queue_wrapper.QueueWrapper, ident assert result assert position is not None - result, orientation = orientation.Orientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert orientation_instance is not None flight_mode = odometry_and_waypoint.OdometryAndWaypoint.FlightMode.AUTO next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( - position, orientation, flight_mode, next_waypoint_local + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None diff --git a/tests/integration/test_decision_worker.py b/tests/integration/test_decision_worker.py index 8e99823..1f091c5 100644 --- a/tests/integration/test_decision_worker.py +++ b/tests/integration/test_decision_worker.py @@ -4,10 +4,10 @@ import multiprocessing as mp import queue +import time from modules import decision_command from modules import detections_and_odometry -from modules import drone_odometry_local from modules import odometry_and_waypoint from modules import lidar_detection from modules.common.modules import position_local @@ -43,16 +43,16 @@ def simulate_data_merge_worker(in_queue: queue_wrapper.QueueWrapper) -> None: assert result assert position is not None - result, orientation = orientation.Orientation.create(0.0, 0.0, 0.0) + result, orientation_instance = orientation.Orientation.create(0.0, 0.0, 0.0) assert result - assert orientation is not None + assert orientation_instance is not None flight_mode = odometry_and_waypoint.FlightMode.AUTO next_waypoint_local = position_local.PositionLocal.create(0.0, 0.0, 0.0) result, odometry = odometry_and_waypoint.OdometryAndWaypoint.create( - position, orientation, flight_mode, next_waypoint_local + position, orientation_instance, flight_mode, next_waypoint_local ) assert result assert odometry is not None @@ -93,7 +93,10 @@ def main() -> int: simulate_data_merge_worker(merged_in_queue) # Test - while True: + start_time = time.time() # Start tracking time + timeout = 5.0 # Exit the loop after 5 seconds (or any reasonable value) + + while time.time() - start_time < timeout: try: input_data: decision_command.DecisionCommand = command_out_queue.queue.get_nowait() diff --git a/tests/integration/test_flight_interface_worker.py b/tests/integration/test_flight_interface_worker.py index cb06b45..7f769c3 100644 --- a/tests/integration/test_flight_interface_worker.py +++ b/tests/integration/test_flight_interface_worker.py @@ -9,8 +9,6 @@ from modules import decision_command from modules import odometry_and_waypoint from modules.flight_interface import flight_interface_worker -from modules.common.modules import position_local -from modules.common.modules import orientation from worker import worker_controller from worker import queue_wrapper From 9789b82c357d66293a9979485dba9f23b54d6f64 Mon Sep 17 00:00:00 2001 From: Vyomm Khanna Date: Wed, 4 Dec 2024 02:26:32 -0500 Subject: [PATCH 3/6] missed a file --- modules/obstacles_and_odometry.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/obstacles_and_odometry.py b/modules/obstacles_and_odometry.py index 04d2b53..c51c6f8 100644 --- a/modules/obstacles_and_odometry.py +++ b/modules/obstacles_and_odometry.py @@ -2,8 +2,8 @@ Obstacles and local odometry merged data structure. """ -from . import drone_odometry_local from . import obstacle +from modules import odometry_and_waypoint class ObstaclesAndOdometry: @@ -17,7 +17,7 @@ class ObstaclesAndOdometry: def create( cls, obstacles: "list[obstacle.Obstacle]", - local_odometry: drone_odometry_local.DroneOdometryLocal, + local_odometry: odometry_and_waypoint.OdometryAndWaypoint, ) -> "tuple[bool, ObstaclesAndOdometry | None]": """ Combines obstacles with local odometry. @@ -31,7 +31,7 @@ def __init__( self, create_key: object, obstacles: "list[obstacle.Obstacle]", - local_odometry: drone_odometry_local.DroneOdometryLocal, + local_odometry: odometry_and_waypoint.OdometryAndWaypoint, ) -> None: """ Private constructor, use create() method. From e0e3f425993a63f96b41b9354f023ab7558873ab Mon Sep 17 00:00:00 2001 From: Vyomm Khanna Date: Wed, 4 Dec 2024 02:29:35 -0500 Subject: [PATCH 4/6] import order issue --- modules/obstacles_and_odometry.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/obstacles_and_odometry.py b/modules/obstacles_and_odometry.py index c51c6f8..0c8e360 100644 --- a/modules/obstacles_and_odometry.py +++ b/modules/obstacles_and_odometry.py @@ -2,8 +2,8 @@ Obstacles and local odometry merged data structure. """ -from . import obstacle from modules import odometry_and_waypoint +from . import obstacle class ObstaclesAndOdometry: From 5273bb3013e0dbfa3a0fa7f2d8c1bb8396bd6138 Mon Sep 17 00:00:00 2001 From: Mmoyv27 Date: Sun, 15 Dec 2024 09:09:55 -0500 Subject: [PATCH 5/6] Delete modules/odometry_and_waypoint.py --- modules/odometry_and_waypoint.py | 98 -------------------------------- 1 file changed, 98 deletions(-) delete mode 100644 modules/odometry_and_waypoint.py diff --git a/modules/odometry_and_waypoint.py b/modules/odometry_and_waypoint.py deleted file mode 100644 index a13032f..0000000 --- a/modules/odometry_and_waypoint.py +++ /dev/null @@ -1,98 +0,0 @@ -""" -Data structure for local odometry data (local position, orientation, -flight mode, and timestamp) combined with next_waypoint for obstacle avoidance. -""" - -import time -import enum -from modules.common.modules import position_local -from modules.common.modules import orientation - - -class FlightMode(enum.Enum): - """ - Possible drone flight modes. - """ - - AUTO = "AUTO" - LOITER = "LOITER" - GUIDED = "GUIDED" - MANUAL = "MANUAL" - - -class OdometryAndWaypoint: - """ - Data structure combining odometry (local position, orientation, and flight mode), - the next waypoint (local position), and a timestamp. - """ - - __create_key = object() - - @classmethod - def create( - cls, - local_position: position_local.PositionLocal, - drone_orientation: orientation.Orientation, - flight_mode: FlightMode, - next_waypoint: position_local.PositionLocal, - ) -> "tuple[bool, OdometryAndWaypoint | None]": - """ - Combines odometry data, next waypoint, and timestamp. - """ - if local_position is None: - return False, None - - if drone_orientation is None: - return False, None - - if flight_mode is None: - return False, None - - if next_waypoint is None: - return False, None - - timestamp = time.time() - - return True, OdometryAndWaypoint( - cls.__create_key, - local_position, - drone_orientation, - flight_mode, - next_waypoint, - timestamp, - ) - - def __init__( - self, - create_key: object, - local_position: position_local.PositionLocal, - drone_orientation: orientation.Orientation, - flight_mode: FlightMode, - next_waypoint: position_local.PositionLocal, - timestamp: float, - ) -> None: - """ - Private constructor, use create() method. - """ - assert create_key is OdometryAndWaypoint.__create_key, "Use create() method" - - self.local_position = local_position - self.drone_orientation = drone_orientation - self.flight_mode = flight_mode - self.next_waypoint = next_waypoint - self.timestamp = timestamp - - def __str__(self) -> str: - """ - String representation. - """ - return ( - f"{self.__class__}, " - f"Local Position: {self.local_position}, " - f"Orientation: Yaw: {self.drone_orientation.yaw}, " - f"Pitch: {self.drone_orientation.pitch}, " - f"Roll: {self.drone_orientation.roll}, " - f"Flight Mode: {self.flight_mode.name}, " - f"Next Waypoint: {self.next_waypoint}, " - f"Timestamp: {self.timestamp}" - ) From a4125ae831e1e32bf5bc9e8de16916e3d5e9df64 Mon Sep 17 00:00:00 2001 From: Mmoyv27 Date: Sun, 15 Dec 2024 09:19:58 -0500 Subject: [PATCH 6/6] Delete modules/flight_interface/conversions.py --- modules/flight_interface/conversions.py | 53 ------------------------- 1 file changed, 53 deletions(-) delete mode 100644 modules/flight_interface/conversions.py diff --git a/modules/flight_interface/conversions.py b/modules/flight_interface/conversions.py deleted file mode 100644 index 7c0fb02..0000000 --- a/modules/flight_interface/conversions.py +++ /dev/null @@ -1,53 +0,0 @@ -""" -Drone position conversions to and from local (NED) and global (geodetic) space. -""" - -import pymap3d as pymap - -from ..common.modules import position_local -from ..common.modules import position_global - - -def position_global_to_local( - global_position: position_global.PositionGlobal, home_location: position_global.PositionGlobal -) -> "tuple[bool, position_local.PositionLocal | None]": - """ - Converts global position (geodetic) to local position (NED). - """ - north, east, down = pymap.geodetic2ned( - global_position.latitude, - global_position.longitude, - global_position.altitude, - home_location.latitude, - home_location.longitude, - home_location.altitude, - ) - - result, local_position = position_local.PositionLocal.create(north, east, down) - if not result: - return False, None - - return True, local_position - - -def position_local_to_global( - local_position: position_local.PositionLocal, - home_location: position_global.PositionGlobal, -) -> "tuple[bool, position_global.PositionGlobal| None]": - """ - Converts local position (NED) to global position (geodetic). - """ - latitude, longitude, altitude = pymap.ned2geodetic( - local_position.north, - local_position.east, - local_position.down, - home_location.latitude, - home_location.longitude, - home_location.altitude, - ) - - result, global_position = position_global.PositionGlobal.create(latitude, longitude, altitude) - if not result: - return False, None - - return True, global_position