1. (update) mavlinkROS2Nodes 新增 local_position 資訊

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -1141,6 +1141,7 @@ class Orchestrator:
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
             'position': 1.0,
+            'position_ned': 1.0,
             'attitude': 1.0,
             'velocity': 0.0,
             'battery': 1.0,

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -385,7 +385,7 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -27,6 +27,7 @@ from rclpy.executors import MultiThreadedExecutor
 import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
+import nav_msgs.msg
 import mavros_msgs.msg
 
 # ROS2 Service imports
@@ -59,13 +60,14 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.0,       # GPS位置
-            'attitude': 0.0,       # 姿態
-            'velocity': 0.0,       # 速度
+            'position': 0.0,       # GNSS位置
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
             'battery': 0.0,        # 電池
-            'vfr_hud': 0.0,        # VFR HUD
-            'mode': 0.0,           # 飛行模式
-            'summary': 0.0,        # 載具摘要
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -169,6 +171,7 @@ class VehicleStatusPublisher(Node):
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
         self._publish_position(sysid, status)
+        self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
@@ -225,6 +228,42 @@ class VehicleStatusPublisher(Node):
         
         publisher.publish(msg)
 
+    def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+        if not self.rate_controller.should_publish(sysid, 'position_ned'):
+            return
+
+        local_pos = status.custom_status.get('local_position')
+        if not local_pos:
+            return
+
+        required_keys = ('x', 'y', 'z', 'vx', 'vy', 'vz')
+        if any(local_pos.get(k) is None for k in required_keys):
+            return
+
+        publisher = self._get_or_create_publisher(
+            sysid, 'position_ned', nav_msgs.msg.Odometry
+        )
+
+        if publisher.get_subscription_count() == 0:
+            return
+
+        msg = nav_msgs.msg.Odometry()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'local_ned'
+        msg.child_frame_id = 'base_link_ned'
+
+        # 保持 MAVLink LOCAL_POSITION_NED 座標定義，不進行 ENU 轉換
+        msg.pose.pose.position.x = float(local_pos['x'])
+        msg.pose.pose.position.y = float(local_pos['y'])
+        msg.pose.pose.position.z = float(local_pos['z'])
+
+        msg.twist.twist.linear.x = float(local_pos['vx'])
+        msg.twist.twist.linear.y = float(local_pos['vy'])
+        msg.twist.twist.linear.z = float(local_pos['vz'])
+
+        publisher.publish(msg)
+    
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
         """發布姿態（IMU）"""
         if not self.rate_controller.should_publish(sysid, 'attitude'):
@@ -257,7 +296,6 @@ class VehicleStatusPublisher(Node):
         
         publisher.publish(msg)
     
-    # TODO 這邊要改成 XY 的位置與速度
     def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
         """發布速度"""
         if not self.rate_controller.should_publish(sysid, 'velocity'):

src/unitdev02/unitdev02/devnote.txt

@@ -28,7 +28,7 @@ python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
 ros2 topic list
-ros2 topic echo 
+ros2 topic echo /fc_network/vehicle/sys3/battery
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
@@ -52,3 +52,6 @@ sudo tcpdump -i lo -X udp port 14550
 1. 專案文件中 .mat 檔案是幹嘛的?
 2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
 3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.