|
|
"""
|
|
|
VehicleStatusPublisher 測試程式
|
|
|
|
|
|
測試從 vehicle_registry 讀取資料並發布到 ROS2 topics
|
|
|
"""
|
|
|
|
|
|
import time
|
|
|
import json
|
|
|
import threading
|
|
|
|
|
|
# ROS2 imports
|
|
|
import rclpy
|
|
|
from rclpy.node import Node
|
|
|
|
|
|
# 標準 ROS2 消息類型
|
|
|
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 (
|
|
|
VehicleStatusPublisher,
|
|
|
fc_ros_manager,
|
|
|
ros2_manager
|
|
|
)
|
|
|
from ..fc_network_adapter.mavlinkVehicleView import (
|
|
|
vehicle_registry,
|
|
|
ConnectionType,
|
|
|
ComponentType,
|
|
|
)
|
|
|
|
|
|
|
|
|
class TestSubscriber(Node):
|
|
|
"""測試用的訂閱者節點 - 接收並記錄收到的消息"""
|
|
|
|
|
|
def __init__(self, sysid: int = 1):
|
|
|
super().__init__(f'test_subscriber_sys{sysid}')
|
|
|
|
|
|
self.sysid = sysid
|
|
|
self.received_messages = {
|
|
|
'position_gnss': [],
|
|
|
'attitude': [],
|
|
|
'velocity': [],
|
|
|
'battery': [],
|
|
|
'vfr_hud': [],
|
|
|
'mode': [],
|
|
|
'summary': [],
|
|
|
}
|
|
|
|
|
|
# 建立所有訂閱者
|
|
|
self._create_subscriptions()
|
|
|
|
|
|
print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics")
|
|
|
|
|
|
def _create_subscriptions(self):
|
|
|
"""建立所有 topic 的訂閱者"""
|
|
|
|
|
|
base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}'
|
|
|
|
|
|
# Position
|
|
|
self.create_subscription(
|
|
|
fc_interfaces.msg.GnssRaw,
|
|
|
f'{base_topic}/position_gnss',
|
|
|
lambda msg: self._on_message('position_gnss', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# Attitude
|
|
|
self.create_subscription(
|
|
|
sensor_msgs.msg.Imu,
|
|
|
f'{base_topic}/attitude',
|
|
|
lambda msg: self._on_message('attitude', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# Velocity
|
|
|
self.create_subscription(
|
|
|
geometry_msgs.msg.TwistStamped,
|
|
|
f'{base_topic}/velocity',
|
|
|
lambda msg: self._on_message('velocity', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# Battery
|
|
|
self.create_subscription(
|
|
|
sensor_msgs.msg.BatteryState,
|
|
|
f'{base_topic}/battery',
|
|
|
lambda msg: self._on_message('battery', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# VFR HUD
|
|
|
self.create_subscription(
|
|
|
mavros_msgs.msg.VfrHud,
|
|
|
f'{base_topic}/vfr_hud',
|
|
|
lambda msg: self._on_message('vfr_hud', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# Mode
|
|
|
self.create_subscription(
|
|
|
std_msgs.msg.String,
|
|
|
f'{base_topic}/mode',
|
|
|
lambda msg: self._on_message('mode', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
# Summary
|
|
|
self.create_subscription(
|
|
|
std_msgs.msg.String,
|
|
|
f'{base_topic}/summary',
|
|
|
lambda msg: self._on_message('summary', msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
def _on_message(self, topic_name: str, msg):
|
|
|
"""通用消息接收回調"""
|
|
|
self.received_messages[topic_name].append(msg)
|
|
|
print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}")
|
|
|
|
|
|
def _format_msg(self, topic_name: str, msg) -> str:
|
|
|
"""格式化消息以便顯示"""
|
|
|
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})"
|
|
|
elif topic_name == 'velocity':
|
|
|
return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})"
|
|
|
elif topic_name == 'battery':
|
|
|
return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%"
|
|
|
elif topic_name == 'vfr_hud':
|
|
|
return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}"
|
|
|
elif topic_name == 'mode':
|
|
|
return f"mode={msg.data}"
|
|
|
elif topic_name == 'summary':
|
|
|
try:
|
|
|
data = json.loads(msg.data)
|
|
|
return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}"
|
|
|
except:
|
|
|
return msg.data
|
|
|
return str(msg)
|
|
|
|
|
|
def get_message_count(self, topic_name: str) -> int:
|
|
|
"""獲取收到的消息數量"""
|
|
|
return len(self.received_messages[topic_name])
|
|
|
|
|
|
def clear_messages(self):
|
|
|
"""清空已收到的消息"""
|
|
|
for key in self.received_messages:
|
|
|
self.received_messages[key].clear()
|
|
|
|
|
|
|
|
|
def setup_test_vehicle(sysid: int = 1, socket_id: int = 10):
|
|
|
"""
|
|
|
建立測試用的載具數據
|
|
|
|
|
|
Args:
|
|
|
sysid: 系統 ID
|
|
|
socket_id: Socket ID
|
|
|
"""
|
|
|
print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===")
|
|
|
|
|
|
# 註冊載具
|
|
|
vehicle = vehicle_registry.register(sysid)
|
|
|
vehicle.kind = "Copter"
|
|
|
vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
|
|
|
vehicle.connected_via = ConnectionType.UDP
|
|
|
vehicle.custom_meta['socket_id'] = socket_id
|
|
|
|
|
|
# 新增 autopilot 組件 (component_id=1)
|
|
|
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
|
|
|
autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
|
|
|
autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
|
|
|
|
|
|
# 填充狀態資料
|
|
|
status = autopilot.status
|
|
|
|
|
|
# 位置
|
|
|
status.position.latitude = 25.0330
|
|
|
status.position.longitude = 121.5654
|
|
|
status.position.altitude = 100.5
|
|
|
status.position.relative_altitude = 50.0
|
|
|
status.position.timestamp = time.time()
|
|
|
|
|
|
# 姿態
|
|
|
status.attitude.roll = 0.1
|
|
|
status.attitude.pitch = -0.05
|
|
|
status.attitude.yaw = 1.57
|
|
|
status.attitude.rollspeed = 0.01
|
|
|
status.attitude.pitchspeed = 0.02
|
|
|
status.attitude.yawspeed = 0.03
|
|
|
status.attitude.timestamp = time.time()
|
|
|
|
|
|
# 飛行模式
|
|
|
status.mode.base_mode = 89
|
|
|
status.mode.custom_mode = 4
|
|
|
status.mode.mode_name = "GUIDED"
|
|
|
status.mode.timestamp = time.time()
|
|
|
|
|
|
# 電池
|
|
|
status.battery.voltage = 12.6
|
|
|
status.battery.current = 15.3
|
|
|
status.battery.remaining = 75
|
|
|
status.battery.temperature = 35.2
|
|
|
status.battery.timestamp = time.time()
|
|
|
|
|
|
# GPS
|
|
|
status.gps.fix_type = 3 # 3D fix
|
|
|
status.gps.satellites_visible = 12
|
|
|
status.gps.eph = 100
|
|
|
status.gps.epv = 150
|
|
|
status.gps.timestamp = time.time()
|
|
|
|
|
|
# VFR
|
|
|
status.vfr.airspeed = 5.5
|
|
|
status.vfr.groundspeed = 6.0
|
|
|
status.vfr.heading = 90
|
|
|
status.vfr.throttle = 65
|
|
|
status.vfr.climb = 1.2
|
|
|
status.vfr.timestamp = time.time()
|
|
|
|
|
|
# 系統狀態
|
|
|
status.armed = True
|
|
|
status.system_status = 4 # MAV_STATE_ACTIVE
|
|
|
|
|
|
# 更新封包統計
|
|
|
autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time())
|
|
|
|
|
|
print(f"✓ 載具 {sysid} 已建立並填充測試數據")
|
|
|
return vehicle
|
|
|
|
|
|
|
|
|
def test_basic_publishing():
|
|
|
"""測試基本的發布功能"""
|
|
|
print("\n" + "="*60)
|
|
|
print("測試 1: 基本發布功能")
|
|
|
print("="*60)
|
|
|
|
|
|
# 清空 registry
|
|
|
vehicle_registry.clear()
|
|
|
|
|
|
# 建立測試載具
|
|
|
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
|
|
|
|
|
|
# 初始化 ROS2 管理器
|
|
|
if not ros2_manager.initialized:
|
|
|
ros2_manager.initialize()
|
|
|
|
|
|
# 建立測試訂閱者
|
|
|
test_sub = TestSubscriber(sysid=1)
|
|
|
|
|
|
# 啟動 publisher
|
|
|
ros2_manager.start()
|
|
|
|
|
|
print("\n--- 開始發布，等待 5 秒 ---")
|
|
|
|
|
|
# 運行 5 秒，持續 spin
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 5.0:
|
|
|
rclpy.spin_once(test_sub, timeout_sec=0.1)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
# 檢查收到的消息
|
|
|
print("\n--- 消息統計 ---")
|
|
|
total_messages = 0
|
|
|
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} 條消息")
|
|
|
|
|
|
print(f"\n總計收到: {total_messages} 條消息")
|
|
|
|
|
|
# 驗證
|
|
|
if total_messages > 0:
|
|
|
print("\n✓ 測試通過：成功接收到消息")
|
|
|
else:
|
|
|
print("\n✗ 測試失敗：沒有接收到任何消息")
|
|
|
|
|
|
# 停止
|
|
|
ros2_manager.stop()
|
|
|
test_sub.destroy_node()
|
|
|
|
|
|
|
|
|
def test_frequency_control():
|
|
|
"""測試頻率控制功能"""
|
|
|
print("\n" + "="*60)
|
|
|
print("測試 2: 頻率控制")
|
|
|
print("="*60)
|
|
|
|
|
|
# 清空 registry
|
|
|
vehicle_registry.clear()
|
|
|
|
|
|
# 建立測試載具
|
|
|
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
|
|
|
|
|
|
# 初始化（如果還沒初始化）
|
|
|
if not ros2_manager.initialized:
|
|
|
ros2_manager.initialize()
|
|
|
|
|
|
# 建立測試訂閱者
|
|
|
test_sub = TestSubscriber(sysid=1)
|
|
|
|
|
|
# 修改頻率設定
|
|
|
publisher_node = ros2_manager.status_publisher
|
|
|
publisher_node.rate_controller.topic_intervals = {
|
|
|
'position_gnss': 1.5,
|
|
|
'attitude': 1.0,
|
|
|
'velocity': 1.0,
|
|
|
'battery': 1.0,
|
|
|
'vfr_hud': 0.5,
|
|
|
'mode': 0.0,
|
|
|
'summary': 0.0,
|
|
|
}
|
|
|
|
|
|
# 啟動 publisher
|
|
|
ros2_manager.start()
|
|
|
|
|
|
print("\n--- 測試頻率控制，運行 3 秒 ---")
|
|
|
|
|
|
# 運行 3 秒
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 3.0:
|
|
|
rclpy.spin_once(test_sub, timeout_sec=0.1)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
# 檢查頻率
|
|
|
print("\n--- 頻率分析 ---")
|
|
|
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
|
|
|
|
|
|
print("2Hz Topics (預期 ~6 條):")
|
|
|
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']:
|
|
|
count = test_sub.get_message_count(topic)
|
|
|
print(f" {topic:15s}: {count:3d} 條")
|
|
|
|
|
|
print("\n✓ 頻率控制測試完成")
|
|
|
|
|
|
# 停止
|
|
|
ros2_manager.stop()
|
|
|
test_sub.destroy_node()
|
|
|
|
|
|
|
|
|
def test_multi_vehicle():
|
|
|
"""測試多載具發布"""
|
|
|
print("\n" + "="*60)
|
|
|
print("測試 3: 多載具發布")
|
|
|
print("="*60)
|
|
|
|
|
|
# 清空 registry
|
|
|
vehicle_registry.clear()
|
|
|
|
|
|
# 建立 3 個測試載具
|
|
|
v1 = setup_test_vehicle(sysid=1, socket_id=10)
|
|
|
v2 = setup_test_vehicle(sysid=2, socket_id=11)
|
|
|
v3 = setup_test_vehicle(sysid=3, socket_id=12)
|
|
|
|
|
|
# 修改各載具的位置以便區分
|
|
|
v2.components[1].status.position.latitude = 26.0
|
|
|
v3.components[1].status.position.latitude = 27.0
|
|
|
|
|
|
# 初始化
|
|
|
if not ros2_manager.initialized:
|
|
|
ros2_manager.initialize()
|
|
|
|
|
|
# 建立 3 個測試訂閱者
|
|
|
test_sub1 = TestSubscriber(sysid=1)
|
|
|
test_sub2 = TestSubscriber(sysid=2)
|
|
|
test_sub3 = TestSubscriber(sysid=3)
|
|
|
|
|
|
# 啟動 publisher
|
|
|
ros2_manager.start()
|
|
|
|
|
|
print("\n--- 測試多載具，運行 3 秒 ---")
|
|
|
|
|
|
# 運行 3 秒
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 3.0:
|
|
|
rclpy.spin_once(test_sub1, timeout_sec=0.05)
|
|
|
rclpy.spin_once(test_sub2, timeout_sec=0.05)
|
|
|
rclpy.spin_once(test_sub3, timeout_sec=0.05)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
# 檢查每個載具的消息
|
|
|
print("\n--- 各載具消息統計 ---")
|
|
|
for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]:
|
|
|
total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
|
|
|
print(f"載具 {sysid}: {total:3d} 條消息")
|
|
|
|
|
|
# 檢查 summary 中的 socket_id
|
|
|
if test_sub.get_message_count('summary') > 0:
|
|
|
last_summary = test_sub.received_messages['summary'][-1]
|
|
|
data = json.loads(last_summary.data)
|
|
|
print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}")
|
|
|
|
|
|
print("\n✓ 多載具測試完成")
|
|
|
|
|
|
# 停止
|
|
|
ros2_manager.stop()
|
|
|
test_sub1.destroy_node()
|
|
|
test_sub2.destroy_node()
|
|
|
test_sub3.destroy_node()
|
|
|
|
|
|
|
|
|
def test_dynamic_vehicle():
|
|
|
"""測試動態新增/移除載具"""
|
|
|
print("\n" + "="*60)
|
|
|
print("測試 4: 動態載具管理")
|
|
|
print("="*60)
|
|
|
|
|
|
# 清空 registry
|
|
|
vehicle_registry.clear()
|
|
|
|
|
|
# 初始化
|
|
|
if not ros2_manager.initialized:
|
|
|
ros2_manager.initialize()
|
|
|
|
|
|
# 建立測試訂閱者
|
|
|
test_sub = TestSubscriber(sysid=1)
|
|
|
|
|
|
# 啟動 publisher
|
|
|
ros2_manager.start()
|
|
|
|
|
|
print("\n--- 階段 1: 無載具，運行 1 秒 ---")
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 1.0:
|
|
|
rclpy.spin_once(test_sub, timeout_sec=0.1)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
|
|
|
print(f"收到消息: {count_before} 條")
|
|
|
|
|
|
# 新增載具
|
|
|
print("\n--- 階段 2: 新增載具，運行 2 秒 ---")
|
|
|
vehicle = setup_test_vehicle(sysid=1, socket_id=10)
|
|
|
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 2.0:
|
|
|
rclpy.spin_once(test_sub, timeout_sec=0.1)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
|
|
|
print(f"收到消息: {count_after - count_before} 條")
|
|
|
|
|
|
# 移除載具
|
|
|
print("\n--- 階段 3: 移除載具，運行 1 秒 ---")
|
|
|
vehicle_registry.unregister(1)
|
|
|
|
|
|
start_time = time.time()
|
|
|
while time.time() - start_time < 1.0:
|
|
|
rclpy.spin_once(test_sub, timeout_sec=0.1)
|
|
|
time.sleep(0.1)
|
|
|
|
|
|
count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
|
|
|
print(f"收到消息: {count_final - count_after} 條（應該為 0）")
|
|
|
|
|
|
if count_final - count_after == 0:
|
|
|
print("\n✓ 動態載具管理測試通過")
|
|
|
else:
|
|
|
print("\n✗ 移除載具後仍收到消息")
|
|
|
|
|
|
# 停止
|
|
|
ros2_manager.stop()
|
|
|
test_sub.destroy_node()
|
|
|
|
|
|
|
|
|
def main():
|
|
|
"""主測試函數"""
|
|
|
print("\n" + "="*60)
|
|
|
print("VehicleStatusPublisher 測試程式")
|
|
|
print("="*60)
|
|
|
|
|
|
try:
|
|
|
# 執行各項測試
|
|
|
test_basic_publishing()
|
|
|
# time.sleep(1)
|
|
|
|
|
|
# test_frequency_control()
|
|
|
# time.sleep(1)
|
|
|
|
|
|
# test_multi_vehicle()
|
|
|
# time.sleep(1)
|
|
|
|
|
|
# test_dynamic_vehicle()
|
|
|
|
|
|
print("\n" + "="*60)
|
|
|
print("所有測試完成！")
|
|
|
print("="*60)
|
|
|
|
|
|
except KeyboardInterrupt:
|
|
|
print("\n\n測試被中斷")
|
|
|
except Exception as e:
|
|
|
print(f"\n\n測試出錯: {e}")
|
|
|
import traceback
|
|
|
traceback.print_exc()
|
|
|
finally:
|
|
|
# 清理
|
|
|
if ros2_manager.initialized:
|
|
|
ros2_manager.shutdown()
|
|
|
vehicle_registry.clear()
|
|
|
print("\n清理完成")
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
main()
|