ROS2 sub
This commit is contained in:
+50
-47
@@ -11,7 +11,7 @@ import socket
|
||||
from pymavlink import mavutil
|
||||
from geometry_msgs.msg import Point, Vector3
|
||||
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
||||
from std_msgs.msg import Float64
|
||||
from std_msgs.msg import Float64, String
|
||||
from mavros_msgs.msg import State, VfrHud
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL
|
||||
|
||||
@@ -371,6 +371,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
|
||||
self.running = False
|
||||
|
||||
class DroneMonitor(Node):
|
||||
# Subscribe to drone ROS2 topics
|
||||
def __init__(self):
|
||||
super().__init__('drone_monitor')
|
||||
self.signals = DroneSignals()
|
||||
@@ -403,12 +404,15 @@ class DroneMonitor(Node):
|
||||
|
||||
def scan_topics(self):
|
||||
topics = self.get_topic_names_and_types()
|
||||
drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
|
||||
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
|
||||
|
||||
found_drones = set()
|
||||
for topic_name, _ in topics:
|
||||
if match := drone_pattern.match(topic_name):
|
||||
drone_id, topic_type = match.groups()
|
||||
sys_id, topic_type = match.groups()
|
||||
# 將 sys11 轉換為 s0_11 格式以保持兼容性
|
||||
drone_num = sys_id.replace('sys', '')
|
||||
drone_id = f's0_{drone_num}'
|
||||
found_drones.add(drone_id)
|
||||
with self.lock:
|
||||
self.drone_topics.setdefault(drone_id, set()).add(topic_type)
|
||||
@@ -418,9 +422,11 @@ class DroneMonitor(Node):
|
||||
self.setup_drone(drone_id)
|
||||
|
||||
def setup_drone(self, drone_id):
|
||||
base_topic = f'/MavLinkBus/{drone_id}'
|
||||
# 從 s0_11 轉換回 sys11 格式
|
||||
sys_id = drone_id.replace('s0_', 'sys')
|
||||
base_topic = f'/fc_network/vehicle/{sys_id}'
|
||||
|
||||
# Add service clients
|
||||
# Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
|
||||
self.arm_clients[drone_id] = self.create_client(
|
||||
CommandBool,
|
||||
f'{base_topic}/cmd/arming'
|
||||
@@ -438,58 +444,22 @@ class DroneMonitor(Node):
|
||||
)
|
||||
|
||||
subs = {
|
||||
'attitude': self.create_subscription(
|
||||
Imu,
|
||||
f'{base_topic}/attitude',
|
||||
lambda msg, did=drone_id: self.attitude_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'battery': self.create_subscription(
|
||||
BatteryState,
|
||||
f'{base_topic}/battery',
|
||||
lambda msg, did=drone_id: self.battery_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'global': self.create_subscription(
|
||||
'position': self.create_subscription(
|
||||
NavSatFix,
|
||||
f'{base_topic}/global_position/global',
|
||||
f'{base_topic}/position',
|
||||
lambda msg, did=drone_id: self.gps_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'rel_alt': self.create_subscription(
|
||||
Float64,
|
||||
f'{base_topic}/global_position/rel_alt',
|
||||
lambda msg, did=drone_id: self.altitude_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'local_pose': self.create_subscription(
|
||||
Point,
|
||||
f'{base_topic}/local_position/pose',
|
||||
lambda msg, did=drone_id: self.local_pose_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'local_vel': self.create_subscription(
|
||||
Vector3,
|
||||
f'{base_topic}/local_position/velocity',
|
||||
lambda msg, did=drone_id: self.local_vel_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'loss_rate': self.create_subscription(
|
||||
Float64,
|
||||
f'{base_topic}/packet_loss_rate',
|
||||
lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'state': self.create_subscription(
|
||||
State,
|
||||
f'{base_topic}/state',
|
||||
lambda msg, did=drone_id: self.state_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'ping': self.create_subscription(
|
||||
Float64,
|
||||
f'{base_topic}/ping',
|
||||
lambda msg, did=drone_id: self.ping_callback(did, msg),
|
||||
'summary': self.create_subscription(
|
||||
String,
|
||||
f'{base_topic}/summary',
|
||||
lambda msg, did=drone_id: self.summary_callback(did, msg),
|
||||
10
|
||||
),
|
||||
'vfr_hud': self.create_subscription(
|
||||
@@ -596,6 +566,39 @@ class DroneMonitor(Node):
|
||||
'armed': msg.armed
|
||||
}
|
||||
|
||||
def summary_callback(self, drone_id, msg):
|
||||
"""處理 summary topic (JSON 格式)"""
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
mode = data.get('mode_name', 'UNKNOWN')
|
||||
if mode in self.filtered_modes:
|
||||
return
|
||||
|
||||
# 根據 socket_id 更新 drone_id
|
||||
socket_id = data.get('socket_id')
|
||||
sysid = data.get('sysid')
|
||||
if socket_id is not None and sysid is not None:
|
||||
# 使用 socket_id 作為前綴
|
||||
actual_drone_id = f's{socket_id}_{sysid}'
|
||||
else:
|
||||
actual_drone_id = drone_id
|
||||
|
||||
self.latest_data[(actual_drone_id, 'state')] = {
|
||||
'mode': mode,
|
||||
'armed': data.get('armed', False),
|
||||
'socket_id': socket_id,
|
||||
'sysid': sysid,
|
||||
'vehicle_type': data.get('vehicle_type'),
|
||||
'autopilot': data.get('autopilot'),
|
||||
'gps_fix': data.get('gps_fix'),
|
||||
'gps_fix_type': data.get('gps_fix'),
|
||||
'connected': data.get('connected')
|
||||
}
|
||||
except json.JSONDecodeError as e:
|
||||
print(f"Error parsing summary JSON for {drone_id}: {e}")
|
||||
except Exception as e:
|
||||
print(f"Error in summary_callback for {drone_id}: {e}")
|
||||
|
||||
def gps_callback(self, drone_id, msg):
|
||||
self.latest_data[(drone_id, 'gps')] = {
|
||||
'lat': msg.latitude,
|
||||
|
||||
Reference in New Issue
Block a user