open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 13:01:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

Closes #3. Closes #12.

Browse Source 
Closes #3. Closes #12.
* Update README and MQTTClient for installation instructions and code improvements

* feat: 支持local_config启动
add: 增加对crt path的说明，为传入config.py的相对路径
move: web component

* add: registry description

* feat: node_info_update srv
fix: OTDeck cant create

* close #12
feat: slave node registry

* feat: show machine name
fix: host node registry not uploaded

* feat: add hplc registry

* feat: add hplc registry

* fix: hplc status typo

* fix: devices/

* fix: device.class possible null

* fix: HPLC additions with online service

* fix: slave mode spin not working

* fix: slave mode spin not working

* feat: 多ProtocolNode 允许子设备ID相同
feat: 上报发现的ActionClient
feat: Host重启动，通过discover机制要求slaveNode重新注册，实现信息及时上报

---------

Co-authored-by: Harvey Que <Q-Query@outlook.com>
This commit is contained in:
Xuwznln
2025-05-01 14:58:36 +08:00
committed by GitHub
parent 74ae2a88ac
commit 01ac3415ae
15 changed files with 300 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
unilabos/ros/nodes/presets/host_node.py
View File

@@ -1,4 +1,5 @@
import copy
import json
import threading
import time
import traceback
@@ -7,12 +8,13 @@ from typing import Optional, Dict, Any, List, ClassVar, Set
from action_msgs.msg import GoalStatus
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList # type: ignore
from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, SerialCommand # type: ignore
from rclpy.action import ActionClient, get_action_server_names_and_types_by_node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.service import Service
from unique_identifier_msgs.msg import UUID
from unilabos.registry.registry import lab_registry
from unilabos.resources.registry import add_schema
from unilabos.ros.initialize_device import initialize_device_from_dict
from unilabos.ros.msgs.message_converter import (
@@ -20,10 +22,12 @@ from unilabos.ros.msgs.message_converter import (
get_ros_type_by_msgname,
convert_from_ros_msg,
convert_to_ros_msg,
msg_converter_manager, ros_action_to_json_schema,
msg_converter_manager,
ros_action_to_json_schema,
)
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
from unilabos.ros.nodes.presets.controller_node import ControllerNode
from unilabos.utils.type_check import TypeEncoder
class HostNode(BaseROS2DeviceNode):
@@ -95,6 +99,7 @@ class HostNode(BaseROS2DeviceNode):
# 创建设备、动作客户端和目标存储
self.devices_names: Dict[str, str] = {} # 存储设备名称和命名空间的映射
self.devices_instances: Dict[str, ROS2DeviceNode] = {} # 存储设备实例
self.device_machine_names: Dict[str, str] = {device_id: "本地", } # 存储设备ID到机器名称的映射
self._action_clients: Dict[str, ActionClient] = {} # 用来存储多个ActionClient实例
self._action_value_mappings: Dict[str, Dict] = (
{}
@@ -106,18 +111,24 @@ class HostNode(BaseROS2DeviceNode):
self._subscribed_topics = set() # 用于跟踪已订阅的话题
# 创建物料增删改查服务（非客户端）
self._init_resource_service()
self._init_host_service()
self.device_status = {} # 用来存储设备状态
self.device_status_timestamps = {} # 用来存储设备状态最后更新时间
from unilabos.app.mq import mqtt_client
for device_config in lab_registry.obtain_registry_device_info():
mqtt_client.publish_registry(device_config["id"], device_config)
# 首次发现网络中的设备
self._discover_devices()
# 初始化所有本机设备节点，多一次过滤，防止重复初始化
for device_id, device_config in devices_config.items():
if device_config.get("type", "device") != "device":
self.lab_logger().debug(f"[Host Node] Skipping type {device_config['type']} {device_id} already existed, skipping.")
self.lab_logger().debug(
f"[Host Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
)
continue
if device_id not in self.devices_names:
self.initialize_device(device_id, device_config)
@@ -150,6 +161,13 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info("[Host Node] Host node initialized.")
HostNode._ready_event.set()
def _send_re_register(self, sclient):
sclient.wait_for_service()
request = SerialCommand.Request()
request.command = ""
future = sclient.call_async(request)
response = future.result()
def _discover_devices(self) -> None:
"""
发现网络中的设备
@@ -166,23 +184,37 @@ class HostNode(BaseROS2DeviceNode):
current_devices = set()
for device_id, namespace in nodes_and_names:
if not namespace.startswith("/devices"):
if not namespace.startswith("/devices/"):
continue
edge_device_id = namespace[9:]
# 将设备添加到当前设备集合
device_key = f"{namespace}/{device_id}"
device_key = f"{namespace}/{edge_device_id}" # namespace已经包含device_id了，这里复写一遍
current_devices.add(device_key)
# 如果是新设备，记录并创建ActionClient
if device_id not in self.devices_names:
if edge_device_id not in self.devices_names:
self.lab_logger().info(f"[Host Node] Discovered new device: {device_key}")
self.devices_names[device_id] = namespace
self.devices_names[edge_device_id] = namespace
self._create_action_clients_for_device(device_id, namespace)
self._online_devices.add(device_key)
sclient = self.create_client(SerialCommand, f"/srv{namespace}/query_host_name")
threading.Thread(
target=self._send_re_register,
args=(sclient,),
daemon=True,
name=f"ROSDevice{self.device_id}_query_host_name_{namespace}"
).start()
elif device_key not in self._online_devices:
# 设备重新上线
self.lab_logger().info(f"[Host Node] Device reconnected: {device_key}")
self._online_devices.add(device_key)
sclient = self.create_client(SerialCommand, f"/srv{namespace}/query_host_name")
threading.Thread(
target=self._send_re_register,
args=(sclient,),
daemon=True,
name=f"ROSDevice{self.device_id}_query_host_name_{namespace}"
).start()
# 检测离线设备
offline_devices = self._online_devices - current_devices
@@ -224,16 +256,22 @@ class HostNode(BaseROS2DeviceNode):
self._action_clients[action_id] = ActionClient(
self, action_type, action_id, callback_group=self.callback_group
)
self.lab_logger().debug(f"[Host Node] Created ActionClient: {action_id}")
self.lab_logger().debug(f"[Host Node] Created ActionClient (Discovery): {action_id}")
action_name = action_id[len(namespace) + 1:]
edge_device_id = namespace[9:]
from unilabos.app.mq import mqtt_client
info_with_schema = ros_action_to_json_schema(action_type)
mqtt_client.publish_actions(action_id, info_with_schema)
mqtt_client.publish_actions(action_name, {
"device_id": edge_device_id,
"action_name": action_name,
"schema": info_with_schema,
})
except Exception as e:
self.lab_logger().error(f"[Host Node] Failed to create ActionClient for {action_id}: {str(e)}")
def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
"""
根据配置初始化设备
根据配置初始化设备，
此函数根据提供的设备配置动态导入适当的设备类并创建其实例。
同时为设备的动作值映射设置动作客户端。
@@ -249,7 +287,8 @@ class HostNode(BaseROS2DeviceNode):
if d is None:
return
# noinspection PyProtectedMember
self.devices_names[device_id] = d._ros_node.namespace
self.devices_names[device_id] = d._ros_node.namespace # 这里不涉及二级device_id
self.device_machine_names[device_id] = "本地"
self.devices_instances[device_id] = d
# noinspection PyProtectedMember
for action_name, action_value_mapping in d._ros_node._action_value_mappings.items():
@@ -257,13 +296,17 @@ class HostNode(BaseROS2DeviceNode):
if action_id not in self._action_clients:
action_type = action_value_mapping["type"]
self._action_clients[action_id] = ActionClient(self, action_type, action_id)
self.lab_logger().debug(f"[Host Node] Created ActionClient: {action_id}")
self.lab_logger().debug(f"[Host Node] Created ActionClient (Local): {action_id}") # 子设备再创建用的是Discover发现的
from unilabos.app.mq import mqtt_client
info_with_schema = ros_action_to_json_schema(action_type)
mqtt_client.publish_actions(action_id, info_with_schema)
mqtt_client.publish_actions(action_name, {
"device_id": device_id,
"action_name": action_name,
"schema": info_with_schema,
})
else:
self.lab_logger().warning(f"[Host Node] ActionClient {action_id} already exists.")
device_key = f"{self.devices_names[device_id]}/{device_id}"
device_key = f"{self.devices_names[device_id]}/{device_id}" # 这里不涉及二级device_id
# 添加到在线设备列表
self._online_devices.add(device_key)
@@ -285,8 +328,8 @@ class HostNode(BaseROS2DeviceNode):
# 解析设备名和属性名
parts = topic.split("/")
if len(parts) >= 4:
device_id = parts[-2]
if len(parts) >= 4: # 可能有ProtocolNode，创建更长的设备
device_id = "/".join(parts[2:-1])
property_name = parts[-1]
# 初始化设备状态字典
@@ -473,7 +516,7 @@ class HostNode(BaseROS2DeviceNode):
"""Resource"""
def _init_resource_service(self):
def _init_host_service(self):
self._resource_services: Dict[str, Service] = {
"resource_add": self.create_service(
ResourceAdd, "/resources/add", self._resource_add_callback, callback_group=ReentrantCallbackGroup()
@@ -496,8 +539,39 @@ class HostNode(BaseROS2DeviceNode):
"resource_list": self.create_service(
ResourceList, "/resources/list", self._resource_list_callback, callback_group=ReentrantCallbackGroup()
),
"node_info_update": self.create_service(
SerialCommand,
"/node_info_update",
self._node_info_update_callback,
callback_group=ReentrantCallbackGroup(),
),
}
def _node_info_update_callback(self, request, response):
"""
更新节点信息回调
"""
self.lab_logger().info(f"[Host Node] Node info update request received: {request}")
try:
from unilabos.app.mq import mqtt_client
info = json.loads(request.command)
if "SYNC_SLAVE_NODE_INFO" in info:
info = info["SYNC_SLAVE_NODE_INFO"]
machine_name = info["machine_name"]
edge_device_id = info["edge_device_id"]
self.device_machine_names[edge_device_id] = machine_name
else:
registry_config = info["registry_config"]
for device_config in registry_config:
mqtt_client.publish_registry(device_config["id"], device_config)
self.lab_logger().debug(f"[Host Node] Node info update: {info}")
response.response = "OK"
except Exception as e:
self.lab_logger().error(f"[Host Node] Error updating node info: {e.args}")
response.response = "ERROR"
return response
def _resource_add_callback(self, request, response):
"""
添加资源回调