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}")
|