1. (modify) 更新 ros2 對於 GNSS topic 的發佈, 使其包涵 epv eph 衛星數與 fix_type
2. (adding) 新增相應的 msg 檔案與修改 CMakeLists, package 3. (adding) test_vehicleStatusPublisher 相對應修正
This commit is contained in:
@@ -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']:
|
||||
|
||||
Reference in New Issue
Block a user