156 lines
5.5 KiB
Python
156 lines
5.5 KiB
Python
import asyncio
|
||
import websockets
|
||
import json
|
||
import sys
|
||
|
||
WS_URL = "ws://127.0.0.1:8765"
|
||
SYSTEM_ID = 2
|
||
GOTO_POSITION = [24.1234567, 120.1234567]
|
||
|
||
|
||
class WebSocketClient:
|
||
def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
|
||
self.ws_url = ws_url
|
||
if isinstance(system_id, bool) or not isinstance(system_id, int):
|
||
raise ValueError("system_id 必須為整數")
|
||
self.system_id = system_id
|
||
|
||
def process_websocket_message(self, data):
|
||
"""解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
|
||
if isinstance(data, str):
|
||
try:
|
||
data = json.loads(data)
|
||
except json.JSONDecodeError as e:
|
||
print(f"JSON 格式錯誤: {e}")
|
||
return
|
||
|
||
if not isinstance(data, dict):
|
||
print(f"未知訊息格式: {data}")
|
||
return
|
||
|
||
if all(key in data for key in ("result", "command", "message")):
|
||
print(
|
||
f"ACK | Command:{data.get('command', '-')} | "
|
||
f"Result:{data.get('result', '-')} | "
|
||
f"Message:{data.get('message', '-')}"
|
||
)
|
||
return
|
||
|
||
if "s" not in data:
|
||
print(f"未知 JSON 訊息: {data}")
|
||
return
|
||
|
||
position = data.get("p")
|
||
if isinstance(position, (list, tuple)) and len(position) >= 2:
|
||
position_text = f"{position[0]}, {position[1]}"
|
||
else:
|
||
position_text = "-"
|
||
|
||
ypr = data.get("ypr")
|
||
if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
|
||
yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
|
||
else:
|
||
yaw, pitch, roll = data.get("y"), None, None
|
||
|
||
dop = data.get("d")
|
||
if isinstance(dop, (list, tuple)) and len(dop) >= 2:
|
||
dop_text = f"{dop[0]}/{dop[1]}"
|
||
else:
|
||
dop_text = "-"
|
||
|
||
armed = data.get("a")
|
||
if armed is None:
|
||
armed_text = "-"
|
||
else:
|
||
armed_text = "ARMED" if bool(armed) else "DISARMED"
|
||
|
||
def value_or_dash(value, suffix=""):
|
||
return f"{value}{suffix}" if value is not None else "-"
|
||
|
||
print(
|
||
f"UAV | ID:{data.get('s', '-')} | "
|
||
f"Mode:{value_or_dash(data.get('m'))} | "
|
||
f"Arm:{armed_text} | "
|
||
f"Battery:{value_or_dash(data.get('b'), '%')} | "
|
||
f"Position:{position_text} | "
|
||
f"Height:{value_or_dash(data.get('h'), 'm')} | "
|
||
f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
|
||
f"Yaw:{value_or_dash(yaw, 'deg')} | "
|
||
f"Pitch:{value_or_dash(pitch, 'deg')} | "
|
||
f"Roll:{value_or_dash(roll, 'deg')} | "
|
||
f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
|
||
)
|
||
|
||
async def send_commands_loop(self, websocket):
|
||
"""從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
|
||
loop = asyncio.get_event_loop()
|
||
print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
|
||
while True:
|
||
# readline 會包含換行
|
||
try:
|
||
line = await loop.run_in_executor(None, sys.stdin.readline)
|
||
except Exception:
|
||
return
|
||
if not line:
|
||
# EOF
|
||
return
|
||
cmd = line.strip()
|
||
if cmd == '1':
|
||
msg = {"s": self.system_id, "c": "START"}
|
||
elif cmd == '2':
|
||
msg = {"s": self.system_id, "c": "STOP"}
|
||
elif cmd == '3':
|
||
msg = {
|
||
"s": self.system_id,
|
||
"c": "GOTO",
|
||
"p": list(GOTO_POSITION),
|
||
}
|
||
else:
|
||
print(f"未知指令: {cmd}")
|
||
continue
|
||
|
||
try:
|
||
await websocket.send(json.dumps(msg))
|
||
print(f"已發送: {msg}")
|
||
except Exception as e:
|
||
print(f"發送失敗: {e}")
|
||
return
|
||
|
||
async def ws_client_loop(self):
|
||
retry_count = 0
|
||
max_retries = 5
|
||
base_delay = 1.0
|
||
while retry_count < max_retries:
|
||
try:
|
||
async with websockets.connect(self.ws_url) as websocket:
|
||
print(f"WebSocket connected: {self.ws_url}")
|
||
retry_count = 0
|
||
# 啟動後台任務來接收 stdin 指令並發送
|
||
send_task = asyncio.create_task(self.send_commands_loop(websocket))
|
||
try:
|
||
async for message in websocket:
|
||
# message 已經是 string
|
||
self.process_websocket_message(message)
|
||
finally:
|
||
send_task.cancel()
|
||
await asyncio.gather(send_task, return_exceptions=True)
|
||
|
||
except websockets.exceptions.ConnectionClosedError:
|
||
print("WebSocket connection closed")
|
||
break
|
||
except Exception as e:
|
||
retry_count += 1
|
||
delay = base_delay * (2 ** min(retry_count, 4))
|
||
print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
|
||
await asyncio.sleep(delay)
|
||
|
||
print("WebSocket client stopped after maximum retries")
|
||
|
||
async def start(self):
|
||
"""啟動 WebSocket 客戶端"""
|
||
await self.ws_client_loop()
|
||
|
||
if __name__ == "__main__":
|
||
client = WebSocketClient()
|
||
asyncio.run(client.start())
|