to sync this branch with main

ArmorDetect_D435i.py → ArmorDetect.py

@@ -8,21 +8,17 @@
 from UART_UTIL import send_data, get_imu
 from camera_source import CameraSource
 from kinematic_prediction import poly_predict
-
 import argparse
 import logging
 import time
 from camera_params import camera_params, DepthSource
-from KalmanFilterClass import KalmanFilter
 from Target import Target
-#from sort import *
-
 import struct
 
 active_cam_config = None
 frame_aligner = None
 
-
+# DO NOT REMOVE (?) -- Someone please check if this is actually needed.
 def nothing(x):
     pass
 
@@ -246,10 +242,10 @@ def get_3d_target_location(imgPoints, frame, depth_frame):
 
         # Get Depth value
         meanDVal = np.linalg.norm(tvec[:, 0])
-        Yaw = np.arctan(tvec[(0,0)]/ tvec[(2,0)]) / 2 / 3.1415926535897932 * 360
-        Pitch = -(np.arctan(tvec[(1, 0)] / tvec[(2, 0)]) / 2 / 3.1415926535897932 * 360)
-
-
+        offsetY = 1 # offset for Yaw
+        Yaw = np.arctan(tvec[(0,0)]/ tvec[(2,0)]) / 2 / 3.1415926535897932 * 360 - offsetY
+        offsetP = -8 # offset for Pitch
+        Pitch = -(np.arctan(tvec[(1, 0)] / tvec[(2, 0)]) / 2 / 3.1415926535897932 * 360) - offsetP
 
 
 
@@ -539,21 +535,30 @@ def float_to_hex(f):
     return ''.join([f'{byte:02x}' for byte in struct.pack('>f', f)])
 
 def decimalToHexSerial(Yaw, Pitch):
-    # 将Yaw和Pitch转换为IEEE 754标准的四字节浮点数表示，并转换为十六进制字符串
+    # ��Yaw��Pitchת��ΪIEEE 754��׼�����ֽڸ�������ʾ����ת��Ϊʮ�������ַ���
     # turn Yaw and Pitch to IEEE 754 standard four-byte floating point representation and convert to hexadecimal string
     hex_Yaw = float_to_hex(Yaw)
     hex_Pitch = float_to_hex(Pitch)
 
-    # 计算校验和
+    # �������
     # calculate checksum
     bytes_for_checksum = struct.pack('>ff', Yaw, Pitch) # only checked Yaw & Pitch data so far
     checksum = sum(bytes_for_checksum) % 256
     hex_checksum = f'{checksum:02x}'
 
-    # 构建十六进制数据列表
+    # ����ʮ�����������б�
     # build hexadecimal data list
     return hex_Yaw, hex_Pitch, hex_checksum
 
+def draw_crosshair(frame):
+    height, width = frame.shape[:2]
+    center_x, center_y = width // 2, height // 2
+    color = (0, 255, 0)  # Green color
+    thickness = 2
+    size = 20
+    cv2.line(frame, (center_x, center_y - size), (center_x, center_y + size), color, thickness)
+    cv2.line(frame, (center_x - size, center_y), (center_x + size, center_y), color, thickness)
+    return frame
 
 def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
     """
@@ -594,9 +599,10 @@ def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
     # Try to Open serial port for data transmission to STM32, if not found, continue without it
     try:
         ser = serial.Serial('/dev/ttyUSB0', 115200)
-    except:
+    except Exception as e:
         ser = None
-        print("Serial port not found, continuing without it")
+        print(f"Failed to open serial port: {str(e)}")
+
 
     detect_success = False
     track_success = False
@@ -610,6 +616,9 @@ def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
 
         color_image, depth_image = camera.get_frames()
 
+        # color_image with crosshair
+        color_image = draw_crosshair(color_image)
+
         """Do detection"""
         binary, frame = read_morphology(color_image, cv_config)
 
@@ -830,7 +839,7 @@ def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
                     cv2.FONT_HERSHEY_SIMPLEX, 0.5, [0, 255, 0])
 
 
-        if show_stream == 'YES':
+        if show_stream == 'YES' or 'yes':
             cv2.imshow("original", frame)
             cv2.waitKey(1)
         else:
@@ -839,7 +848,7 @@ def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
 
         endtime = time.time()
         fps = 1 / (endtime - startTime)
-        # print(fps)
+        print(fps)
 
 
 if __name__ == "__main__":
@@ -853,7 +862,7 @@ def main(camera: CameraSource, target_color: TargetColor, show_stream: str):
                         help='Path to record camera video to (MP4 format)')
     parser.add_argument('--debug', action='store_true',
                         help='Show intermediate results and debug output')
-    parser.add_argument('--show-stream', type=str, choices=['YES', 'NO'], default='NO',
+    parser.add_argument('--show-stream', type=str, choices=['YES', 'NO'], default='NO' or 'no',
                         help='Display the camera stream (YES or NO)')