Upload files to 'src/unitdev01/unitdev01'

src/unitdev01/unitdev01/gui.py

@@ -191,6 +191,8 @@ class ControlStationUI(QMainWindow):
         self.executor.add_node(self.monitor)
         
         # 初始化UI
+        self.drone_positions = {}
+        self.map_loaded = False
         self.init_ui()
         
         # 定时处理ROS事件
@@ -221,23 +223,48 @@ class ControlStationUI(QMainWindow):
 
         inline_html = '''
             <!DOCTYPE html>
-        <html><head>
+            <html>
+            <head>
             <meta charset="utf-8" />
             <link rel="stylesheet" href="https://unpkg.com/leaflet/dist/leaflet.css" />
             <script src="https://unpkg.com/leaflet/dist/leaflet.js"></script>
-          <style>html,body,#map{height:100%;margin:0}</style>
-        </head><body>
+            <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
+            <style>
+                html, body, #map { height: 100%; margin: 0; }
+            </style>
+            </head>
+            <body>
             <div id="map"></div>
             <script>
                 var map = L.map('map').setView([0, 0], 2);
-            L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png',{maxZoom:19}).addTo(map);
+                L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+                maxZoom: 19
+                }).addTo(map);
+
+                var arrowIcon = L.icon({
+                iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png', // 朝上的箭頭
+                iconSize: [40, 40],
+                iconAnchor: [20, 20]
+                });
+
                 var markers = {};
-            function updateDrone(lat,lon,id){
-              if(markers[id]) markers[id].setLatLng([lat,lon]);
-              else markers[id]=L.marker([lat,lon]).addTo(map).bindPopup(id);
+
+                function updateDrone(lat, lon, id, heading) {
+                if (markers[id]) {
+                    markers[id].setLatLng([lat, lon]);
+                    markers[id].setRotationAngle(heading);
+                } else {
+                    markers[id] = L.marker([lat, lon], {
+                    icon: arrowIcon,
+                    rotationAngle: heading,
+                    rotationOrigin: 'center'
+                    }).addTo(map).bindPopup(id);
+                }
+                map.setView([lat, lon], 16);
                 }
             </script>
-        </body></html>
+            </body>
+            </html>
         '''
         self.map_view.setHtml(inline_html)
         self.map_view.loadFinished.connect(self.on_map_loaded)
@@ -349,12 +376,11 @@ class ControlStationUI(QMainWindow):
                             '#FF6464' if voltage < 12 else '#FFFFFF')
         
         elif msg_type == 'gps':
-            text = (f"緯度: {data['lat']:.6f}°\n"
-                    f"經度: {data['lon']:.6f}°")
+            lat, lon = data['lat'], data['lon']
+            self.drone_positions[drone_id] = (lat, lon)
+            text = (f"緯度: {lat:.6f}°\n"
+                    f"經度: {lon:.6f}°")
             self.update_field(panel, drone_id, 'gps', text)
-            if self.on_map_loaded:
-                js = f"updateDrone({data['lat']}, {data['lon']:.6f}, {drone_id})"
-                self.map_view.page().runJavaScript(js)
 
         elif msg_type == 'altitude':
             text = (f"{data['altitude']:.1f} m")
@@ -365,9 +391,15 @@ class ControlStationUI(QMainWindow):
             self.update_field(panel, drone_id, 'local', text)
         
         elif msg_type == 'hud':
+            heading = data['heading']
             text = (f"地速: {data['groundspeed']:.1f} m/s\n"
-                    f"航向: {data['heading']:.1f}°")
+                    f"航向: {heading:.1f}°")
             self.update_field(panel, drone_id, 'hud', text)
+
+            if self.map_loaded and drone_id in self.drone_positions:
+                lat, lon = self.drone_positions[drone_id]
+                js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});"
+                self.map_view.page().runJavaScript(js)
     '''
         elif msg_type == 'velocity':
             text = (f"VX: {data['vx']:.1f} m/s\n"