You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
237 lines
8.1 KiB
Python
237 lines
8.1 KiB
Python
import json
|
|
import math
|
|
import socket
|
|
import struct
|
|
import time
|
|
|
|
from config import *
|
|
from helpers import clamp
|
|
|
|
|
|
UNIT_CODES = {"norm": 0, "px": 1, "deg": 2, "m": 3}
|
|
GUIDANCE_V1_REQUEST = 1
|
|
GUIDANCE_V1_RESPONSE = 2
|
|
GUIDANCE_V1_STRUCT = struct.Struct("<BBBhhbbb")
|
|
|
|
|
|
def build_error_payload(
|
|
state,
|
|
*,
|
|
units="px",
|
|
hfov_deg=90.0,
|
|
vfov_deg=60.0,
|
|
range_m=0.0,
|
|
object_id=1,
|
|
timestamp=None,
|
|
):
|
|
frame_w = max(1.0, float(state.get("frame_w") or 1.0))
|
|
frame_h = max(1.0, float(state.get("frame_h") or 1.0))
|
|
x_norm = float(clamp(float(state.get("error_x") or 0.0), -1.0, 1.0))
|
|
y_norm = float(clamp(float(state.get("error_y") or 0.0), -1.0, 1.0))
|
|
x_px = x_norm * 0.5 * frame_w
|
|
y_px = y_norm * 0.5 * frame_h
|
|
|
|
hfov_rad = math.radians(float(hfov_deg))
|
|
vfov_rad = math.radians(float(vfov_deg))
|
|
x_rad = math.atan(math.tan(0.5 * hfov_rad) * x_norm)
|
|
y_rad = math.atan(math.tan(0.5 * vfov_rad) * y_norm)
|
|
x_deg = math.degrees(x_rad)
|
|
y_deg = math.degrees(y_rad)
|
|
|
|
distance_m = float(range_m or 0.0)
|
|
meter_valid = distance_m > 0.0
|
|
x_m = math.tan(x_rad) * distance_m if meter_valid else 0.0
|
|
y_m = math.tan(y_rad) * distance_m if meter_valid else 0.0
|
|
det_count = max(0, int(state.get("det_count") or 0))
|
|
active = bool(state.get("active", False))
|
|
if not active:
|
|
target_state = 0
|
|
elif det_count > 1:
|
|
target_state = 2
|
|
elif det_count == 1:
|
|
target_state = 1
|
|
else:
|
|
target_state = 3
|
|
|
|
box_w = max(0.0, float(state.get("box_w") or 0.0))
|
|
box_h = max(0.0, float(state.get("box_h") or 0.0))
|
|
box_area_percent = int(clamp(round(100.0 * box_w * box_h / (frame_w * frame_h)), 0, 100))
|
|
|
|
units = str(units or "px").lower()
|
|
if units == "norm":
|
|
x, y, valid = x_norm, y_norm, True
|
|
elif units == "deg":
|
|
x, y, valid = x_deg, y_deg, True
|
|
elif units == "m":
|
|
x, y, valid = x_m, y_m, meter_valid
|
|
else:
|
|
units = "px"
|
|
x, y, valid = x_px, y_px, True
|
|
|
|
return {
|
|
"type": "fpv_error",
|
|
"timestamp": time.time() if timestamp is None else float(timestamp),
|
|
"frame_id": int(state.get("frame_id") or 0),
|
|
"active": active,
|
|
"status": str(state.get("status") or "SEARCH"),
|
|
"target_id": state.get("target_id"),
|
|
"object_id": int(clamp(int(object_id), 1, 255)),
|
|
"target_state": target_state,
|
|
"det_count": det_count,
|
|
"box_area_percent": box_area_percent,
|
|
"confidence": float(state.get("confidence") or 0.0),
|
|
"unit": units,
|
|
"valid": bool(valid),
|
|
"x": float(x),
|
|
"y": float(y),
|
|
"mag": float(math.hypot(float(x), float(y))),
|
|
"x_norm": float(x_norm),
|
|
"y_norm": float(y_norm),
|
|
"x_px": float(x_px),
|
|
"y_px": float(y_px),
|
|
"x_deg": float(x_deg),
|
|
"y_deg": float(y_deg),
|
|
"x_m": float(x_m),
|
|
"y_m": float(y_m),
|
|
"range_m": distance_m if meter_valid else None,
|
|
"frame_w": int(frame_w),
|
|
"frame_h": int(frame_h),
|
|
}
|
|
|
|
|
|
def encode_error_payload(payload, protocol):
|
|
protocol = str(protocol or "json").lower()
|
|
if protocol == "guidance_v1":
|
|
if int(payload["target_state"]) == 0:
|
|
vertical_px = horizontal_px = vertical_percent = horizontal_percent = box_percent = 0
|
|
else:
|
|
vertical_px = int(clamp(round(-float(payload["y_px"])), -32768, 32767))
|
|
horizontal_px = int(clamp(round(float(payload["x_px"])), -32768, 32767))
|
|
vertical_percent = int(clamp(round(-100.0 * float(payload["y_norm"])), -100, 100))
|
|
horizontal_percent = int(clamp(round(100.0 * float(payload["x_norm"])), -100, 100))
|
|
box_percent = int(clamp(int(payload["box_area_percent"]), 0, 100))
|
|
return GUIDANCE_V1_STRUCT.pack(
|
|
GUIDANCE_V1_REQUEST,
|
|
int(payload["object_id"]),
|
|
int(payload["target_state"]),
|
|
vertical_px,
|
|
horizontal_px,
|
|
vertical_percent,
|
|
horizontal_percent,
|
|
box_percent,
|
|
)
|
|
if protocol == "csv":
|
|
values = [
|
|
payload["frame_id"],
|
|
f"{payload['timestamp']:.6f}",
|
|
int(payload["active"]),
|
|
int(payload["valid"]),
|
|
payload["unit"],
|
|
f"{payload['x']:.6f}",
|
|
f"{payload['y']:.6f}",
|
|
f"{payload['mag']:.6f}",
|
|
f"{payload['confidence']:.6f}",
|
|
payload["status"],
|
|
"" if payload["target_id"] is None else payload["target_id"],
|
|
]
|
|
return (",".join(map(str, values)) + "\n").encode("ascii", errors="replace")
|
|
if protocol == "bin":
|
|
target_id = int(payload["target_id"] or 0)
|
|
return struct.pack(
|
|
"<4sIdBBffffi",
|
|
b"FPVE",
|
|
int(payload["frame_id"]),
|
|
float(payload["timestamp"]),
|
|
1 if payload["active"] else 0,
|
|
UNIT_CODES.get(payload["unit"], 1),
|
|
float(payload["x"]),
|
|
float(payload["y"]),
|
|
float(payload["mag"]),
|
|
float(payload["confidence"]),
|
|
target_id,
|
|
)
|
|
return json.dumps(payload, ensure_ascii=True, separators=(",", ":")).encode("utf-8")
|
|
|
|
|
|
def decode_guidance_v1_response(data):
|
|
if len(data) != 2 or data[0] != GUIDANCE_V1_RESPONSE or data[1] not in (0, 1, 2):
|
|
return None
|
|
return {"descriptor": data[0], "response": data[1]}
|
|
|
|
|
|
class ErrorOutputSender:
|
|
def __init__(self):
|
|
self.enabled = bool(ERROR_OUTPUT_ENABLE)
|
|
self.protocol = str(ERROR_OUTPUT_PROTOCOL).lower().strip()
|
|
self.units = str(ERROR_OUTPUT_UNITS).lower().strip()
|
|
self.host = str(ERROR_OUTPUT_HOST)
|
|
self.port = int(ERROR_OUTPUT_PORT)
|
|
self.every = max(1, int(ERROR_OUTPUT_EVERY))
|
|
self.hfov_deg = float(ERROR_OUTPUT_HFOV_DEG)
|
|
self.vfov_deg = float(ERROR_OUTPUT_VFOV_DEG)
|
|
self.range_m = float(ERROR_OUTPUT_RANGE_M)
|
|
self.object_id = int(clamp(int(ERROR_OUTPUT_OBJECT_ID), 1, 255))
|
|
self._sock = None
|
|
self._last_response = None
|
|
|
|
def start(self):
|
|
if not self.enabled:
|
|
return
|
|
try:
|
|
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
self._sock.setblocking(False)
|
|
except OSError as exc:
|
|
self._sock = None
|
|
print(f"[error-output] socket failed: {exc}", flush=True)
|
|
|
|
def close(self):
|
|
if self._sock is not None:
|
|
try:
|
|
self._sock.close()
|
|
except OSError:
|
|
pass
|
|
self._sock = None
|
|
|
|
def status_line(self):
|
|
if not self.enabled:
|
|
return "Error output disabled"
|
|
object_text = f" object={self.object_id}" if self.protocol == "guidance_v1" else ""
|
|
return f"Error output UDP {self.protocol}: {self.host}:{self.port} units={self.units}{object_text}"
|
|
|
|
def _poll_response(self):
|
|
if self.protocol != "guidance_v1" or self._sock is None:
|
|
return
|
|
while True:
|
|
try:
|
|
data, _address = self._sock.recvfrom(64)
|
|
except BlockingIOError:
|
|
return
|
|
except OSError:
|
|
return
|
|
response = decode_guidance_v1_response(data)
|
|
if response is not None and response["response"] != self._last_response:
|
|
self._last_response = response["response"]
|
|
print(f"[error-output] guidance_v1 response={self._last_response}", flush=True)
|
|
|
|
def send(self, state):
|
|
if (not self.enabled) or self._sock is None or (not state.get("active", False)):
|
|
return None
|
|
frame_id = int(state.get("frame_id") or 0)
|
|
if frame_id % self.every != 0:
|
|
return None
|
|
payload = build_error_payload(
|
|
state,
|
|
units=self.units,
|
|
hfov_deg=self.hfov_deg,
|
|
vfov_deg=self.vfov_deg,
|
|
range_m=self.range_m,
|
|
object_id=self.object_id,
|
|
)
|
|
data = encode_error_payload(payload, self.protocol)
|
|
try:
|
|
self._sock.sendto(data, (self.host, self.port))
|
|
except OSError:
|
|
pass
|
|
self._poll_response()
|
|
return payload
|