open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2026-02-04 13:25:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dev' into prcix9320

Browse Source
This commit is contained in:
zhangshixiang
2026-01-12 14:31:56 +08:00
parent 31e8d065c4
commit de7fbe7ac8
18 changed files with 436 additions and 1239 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
unilabos/ros/main_slave_run.py
View File

@@ -1,4 +1,5 @@
import json
# from nt import device_encoding
import threading
import time
@@ -55,7 +56,11 @@ def main(
) -> None:
"""主函数"""
rclpy.init(args=rclpy_init_args)
# Support restart - check if rclpy is already initialized
if not rclpy.ok():
rclpy.init(args=rclpy_init_args)
else:
logger.info("[ROS] rclpy already initialized, reusing context")
executor = rclpy.__executor = MultiThreadedExecutor()
# 创建主机节点
host_node = HostNode(
@@ -88,7 +93,7 @@ def main(
joint_republisher = JointRepublisher("joint_republisher", host_node.resource_tracker)
# lh_joint_pub = LiquidHandlerJointPublisher(
# resources_config=resources_list, resource_tracker=host_node.resource_tracker
# )
# )
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
# executor.add_node(lh_joint_pub)

27
unilabos/ros/nodes/base_device_node.py
View File

@@ -20,6 +20,8 @@ from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.service import Service
from unilabos_msgs.action import SendCmd
from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
from unilabos.config.config import BasicConfig
from unilabos.utils.decorator import get_topic_config, get_all_subscriptions
from unilabos.resources.container import RegularContainer
@@ -790,7 +792,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
def _handle_update(
plr_resources: List[Union[ResourcePLR, ResourceDictInstance]], tree_set: ResourceTreeSet, additional_add_params: Dict[str, Any]
) -> Dict[str, Any]:
) -> Tuple[Dict[str, Any], List[ResourcePLR]]:
"""
处理资源更新操作的内部函数
@@ -802,6 +804,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
Returns:
操作结果字典
"""
original_instances = []
for plr_resource, tree in zip(plr_resources, tree_set.trees):
if isinstance(plr_resource, ResourceDictInstance):
self._lab_logger.info(f"跳过 非资源{plr_resource.res_content.name} 的更新")
@@ -861,13 +864,14 @@ class BaseROS2DeviceNode(Node, Generic[T]):
self.lab_logger().info(
f"更新了资源属性 {plr_resource}[{tree.root_node.res_content.uuid}] " f"及其子节点 {child_count} 个"
)
original_instances.append(original_instance)
# 调用driver的update回调
func = getattr(self.driver_instance, "resource_tree_update", None)
if callable(func):
func(plr_resources)
func(original_instances)
return {"success": True, "action": "update"}
return {"success": True, "action": "update"}, original_instances
try:
data = json.loads(req.command)
@@ -908,14 +912,15 @@ class BaseROS2DeviceNode(Node, Generic[T]):
plr_resources.append(tree.root_node)
else:
plr_resources.append(ResourceTreeSet([tree]).to_plr_resources()[0])
new_tree_set = ResourceTreeSet.from_plr_resources(plr_resources)
result = _handle_update(plr_resources, tree_set, additional_add_params)
r = SerialCommand.Request()
r.command = json.dumps(
{"data": {"data": new_tree_set.dump()}, "action": "update"}) # 和Update Resource一致
response: SerialCommand_Response = await self._resource_clients[
"c2s_update_resource_tree"].call_async(r) # type: ignore
self.lab_logger().info(f"确认资源云端 Update 结果: {response.response}")
result, original_instances = _handle_update(plr_resources, tree_set, additional_add_params)
if not BasicConfig.no_update_feedback:
new_tree_set = ResourceTreeSet.from_plr_resources(original_instances)
r = SerialCommand.Request()
r.command = json.dumps(
{"data": {"data": new_tree_set.dump()}, "action": "update"}) # 和Update Resource一致
response: SerialCommand_Response = await self._resource_clients[
"c2s_update_resource_tree"].call_async(r) # type: ignore
self.lab_logger().info(f"确认资源云端 Update 结果: {response.response}")
results.append(result)
elif action == "remove":
result = _handle_remove(resources_uuid)

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

@@ -70,6 +70,8 @@ class HostNode(BaseROS2DeviceNode):
_instance: ClassVar[Optional["HostNode"]] = None
_ready_event: ClassVar[threading.Event] = threading.Event()
_shutting_down: ClassVar[bool] = False # Flag to signal shutdown to background threads
_background_threads: ClassVar[List[threading.Thread]] = [] # Track all background threads for cleanup
_device_action_status: ClassVar[collections.defaultdict[str, DeviceActionStatus]] = collections.defaultdict(
DeviceActionStatus
)
@@ -81,6 +83,48 @@ class HostNode(BaseROS2DeviceNode):
return cls._instance
return None
@classmethod
def shutdown_background_threads(cls, timeout: float = 5.0) -> None:
"""
Gracefully shutdown all background threads for clean exit or restart.
This method:
1. Sets shutdown flag to stop background operations
2. Waits for background threads to finish with timeout
3. Cleans up finished threads from tracking list
Args:
timeout: Maximum time to wait for each thread (seconds)
"""
cls._shutting_down = True
# Wait for background threads to finish
active_threads = []
for t in cls._background_threads:
if t.is_alive():
t.join(timeout=timeout)
if t.is_alive():
active_threads.append(t.name)
if active_threads:
logger.warning(f"[Host Node] Some background threads still running: {active_threads}")
# Clear the thread list
cls._background_threads.clear()
logger.info(f"[Host Node] Background threads shutdown complete")
@classmethod
def reset_state(cls) -> None:
"""
Reset the HostNode singleton state for restart or clean exit.
Call this after destroying the instance.
"""
cls._instance = None
cls._ready_event.clear()
cls._shutting_down = False
cls._background_threads.clear()
logger.info("[Host Node] State reset complete")
def __init__(
self,
device_id: str,
@@ -294,12 +338,37 @@ class HostNode(BaseROS2DeviceNode):
bridge.publish_host_ready()
self.lab_logger().debug(f"Host ready signal sent via {bridge.__class__.__name__}")
def _send_re_register(self, sclient):
sclient.wait_for_service()
request = SerialCommand.Request()
request.command = ""
future = sclient.call_async(request)
response = future.result()
def _send_re_register(self, sclient, device_namespace: str):
"""
Send re-register command to a device. This is a one-time operation.
Args:
sclient: The service client
device_namespace: The device namespace for logging
"""
try:
# Use timeout to prevent indefinite blocking
if not sclient.wait_for_service(timeout_sec=10.0):
self.lab_logger().debug(f"[Host Node] Re-register timeout for {device_namespace}")
return
# Check shutdown flag after wait
if self._shutting_down:
self.lab_logger().debug(f"[Host Node] Re-register aborted for {device_namespace} (shutdown)")
return
request = SerialCommand.Request()
request.command = ""
future = sclient.call_async(request)
# Use timeout for result as well
future.result(timeout_sec=5.0)
self.lab_logger().debug(f"[Host Node] Re-register completed for {device_namespace}")
except Exception as e:
# Gracefully handle destruction during shutdown
if "destruction was requested" in str(e) or self._shutting_down:
self.lab_logger().debug(f"[Host Node] Re-register aborted for {device_namespace} (cleanup)")
else:
self.lab_logger().warning(f"[Host Node] Re-register failed for {device_namespace}: {e}")
def _discover_devices(self) -> None:
"""
@@ -331,23 +400,27 @@ class HostNode(BaseROS2DeviceNode):
self._create_action_clients_for_device(device_id, namespace)
self._online_devices.add(device_key)
sclient = self.create_client(SerialCommand, f"/srv{namespace}/re_register_device")
threading.Thread(
t = threading.Thread(
target=self._send_re_register,
args=(sclient,),
args=(sclient, namespace),
daemon=True,
name=f"ROSDevice{self.device_id}_re_register_device_{namespace}",
).start()
)
self._background_threads.append(t)
t.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}/re_register_device")
threading.Thread(
t = threading.Thread(
target=self._send_re_register,
args=(sclient,),
args=(sclient, namespace),
daemon=True,
name=f"ROSDevice{self.device_id}_re_register_device_{namespace}",
).start()
)
self._background_threads.append(t)
t.start()
# 检测离线设备
offline_devices = self._online_devices - current_devices
@@ -705,13 +778,14 @@ class HostNode(BaseROS2DeviceNode):
raise ValueError(f"ActionClient {action_id} not found.")
action_client: ActionClient = self._action_clients[action_id]
# 遍历action_kwargs下的所有子dict，将"sample_uuid"的值赋给"sample_id"
def assign_sample_id(obj):
if isinstance(obj, dict):
if "sample_uuid" in obj:
obj["sample_id"] = obj["sample_uuid"]
obj.pop("sample_uuid")
for k,v in obj.items():
for k, v in obj.items():
if k != "unilabos_extra":
assign_sample_id(v)
elif isinstance(obj, list):
@@ -742,9 +816,7 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info(f"[Host Node] Goal {action_id} ({item.job_id}) accepted")
self._goals[item.job_id] = goal_handle
goal_future = goal_handle.get_result_async()
goal_future.add_done_callback(
lambda f: self.get_result_callback(item, action_id, f)
)
goal_future.add_done_callback(lambda f: self.get_result_callback(item, action_id, f))
goal_future.result()
def feedback_callback(self, item: "QueueItem", action_id: str, feedback_msg) -> None:
@@ -1167,6 +1239,7 @@ class HostNode(BaseROS2DeviceNode):
"""
try:
from unilabos.app.web import http_client
data = json.loads(request.command)
if "uuid" in data and data["uuid"] is not None:
http_req = http_client.resource_tree_get([data["uuid"]], data["with_children"])