165 lines
7.1 KiB
Python
165 lines
7.1 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
DDS (Data Distribution Service) 适配器
|
||
基于 cyclonedds-python 实现
|
||
支持：Publisher/Subscriber、QoS 策略、Domain 隔离
|
||
DDS 是实时分布式系统的工业标准（无人机、机器人、航空航天广泛使用）
|
||
"""
|
||
|
||
import asyncio
|
||
import json
|
||
import logging
|
||
from typing import Dict, Any
|
||
|
||
try:
|
||
import cyclonedds.domain as dds_domain
|
||
import cyclonedds.pub as dds_pub
|
||
import cyclonedds.sub as dds_sub
|
||
import cyclonedds.topic as dds_topic
|
||
import cyclonedds.core as dds_core
|
||
from cyclonedds.idl import IdlStruct
|
||
from cyclonedds.idl.types import array, sequence
|
||
from cyclonedds.qos import Qos, Policy
|
||
HAS_DDS = True
|
||
except ImportError:
|
||
HAS_DDS = False
|
||
logging.getLogger(__name__).warning(
|
||
"⚠️ cyclonedds 未安装，DDS 适配器运行在模拟模式。"
|
||
" 安装: pip install cyclonedds"
|
||
)
|
||
|
||
from uas.comm.base import CommBaseAdapter, ConnectionConfig, MessageHandler
|
||
from uas.comm.message import Message
|
||
|
||
|
||
class DDSAdapterComm(CommBaseAdapter):
|
||
"""
|
||
DDS 通信适配器（基于 CycloneDDS）
|
||
URL 格式: dds://[domain_id@]host[:port][/topic_prefix][?qos_options]
|
||
示例: dds://0@localhost:7400/rt?reliability=RELIABLE&durability=TRANSIENT_LOCAL
|
||
"""
|
||
|
||
def __init__(self, config: ConnectionConfig):
|
||
super().__init__(config)
|
||
self._domain_id = int(config.username) if config.username.isdigit() else 0
|
||
self._topic_prefix = config.database or ""
|
||
self._participant = None
|
||
self._publishers: Dict[str, Any] = {}
|
||
self._subscribers: Dict[str, Any] = {}
|
||
self._writers: Dict[str, Any] = {}
|
||
self._readers: Dict[str, Any] = {}
|
||
self._poll_tasks: Dict[str, asyncio.Task] = {}
|
||
|
||
def _build_qos(self) -> Any:
|
||
"""根据 URL options 构建 QoS 策略"""
|
||
if not HAS_DDS:
|
||
return None
|
||
opts = self.config.options
|
||
reliability = opts.get("reliability", "BEST_EFFORT")
|
||
durability = opts.get("durability", "VOLATILE")
|
||
policies = []
|
||
if reliability == "RELIABLE":
|
||
policies.append(Policy.Reliability.Reliable())
|
||
else:
|
||
policies.append(Policy.Reliability.BestEffort())
|
||
if durability == "TRANSIENT_LOCAL":
|
||
policies.append(Policy.Durability.TransientLocal())
|
||
elif durability == "PERSISTENT":
|
||
policies.append(Policy.Durability.Persistent())
|
||
else:
|
||
policies.append(Policy.Durability.Volatile())
|
||
return Qos(*policies)
|
||
|
||
# ── 生命周期 ──────────────────────────────────────────────────
|
||
|
||
async def connect(self) -> None:
|
||
if HAS_DDS:
|
||
self._participant = dds_domain.DomainParticipant(self._domain_id)
|
||
self.logger.info(
|
||
f"✅ DDS 已连接: domain={self._domain_id} prefix={self._topic_prefix!r}"
|
||
)
|
||
else:
|
||
self.logger.warning("⚠️ DDS 模拟模式已启动（cyclonedds 未安装）")
|
||
self._running = True
|
||
|
||
async def disconnect(self) -> None:
|
||
self._running = False
|
||
for task in self._poll_tasks.values():
|
||
task.cancel()
|
||
# CycloneDDS 资源自动释放（GC）
|
||
self._participant = None
|
||
self.logger.info("🔌 DDS 连接已关闭")
|
||
|
||
# ── 核心功能 ──────────────────────────────────────────────────
|
||
|
||
async def publish(self, message: Message) -> bool:
|
||
full_topic = f"{self._topic_prefix}/{message.topic}".lstrip("/")
|
||
if HAS_DDS:
|
||
try:
|
||
writer = self._get_or_create_writer(full_topic)
|
||
# 序列化为 JSON bytes 写入
|
||
writer.write(json.dumps(message.to_dict()).encode())
|
||
self.logger.debug(f"📤 DDS 发布 topic={full_topic!r}")
|
||
return True
|
||
except Exception as e:
|
||
self.logger.error(f"DDS 发布失败: {e}")
|
||
return False
|
||
else:
|
||
# 模拟模式：直接本地分发
|
||
self.logger.debug(f"[MOCK] DDS 发布 topic={full_topic!r}")
|
||
await self._dispatch(message)
|
||
return True
|
||
|
||
async def subscribe(self, topic: str, handler: MessageHandler) -> None:
|
||
full_topic = f"{self._topic_prefix}/{topic}".lstrip("/")
|
||
self._register_handler(topic, handler)
|
||
|
||
if HAS_DDS and full_topic not in self._readers:
|
||
reader = self._get_or_create_reader(full_topic)
|
||
self._poll_tasks[full_topic] = asyncio.create_task(
|
||
self._poll_loop(topic, full_topic, reader)
|
||
)
|
||
self.logger.info(f"📥 订阅 DDS topic: {full_topic!r} domain={self._domain_id}")
|
||
|
||
async def unsubscribe(self, topic: str) -> None:
|
||
full_topic = f"{self._topic_prefix}/{topic}".lstrip("/")
|
||
if full_topic in self._poll_tasks:
|
||
self._poll_tasks[full_topic].cancel()
|
||
del self._poll_tasks[full_topic]
|
||
self._readers.pop(full_topic, None)
|
||
self._handlers.pop(topic, None)
|
||
self.logger.info(f"🚫 取消订阅 DDS topic: {full_topic!r}")
|
||
|
||
# ── 内部工具 ──────────────────────────────────────────────────
|
||
|
||
def _get_or_create_writer(self, topic_name: str):
|
||
if topic_name not in self._writers:
|
||
tp = dds_topic.Topic(self._participant, topic_name, bytes)
|
||
pub = dds_pub.Publisher(self._participant)
|
||
self._writers[topic_name] = dds_pub.DataWriter(pub, tp, qos=self._build_qos())
|
||
return self._writers[topic_name]
|
||
|
||
def _get_or_create_reader(self, topic_name: str):
|
||
if topic_name not in self._readers:
|
||
tp = dds_topic.Topic(self._participant, topic_name, bytes)
|
||
sub = dds_sub.Subscriber(self._participant)
|
||
self._readers[topic_name] = dds_sub.DataReader(sub, tp, qos=self._build_qos())
|
||
return self._readers[topic_name]
|
||
|
||
async def _poll_loop(self, logical_topic: str, full_topic: str, reader):
|
||
"""轮询 DDS Reader（DDS 无原生 asyncio 支持，用 run_in_executor 桥接）"""
|
||
loop = asyncio.get_event_loop()
|
||
self.logger.info(f"🔄 DDS 轮询循环启动: {full_topic!r}")
|
||
try:
|
||
while self._running:
|
||
samples = await loop.run_in_executor(None, reader.take, 10)
|
||
for sample in samples:
|
||
try:
|
||
data = json.loads(bytes(sample))
|
||
message = Message.from_dict(data)
|
||
await self._dispatch(message)
|
||
except Exception as e:
|
||
self.logger.error(f"DDS 消息解析异常: {e}")
|
||
await asyncio.sleep(0.01) # 10ms 轮询间隔
|
||
except asyncio.CancelledError:
|
||
self.logger.info(f"🛑 DDS 轮询循环停止: {full_topic!r}") |