wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

1. (modify) 更新 ros2 對於 GNSS topic 的發佈, 使其包涵 epv eph 衛星數與 fix_type

Browse Source 
2. (adding) 新增相應的 msg 檔案與修改 CMakeLists, package
3. (adding) test_vehicleStatusPublisher 相對應修正
This commit is contained in:
chiyu1468
2026-05-13 18:14:47 +08:00
parent d2a5f6fad7
commit c8f070bfb9
7 changed files with 50 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_interfaces/CMakeLists.txt
+3 -1
View File
@@ -9,13 +9,15 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AttitudeRaw.msg"
"msg/GnssRaw.msg"
"srv/MavPing.srv"
"srv/MavCommandLong.srv"
"srv/MavPositionTargetGlobalInt.srv"
DEPENDENCIES std_msgs
DEPENDENCIES std_msgs builtin_interfaces
)
ament_package()
src/fc_interfaces/msg/GnssRaw.msg
+8
View File
@@ -0,0 +1,8 @@
builtin_interfaces/Time stamp
float64 latitude
float64 longitude
float64 altitude
uint8 fix_type
uint8 satellites_visible
float32 eph
float32 epv
src/fc_interfaces/package.xml
+2
View File
@@ -17,6 +17,8 @@
<build_depend>std_msgs</build_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>builtin_interfaces</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<export>
<build_type>ament_cmake</build_type>
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+1 -1
View File
@@ -1145,7 +1145,7 @@ class Orchestrator:
self.fc_ros_manager = mros.ros2_manager
self.fc_ros_manager.initialize()
self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
'position': 1.0,
'position_gnss': 1.0,
'position_ned': 1.0,
'attitude': 1.0,
'velocity': 0.0,
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+12 -1
View File
@@ -109,6 +109,7 @@ class mavlink_bridge:
"""初始化訊息處理器映射表，提高處理效率"""
self.message_handlers = {
0: self._handle_heartbeat, # HEARTBEAT
24: self._handle_gps_raw_int, # GPS_RAW_INT
30: self._handle_attitude, # ATTITUDE
32: self._handle_local_position, # LOCAL_POSITION_NED
33: self._handle_global_position, # GLOBAL_POSITION_INT
@@ -239,6 +240,14 @@ class mavlink_bridge:
component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺
component.status.position.timestamp = timestamp
def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
"""處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
component.status.gps.fix_type = msg.fix_type
component.status.gps.satellites_visible = msg.satellites_visible
component.status.gps.eph = None if msg.eph == 65535 else msg.eph
component.status.gps.epv = None if msg.epv == 65535 else msg.epv
component.status.gps.timestamp = timestamp
def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
"""處理 VFR_HUD 訊息 (msg_id: 74)"""
component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +395,9 @@ class mavlink_object:
self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
# 記錄訊息過濾類型 (可選)
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
# 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE,
# 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])
self.return_msg_types = set([])
# 轉發到別的 mavlink object 作為目標端口 的列表
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+15 -10
View File
@@ -61,7 +61,7 @@ class PublishRateController:
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
self.topic_intervals = {
'position': 0.0, # GNSS位置
'position_gnss': 0.0, # GNSS位置
'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
@@ -171,7 +171,7 @@ class VehicleStatusPublisher(Node):
# 例如：self._publish_ekf_status(sysid, status)
# ═══════════════════════════════════════════════════════════════
# 發布各種狀態（通過頻率控制器判斷是否發布）
self._publish_position(sysid, status)
self._publish_position_gnss(sysid, status)
self._publish_position_ned(sysid, status)
self._publish_attitude(sysid, status)
self._publish_velocity(sysid, status)
@@ -202,9 +202,9 @@ class VehicleStatusPublisher(Node):
logger.info(f"Created publisher: {topic_name}")
return self.fc_publishers[key]
def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
"""發布 GPS 位置"""
if not self.rate_controller.should_publish(sysid, 'position'):
if not self.rate_controller.should_publish(sysid, 'position_gnss'):
return
pos = status.position
@@ -212,22 +212,27 @@ class VehicleStatusPublisher(Node):
return
publisher = self._get_or_create_publisher(
sysid, 'position', sensor_msgs.msg.NavSatFix
sysid, 'position_gnss', fcmsg.GnssRaw
)
# 檢查是否有訂閱者
if publisher.get_subscription_count() == 0:
return
msg = sensor_msgs.msg.NavSatFix()
msg.latitude = pos.latitude
msg.longitude = pos.longitude
msg.altitude = pos.altitude if pos.altitude is not None else 0.0
msg = fcmsg.GnssRaw()
msg.stamp = self.get_clock().now().to_msg()
msg.latitude = float(pos.latitude)
msg.longitude = float(pos.longitude)
msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
# GPS 狀態資訊
gps = status.gps
if gps.fix_type is not None:
msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
msg.fix_type = int(gps.fix_type)
if gps.satellites_visible is not None:
msg.satellites_visible = int(gps.satellites_visible)
msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
publisher.publish(msg)
src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
+9 -8
View File
@@ -17,6 +17,7 @@ import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg
import fc_interfaces.msg
# 專案 imports
from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
self.sysid = sysid
self.received_messages = {
'position': [],
'position_gnss': [],
'attitude': [],
'velocity': [],
'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
# Position
self.create_subscription(
sensor_msgs.msg.NavSatFix,
f'{base_topic}/position',
lambda msg: self._on_message('position', msg),
fc_interfaces.msg.GnssRaw,
f'{base_topic}/position_gnss',
lambda msg: self._on_message('position_gnss', msg),
10
)
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
def _format_msg(self, topic_name: str, msg) -> str:
"""格式化消息以便顯示"""
if topic_name == 'position':
if topic_name == 'position_gnss':
return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
elif topic_name == 'attitude':
return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
# 檢查收到的消息
print("\n--- 消息統計 ---")
total_messages = 0
for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
count = test_sub.get_message_count(topic)
total_messages += count
print(f" {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
# 修改頻率設定
publisher_node = ros2_manager.status_publisher
publisher_node.rate_controller.topic_intervals = {
'position': 1.5,
'position_gnss': 1.5,
'attitude': 1.0,
'velocity': 1.0,
'battery': 1.0,
@@ -329,7 +330,7 @@ def test_frequency_control():
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
print("2Hz Topics (預期 ~6 條):")
for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
count = test_sub.get_message_count(topic)
print(f" {topic:15s}: {count:3d} 條")
for topic in ['battery', 'mode', 'summary']: