diff --git a/.gitignore b/.gitignore index ab42e47..f247176 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,10 @@ Makefile **/*.pyo logs/ src/logs/ + +# 個人環境筆記 +my_env_notes.md + +# Claude Code 本地設定 +CLAUDE.md +.claude/ diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py deleted file mode 100644 index 22b6e22..0000000 --- a/src/GUI/validation/test_mission.py +++ /dev/null @@ -1,299 +0,0 @@ -#!/usr/bin/env python3 -""" -獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作 - -使用方式: - 1. 啟動 SITL - 2. 修改下方 CONFIG 區塊 - 3. python3 test_mission.py -""" -import sys, os -sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) - -import time -from pymavlink import mavutil -from PyQt6.QtWidgets import QApplication -from PyQt6.QtCore import QTimer - -from mission_planner import FormationPlanner, MissionType -from command_sender import MavlinkSender -from mission_executor import MissionExecutor, MissionState - - -# ================================================================================ -# CONFIG -# ================================================================================ - -# 接收用連線 (讀取無人機狀態) -RECV_CONNECTION = "udp:127.0.0.1:14550" - -# 發送用連線 (發送 setpoint 指令) -SEND_CONNECTION = "udpout:127.0.0.1:14550" - -# 要控制的無人機 sysid 列表 -DRONE_SYSIDS = [1] - -# 起飛高度 (公尺) -TAKEOFF_ALT = 10.0 - -# 任務規劃參數 -FORMATION_SPACING = 5.0 -BASE_ALTITUDE = 10.0 -ALTITUDE_DIFF = 2.0 -ARRIVAL_RADIUS = 2.0 - -# 測試模式: "formation" 或 "grid_sweep" -TEST_MODE = "formation" - -# Grid Sweep 專用設定 -GRID_LINE_SPACING = 5.0 - -# ================================================================================ - - -class SITLDroneManager: - """管理 SITL 無人機的連線、起飛前置作業""" - - def __init__(self, connection_string, sysids): - self.connection_string = connection_string - self.sysids = sysids - self.mav = None - self.drone_gps = {} - - def connect(self): - """建立 MAVLink 連線並等待心跳""" - print(f"連線到 {self.connection_string} ...") - self.mav = mavutil.mavlink_connection(self.connection_string) - self.mav.wait_heartbeat() - print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}") - - def set_guided_and_arm(self, sysid): - """切換到 GUIDED 模式並解鎖""" - print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---") - - # 切換 GUIDED 模式 - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_DO_SET_MODE, - 0, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - 4, # GUIDED = 4 - 0, 0, 0, 0, 0 - ) - time.sleep(1) - - # 解鎖 - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, 1, 0, 0, 0, 0, 0, 0 - ) - time.sleep(1) - print(f" sysid={sysid}: GUIDED + ARMED") - - def takeoff(self, sysid, altitude): - """起飛到指定高度""" - print(f" sysid={sysid}: 起飛到 {altitude}m ...") - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, 0, 0, 0, 0, 0, 0, altitude - ) - - def wait_for_altitude(self, sysid, target_alt, timeout=30): - """等待無人機到達指定高度""" - print(f" sysid={sysid}: 等待到達 {target_alt}m ...") - start = time.time() - while time.time() - start < timeout: - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if msg and msg.get_srcSystem() == sysid: - alt = msg.relative_alt / 1000.0 - if alt >= target_alt * 0.9: - print(f" sysid={sysid}: 已到達 {alt:.1f}m") - return True - print(f" sysid={sysid}: 等待超時!") - return False - - def update_gps_once(self): - """讀取一輪 GPS 資料更新 drone_gps""" - deadline = time.time() + 3 - received = set() - while time.time() < deadline and len(received) < len(self.sysids): - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if msg is None: - continue - sid = msg.get_srcSystem() - if sid in self.sysids: - drone_id = f"s0_{sid}" - self.drone_gps[drone_id] = { - 'lat': msg.lat / 1e7, - 'lon': msg.lon / 1e7, - 'alt': msg.relative_alt / 1000.0 - } - received.add(sid) - - for sid in self.sysids: - drone_id = f"s0_{sid}" - if drone_id in self.drone_gps: - gps = self.drone_gps[drone_id] - print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)") - else: - print(f" {drone_id}: 尚未收到 GPS") - - def start_gps_polling(self, interval_ms=200): - """啟動定時 GPS 輪詢 (用 QTimer)""" - self._gps_timer = QTimer() - self._gps_timer.timeout.connect(self._poll_gps) - self._gps_timer.start(interval_ms) - - def _poll_gps(self): - """非阻塞方式讀取最新 GPS""" - while True: - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False) - if msg is None: - break - sid = msg.get_srcSystem() - if sid in self.sysids: - drone_id = f"s0_{sid}" - self.drone_gps[drone_id] = { - 'lat': msg.lat / 1e7, - 'lon': msg.lon / 1e7, - 'alt': msg.relative_alt / 1000.0 - } - - -def main(): - # 建立 Qt 應用 (MissionExecutor 需要 QTimer) - app = QApplication(sys.argv) - - # 連線 + 起飛前置作業 - manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS) - manager.connect() - - for sysid in DRONE_SYSIDS: - manager.set_guided_and_arm(sysid) - manager.takeoff(sysid, TAKEOFF_ALT) - - # 等待所有無人機到達起飛高度 - for sysid in DRONE_SYSIDS: - manager.wait_for_altitude(sysid, TAKEOFF_ALT) - - time.sleep(2) - - # 讀取當前 GPS 位置 - print("\n讀取當前 GPS 位置 ...") - manager.update_gps_once() - - drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS] - drone_gps_positions = [] - for drone_id in drone_ids: - gps = manager.drone_gps.get(drone_id) - if gps is None: - print(f"錯誤: 讀不到 {drone_id} 的 GPS") - return - drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt'])) - - # 規劃任務 - print(f"\n規劃任務 (模式: {TEST_MODE}) ...") - planner = FormationPlanner( - spacing=FORMATION_SPACING, - base_altitude=BASE_ALTITUDE, - altitude_diff=ALTITUDE_DIFF - ) - - center_lat = drone_gps_positions[0][0] - center_lon = drone_gps_positions[0][1] - - if TEST_MODE == "formation": - target_lat = center_lat + 30.0 / 111000.0 - target_lon = center_lon - target_gps = (target_lat, target_lon, BASE_ALTITUDE) - print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})") - - waypoints_per_drone, origin = planner.plan_formation_mission( - drone_gps_positions, target_gps, MissionType.M_FORMATION - ) - - elif TEST_MODE == "grid_sweep": - # 在無人機前方 30m 處建立 40m x 30m 的矩形 - offset_lat = 30.0 / 111000.0 - half_w = 20.0 / (111000.0 * 0.9) - half_h = 15.0 / 111000.0 - - rect_center_lat = center_lat + offset_lat - rect_center_lon = center_lon - - rect_corners = [ - (rect_center_lat - half_h, rect_center_lon - half_w), - (rect_center_lat - half_h, rect_center_lon + half_w), - (rect_center_lat + half_h, rect_center_lon + half_w), - (rect_center_lat + half_h, rect_center_lon - half_w), - ] - target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE) - print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})") - - waypoints_per_drone, origin = planner.plan_formation_mission( - drone_gps_positions, target_gps, MissionType.GRID_SWEEP, - params={ - 'rect_corners': rect_corners, - 'line_spacing': GRID_LINE_SPACING, - 'altitude': BASE_ALTITUDE - } - ) - else: - print(f"未知測試模式: {TEST_MODE}") - return - - planned_waypoints = { - 'drone_ids': drone_ids, - 'waypoints': waypoints_per_drone - } - - # 印出規劃結果 - for i, did in enumerate(drone_ids): - wps = waypoints_per_drone[i] - print(f" {did}: {len(wps)} 個航點") - for j, wp in enumerate(wps): - print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)") - - # 啟動任務 - print("\n啟動任務 ...") - manager.start_gps_polling(interval_ms=200) - - sender = MavlinkSender(SEND_CONNECTION) - executor = MissionExecutor( - sender=sender, - drone_gps=manager.drone_gps, - arrival_radius=ARRIVAL_RADIUS, - send_rate_hz=2.0 - ) - - executor.drone_waypoint_reached.connect( - lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}") - ) - executor.mission_completed.connect( - lambda: (print("\n===== 任務全部完成 ====="), app.quit()) - ) - - # 設定超時自動退出 - timeout_timer = QTimer() - timeout_timer.setSingleShot(True) - timeout_timer.timeout.connect(lambda: ( - print("\n⚠ 測試超時，強制退出"), - executor.stop(), - app.quit() - )) - timeout_timer.start(180_000) # 180 秒超時 - - executor.start(planned_waypoints) - - print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n") - app.exec() - - executor.stop() - sender.close() - print("測試結束") - - -if __name__ == "__main__": - main() \ No newline at end of file