Upload files to 'src/unitdev01/unitdev01'
This commit is contained in:
+140
-49
@@ -41,48 +41,82 @@ class DroneMonitor(Node):
|
||||
# 定義需要過濾的模式
|
||||
self.filtered_modes = ['Mode(0x000000c0)']
|
||||
|
||||
# 啟動 WebSocket client 執行緒
|
||||
threading.Thread(target=self.start_ws_client, daemon=True).start()
|
||||
# 定義多個 WebSocket 連接配置
|
||||
self.websocket_connections = [
|
||||
{
|
||||
'name': 'local_1',
|
||||
'url': 'ws://192.168.137.1:5163',
|
||||
'enabled': True
|
||||
},
|
||||
{
|
||||
'name': 'local_2',
|
||||
'url': 'ws://0.0.0.0:8756',
|
||||
'enabled': True # 新增的端口
|
||||
},
|
||||
{
|
||||
'name': 'remote_8756',
|
||||
'url': 'ws://192.168.50.48:8756',
|
||||
'enabled': False # 可選擇啟用
|
||||
}
|
||||
]
|
||||
|
||||
# 啟動多個 WebSocket client 執行緒
|
||||
for connection in self.websocket_connections:
|
||||
if connection['enabled']:
|
||||
threading.Thread(
|
||||
target=self.start_ws_client,
|
||||
args=(connection,),
|
||||
daemon=True,
|
||||
name=f"WebSocket-{connection['name']}"
|
||||
).start()
|
||||
|
||||
# 主题检测定时器
|
||||
self.create_timer(1.0, self.scan_topics)
|
||||
|
||||
def start_ws_client(self):
|
||||
def start_ws_client(self, connection_config):
|
||||
"""啟動單個 WebSocket 客戶端"""
|
||||
asyncio.set_event_loop(asyncio.new_event_loop())
|
||||
asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
|
||||
asyncio.get_event_loop().run_until_complete(
|
||||
self.ws_client_loop(connection_config)
|
||||
)
|
||||
|
||||
async def ws_client_loop(self):
|
||||
async def ws_client_loop(self, connection_config):
|
||||
"""單個 WebSocket 連接的主循環"""
|
||||
retry_count = 0
|
||||
max_retries = 5
|
||||
base_delay = 1.0
|
||||
local = "ws://0.0.0.0:8765" # 本地 WebSocket 地址
|
||||
remote = "ws://192.168.50.48:8756"
|
||||
connection_name = connection_config['name']
|
||||
url = connection_config['url']
|
||||
|
||||
print(f"Starting WebSocket client for {connection_name} at {url}")
|
||||
|
||||
while retry_count < max_retries:
|
||||
try:
|
||||
async with websockets.connect(local) as websocket:
|
||||
print("WebSocket connected")
|
||||
async with websockets.connect(url) as websocket:
|
||||
print(f"WebSocket {connection_name} connected to {url}")
|
||||
retry_count = 0 # 重置重試計數
|
||||
|
||||
async for message in websocket:
|
||||
try:
|
||||
data = json.loads(message)
|
||||
# 添加連接來源信息
|
||||
data['_connection_source'] = connection_name
|
||||
self.process_websocket_message(data)
|
||||
except json.JSONDecodeError as e:
|
||||
print(f"WebSocket JSON decode error: {e}")
|
||||
print(f"WebSocket {connection_name} JSON decode error: {e}")
|
||||
except Exception as e:
|
||||
print(f"WebSocket message processing error: {e}")
|
||||
print(f"WebSocket {connection_name} message processing error: {e}")
|
||||
|
||||
except websockets.exceptions.ConnectionClosedError:
|
||||
print("WebSocket connection closed")
|
||||
print(f"WebSocket {connection_name} 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})")
|
||||
print(f"WebSocket {connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
|
||||
await asyncio.sleep(delay)
|
||||
|
||||
print("WebSocket client stopped after maximum retries")
|
||||
|
||||
print(f"WebSocket client {connection_name} stopped after maximum retries")
|
||||
|
||||
def process_websocket_message(self, data):
|
||||
try:
|
||||
@@ -446,7 +480,7 @@ class ControlStationUI(QMainWindow):
|
||||
scroll = QScrollArea()
|
||||
scroll.setWidget(self.drone_panel_container)
|
||||
scroll.setWidgetResizable(True)
|
||||
self.left_tab.addTab(scroll, "無人機")
|
||||
self.left_tab.addTab(scroll, "無人載具")
|
||||
|
||||
# 底部控制按鈕區域
|
||||
bottom_control = QWidget()
|
||||
@@ -594,35 +628,57 @@ class ControlStationUI(QMainWindow):
|
||||
<script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
|
||||
<style>
|
||||
html, body, #map { height: 100%; margin: 0; }
|
||||
.map-controls {
|
||||
position: absolute;
|
||||
top: 10px;
|
||||
right: 10px;
|
||||
z-index: 1000;
|
||||
}
|
||||
.control-button {
|
||||
padding: 8px 12px;
|
||||
background-color: #f44336;
|
||||
color: white;
|
||||
border: none;
|
||||
border-radius: 4px;
|
||||
cursor: pointer;
|
||||
font-size: 12px;
|
||||
box-shadow: 0 2px 5px rgba(0,0,0,0.2);
|
||||
}
|
||||
.control-button:hover {
|
||||
background-color: #d32f2f;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div id="map"></div>
|
||||
<div class="map-controls">
|
||||
<button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
var map = L.map('map').setView([0, 0], 20);
|
||||
var map = L.map('map').setView([0, 0], 19);
|
||||
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
|
||||
maxZoom: 19, // OpenStreetMap 支持的最大縮放級別
|
||||
maxZoom: 19,
|
||||
attribution: '© OpenStreetMap contributors'
|
||||
}).addTo(map);
|
||||
|
||||
var arrowIcon = L.icon({
|
||||
iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png',
|
||||
iconSize: [40, 40],
|
||||
iconSize: [40, 40],
|
||||
iconAnchor: [20, 20]
|
||||
});
|
||||
|
||||
function getColorBySocketId(id) {
|
||||
if (id.startsWith("s0_")) return "#00BFFF"; // DeepSkyBlue
|
||||
if (id.startsWith("s1_")) return "#FFD700"; // Gold
|
||||
if (id.startsWith("s2_")) return "#FF69B4"; // HotPink
|
||||
if (id.startsWith("s9_")) return "#7CFC00"; // LawnGreen
|
||||
return "#AAAAAA"; // Default
|
||||
if (id.startsWith("s0_")) return "#00BFFF";
|
||||
if (id.startsWith("s1_")) return "#FFD700";
|
||||
if (id.startsWith("s2_")) return "#FF69B4";
|
||||
if (id.startsWith("s9_")) return "#7CFC00";
|
||||
return "#AAAAAA";
|
||||
}
|
||||
|
||||
// 新增 ID 標籤的圖標
|
||||
function createIdIcon(id) {
|
||||
const color = getColorBySocketId(id);
|
||||
const sysid = id.split('_')[1]; // e.g., 's3_2' → '2'
|
||||
const sysid = id.split('_')[1];
|
||||
return L.divIcon({
|
||||
className: 'drone-id',
|
||||
html: `<div style="
|
||||
@@ -644,12 +700,39 @@ class ControlStationUI(QMainWindow):
|
||||
});
|
||||
}
|
||||
|
||||
var markers = {}; // 儲存所有無人機
|
||||
var idLabels = {}; // 新增：儲存 ID 標籤
|
||||
var focusedId = null; // 目前被鎖定的 sysid
|
||||
var initialized = false; // 是否完成首次初始化
|
||||
var markers = {};
|
||||
var idLabels = {};
|
||||
var trajectories = {};
|
||||
var trajectoryLines = {};
|
||||
var focusedId = null;
|
||||
var initialized = false;
|
||||
var trajectoryGroup = L.layerGroup().addTo(map);
|
||||
|
||||
function initTrajectory(id) {
|
||||
if (!trajectories[id]) {
|
||||
trajectories[id] = [];
|
||||
const color = getColorBySocketId(id);
|
||||
trajectoryLines[id] = L.polyline([], {
|
||||
color: color,
|
||||
weight: 3,
|
||||
opacity: 0.7,
|
||||
smoothFactor: 1
|
||||
}).addTo(trajectoryGroup);
|
||||
}
|
||||
}
|
||||
|
||||
function addTrajectoryPoint(id, lat, lon) {
|
||||
initTrajectory(id);
|
||||
const point = [lat, lon];
|
||||
trajectories[id].push(point);
|
||||
|
||||
if (trajectories[id].length > 1000) {
|
||||
trajectories[id].shift();
|
||||
}
|
||||
|
||||
trajectoryLines[id].setLatLngs([...trajectories[id]]);
|
||||
}
|
||||
|
||||
// ⬆️ 鎖定指定無人機（置中地圖並彈出名稱）
|
||||
function focusOn(id) {
|
||||
if (!markers[id]) return;
|
||||
focusedId = id;
|
||||
@@ -657,23 +740,29 @@ class ControlStationUI(QMainWindow):
|
||||
map.flyTo(latlng, map.getZoom());
|
||||
}
|
||||
|
||||
// 🔁 定期重新鎖定 focusedId
|
||||
setInterval(() => {
|
||||
if (focusedId && markers[focusedId]) {
|
||||
var latlng = markers[focusedId].getLatLng();
|
||||
map.panTo(latlng); // 或 flyTo / setView
|
||||
map.panTo(latlng);
|
||||
}
|
||||
}, 1000); // 每秒更新一次視角
|
||||
}, 1000);
|
||||
|
||||
// 📡 更新／新增無人機 marker
|
||||
function updateDrone(lat, lon, id, heading) {
|
||||
if (markers[id]) {
|
||||
const lastPos = markers[id].getLatLng();
|
||||
const distance = lastPos.distanceTo([lat, lon]);
|
||||
if (distance > 1) {
|
||||
addTrajectoryPoint(id, lat, lon);
|
||||
}
|
||||
|
||||
markers[id]
|
||||
.setLatLng([lat, lon])
|
||||
.setRotationAngle(heading);
|
||||
// 更新 ID 標籤位置
|
||||
idLabels[id].setLatLng([lat, lon]);
|
||||
} else {
|
||||
initTrajectory(id);
|
||||
addTrajectoryPoint(id, lat, lon);
|
||||
|
||||
markers[id] = L.marker([lat, lon], {
|
||||
icon: arrowIcon,
|
||||
rotationAngle: heading,
|
||||
@@ -682,28 +771,32 @@ class ControlStationUI(QMainWindow):
|
||||
.on('click', function () {
|
||||
focusOn(id);
|
||||
})
|
||||
.addTo(map)
|
||||
.addTo(map);
|
||||
|
||||
// 新增 ID 標籤
|
||||
idLabels[id] = L.marker([lat, lon], {
|
||||
icon: createIdIcon(id),
|
||||
zIndexOffset: 1000 // 確保 ID 標籤在箭頭上方
|
||||
zIndexOffset: 1000
|
||||
})
|
||||
.on('click', function() {
|
||||
focusOn(id);
|
||||
focusOn(id);
|
||||
})
|
||||
.addTo(map);
|
||||
|
||||
// 🧭 第一次加入 → 若未初始化，則以 sysid 最小的初始化
|
||||
if (!initialized || id < focusedId) {
|
||||
focusOn(id);
|
||||
markers[id]
|
||||
.setLatLng([lat, lon])
|
||||
.setRotationAngle(heading);
|
||||
initialized = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function clearAllTrajectories() {
|
||||
trajectories = {};
|
||||
Object.values(trajectoryLines).forEach(line => {
|
||||
trajectoryGroup.removeLayer(line);
|
||||
});
|
||||
trajectoryLines = {};
|
||||
console.log('所有軌跡已清除');
|
||||
}
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -1167,18 +1260,16 @@ class ControlStationUI(QMainWindow):
|
||||
elif msg_type == 'battery':
|
||||
voltage = data.get('voltage', 16)
|
||||
|
||||
# 使用標準電壓判斷電池節數
|
||||
cell_max = 4.2
|
||||
# 判斷電池節數
|
||||
cells = round(voltage / 3.95)
|
||||
max = cell_max * cells
|
||||
|
||||
# 計算電量百分比
|
||||
percentage = voltage / max * 100
|
||||
percentage = (voltage / cells - 3.7) / 0.5 * 100
|
||||
|
||||
# 根據百分比設置顏色
|
||||
if percentage < 20:
|
||||
voltage_color = '#FF6464' # 紅色 (低電量)
|
||||
elif percentage < 40:
|
||||
elif percentage < 50:
|
||||
voltage_color = '#FFA500' # 橘色 (中低電量)
|
||||
else:
|
||||
voltage_color = '#FFFFFF' # 白色 (正常電量)
|
||||
|
||||
Reference in New Issue
Block a user