diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 125b45b..84e89a9 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/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, diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 2602e57..dd9515b 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/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 作為目標端口 的列表 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index cc53edb..89ab179 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/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) @@ -224,6 +227,42 @@ class VehicleStatusPublisher(Node): msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus 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)""" @@ -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'): diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index b99ecd3..d8309aa 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/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.