From ab2c3107ccdd23e1e368b522dc723c04760a0a4e Mon Sep 17 00:00:00 2001
From: Xierumeng <xierumeng@hotmail.com>
Date: Mon, 29 Jan 2024 20:49:26 -0800
Subject: [PATCH] Fix position and orientation usage in main_2024.py (#162)

---
 main_2024.py   | 33 ++++++++++++++++++++-------------
 modules/common |  2 +-
 2 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/main_2024.py b/main_2024.py
index 0b586875..c7225af1 100644
--- a/main_2024.py
+++ b/main_2024.py
@@ -9,12 +9,14 @@
 import cv2
 import yaml
 
-from utilities.workers import queue_proxy_wrapper
-from utilities.workers import worker_controller
-from utilities.workers import worker_manager
+from modules import odometry_and_time
 from modules.detect_target import detect_target_worker
 from modules.flight_interface import flight_interface_worker
 from modules.video_input import video_input_worker
+from utilities.workers import queue_proxy_wrapper
+from utilities.workers import worker_controller
+from utilities.workers import worker_manager
+
 
 
 CONFIG_FILE_PATH = pathlib.Path("config.yaml")
@@ -135,16 +137,21 @@ def main() -> int:
         except queue.Empty:
             image = None
 
-        odometry_and_time = flight_interface_to_main_queue.queue.get()
-
-        if odometry_and_time is not None:
-            print("timestamp: " + str(odometry_and_time.timestamp))
-            print("lat: " + str(odometry_and_time.odometry_data.position.latitude))
-            print("lon: " + str(odometry_and_time.odometry_data.position.longitude))
-            print("alt: " + str(odometry_and_time.odometry_data.position.altitude))
-            print("yaw: " + str(odometry_and_time.odometry_data.orientation.yaw))
-            print("roll: " + str(odometry_and_time.odometry_data.orientation.roll))
-            print("pitch: " + str(odometry_and_time.odometry_data.orientation.pitch))
+        odometry_and_time_info: "odometry_and_time.OdometryAndTime | None" = \
+            flight_interface_to_main_queue.queue.get()
+
+        if odometry_and_time_info is not None:
+            timestamp = odometry_and_time_info.timestamp
+            position = odometry_and_time_info.odometry_data.position
+            orientation = odometry_and_time_info.odometry_data.orientation.orientation
+
+            print("timestamp: " + str(timestamp))
+            print("north: " + str(position.north))
+            print("east: " + str(position.east))
+            print("down: " + str(position.down))
+            print("yaw: " + str(orientation.yaw))
+            print("roll: " + str(orientation.roll))
+            print("pitch: " + str(orientation.pitch))
             print("")
 
         if image is None:
diff --git a/modules/common b/modules/common
index 32d6f705..8c8cf6a2 160000
--- a/modules/common
+++ b/modules/common
@@ -1 +1 @@
-Subproject commit 32d6f705fb9b1b834c9b685d0db6ad796ad923bc
+Subproject commit 8c8cf6a2fb0d6337a1cb4adb0c713854f51bc651