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

Merge dev branch: Add battery resources, bioyond_cell device registry, and fix file path resolution

Browse Source
This commit is contained in:
dijkstra402
2025-12-18 11:11:59 +08:00
parent 5072f00836 143104e9e3
commit 9f8b5eaf71
306 changed files with 47518 additions and 4826 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
unilabos/ros/nodes/presets/camera.py
View File

@@ -6,12 +6,13 @@ from cv_bridge import CvBridge
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker
class VideoPublisher(BaseROS2DeviceNode):
def __init__(self, device_id='video_publisher', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
def __init__(self, device_id='video_publisher', device_uuid='', camera_index=0, period: float = 0.1, resource_tracker: DeviceNodeResourceTracker = None):
# 初始化BaseROS2DeviceNode，使用自身作为driver_instance
BaseROS2DeviceNode.__init__(
self,
driver_instance=self,
device_id=device_id,
device_uuid=device_uuid,
status_types={},
action_value_mappings={},
hardware_interface="camera",

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

@@ -5,7 +5,7 @@ import threading
import time
import traceback
import uuid
from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, Union
from typing import TYPE_CHECKING, Optional, Dict, Any, List, ClassVar, Set, TypedDict, Union
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import Point
@@ -38,6 +38,7 @@ from unilabos.ros.msgs.message_converter import (
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
from unilabos.ros.nodes.presets.controller_node import ControllerNode
from unilabos.ros.nodes.resource_tracker import (
ResourceDict,
ResourceDictInstance,
ResourceTreeSet,
ResourceTreeInstance,
@@ -48,7 +49,7 @@ from unilabos.utils.type_check import serialize_result_info
from unilabos.registry.placeholder_type import ResourceSlot, DeviceSlot
if TYPE_CHECKING:
from unilabos.app.ws_client import QueueItem, WSResourceChatData
from unilabos.app.ws_client import QueueItem
@dataclass
@@ -56,6 +57,11 @@ class DeviceActionStatus:
job_ids: Dict[str, float] = field(default_factory=dict)
class TestResourceReturn(TypedDict):
resources: List[List[ResourceDict]]
devices: List[DeviceSlot]
class HostNode(BaseROS2DeviceNode):
"""
主机节点类，负责管理设备、资源和控制器
@@ -283,6 +289,12 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info("[Host Node] Host node initialized.")
HostNode._ready_event.set()
# 发送host_node ready信号到所有桥接器
for bridge in self.bridges:
if hasattr(bridge, "publish_host_ready"):
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()
@@ -526,7 +538,7 @@ class HostNode(BaseROS2DeviceNode):
self.lab_logger().info(f"[Host Node] Initializing device: {device_id}")
try:
d = initialize_device_from_dict(device_id, device_config.get_nested_dict())
d = initialize_device_from_dict(device_id, device_config)
except DeviceClassInvalid as e:
self.lab_logger().error(f"[Host Node] Device class invalid: {e}")
d = None
@@ -706,7 +718,7 @@ class HostNode(BaseROS2DeviceNode):
feedback_callback=lambda feedback_msg: self.feedback_callback(item, action_id, feedback_msg),
goal_uuid=goal_uuid_obj,
)
future.add_done_callback(lambda future: self.goal_response_callback(item, action_id, future))
future.add_done_callback(lambda f: self.goal_response_callback(item, action_id, f))
def goal_response_callback(self, item: "QueueItem", action_id: str, future) -> None:
"""目标响应回调"""
@@ -717,9 +729,11 @@ 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_handle.get_result_async().add_done_callback(
lambda future: self.get_result_callback(item, action_id, future)
goal_future = goal_handle.get_result_async()
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:
"""反馈回调"""
@@ -734,46 +748,133 @@ class HostNode(BaseROS2DeviceNode):
def get_result_callback(self, item: "QueueItem", action_id: str, future) -> None:
"""获取结果回调"""
job_id = item.job_id
result_msg = future.result().result
result_data = convert_from_ros_msg(result_msg)
status = "success"
return_info_str = result_data.get("return_info")
if return_info_str is not None:
try:
return_info = json.loads(return_info_str)
suc = return_info.get("suc", False)
if not suc:
status = "failed"
except json.JSONDecodeError:
try:
result = future.result()
result_msg = result.result
goal_status = result.status
# 检查是否是被取消的任务
if goal_status == GoalStatus.STATUS_CANCELED:
self.lab_logger().info(f"[Host Node] Goal {action_id} ({job_id[:8]}) was cancelled")
status = "failed"
return_info = serialize_result_info("", False, result_data)
self.lab_logger().critical("错误的return_info类型，请断点修复")
else:
# 无 return_info 字段时，回退到 success 字段（若存在）
suc_field = result_data.get("success")
if isinstance(suc_field, bool):
status = "success" if suc_field else "failed"
return_info = serialize_result_info("", suc_field, result_data)
return_info = serialize_result_info("Job was cancelled", False, {})
else:
# 最保守的回退：标记失败并返回空JSON
status = "failed"
return_info = serialize_result_info("缺少return_info", False, result_data)
result_data = convert_from_ros_msg(result_msg)
status = "success"
return_info_str = result_data.get("return_info")
if return_info_str is not None:
try:
return_info = json.loads(return_info_str)
# 适配后端的一些额外处理
return_value = return_info.get("return_value")
if isinstance(return_value, dict):
unilabos_samples = return_info.get("unilabos_samples")
if isinstance(unilabos_samples, list):
return_info["unilabos_samples"] = unilabos_samples
suc = return_info.get("suc", False)
if not suc:
status = "failed"
except json.JSONDecodeError:
status = "failed"
return_info = serialize_result_info("", False, result_data)
self.lab_logger().critical("错误的return_info类型，请断点修复")
else:
# 无 return_info 字段时，回退到 success 字段（若存在）
suc_field = result_data.get("success")
if isinstance(suc_field, bool):
status = "success" if suc_field else "failed"
return_info = serialize_result_info("", suc_field, result_data)
else:
# 最保守的回退：标记失败并返回空JSON
status = "failed"
return_info = serialize_result_info("缺少return_info", False, result_data)
self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id}): {status}")
self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
self.lab_logger().info(f"[Host Node] Result for {action_id} ({job_id[:8]}): {status}")
if goal_status != GoalStatus.STATUS_CANCELED:
self.lab_logger().debug(f"[Host Node] Result data: {result_data}")
if job_id:
# 清理 _goals 中的记录
if job_id in self._goals:
del self._goals[job_id]
self.lab_logger().debug(f"[Host Node] Removed goal {job_id[:8]} from _goals")
# 存储结果供 HTTP API 查询
try:
from unilabos.app.web.controller import store_job_result
if goal_status == GoalStatus.STATUS_CANCELED:
store_job_result(job_id, status, return_info, {})
else:
store_job_result(job_id, status, return_info, result_data)
except ImportError:
pass # controller 模块可能未加载
# 发布状态到桥接器
if job_id:
for bridge in self.bridges:
if hasattr(bridge, "publish_job_status"):
if goal_status == GoalStatus.STATUS_CANCELED:
bridge.publish_job_status({}, item, status, return_info)
else:
bridge.publish_job_status(result_data, item, status, return_info)
except Exception as e:
self.lab_logger().error(
f"[Host Node] Error in get_result_callback for {action_id} ({job_id[:8]}): {str(e)}"
)
import traceback
self.lab_logger().error(traceback.format_exc())
# 清理 _goals 中的记录
if job_id in self._goals:
del self._goals[job_id]
# 发布失败状态
for bridge in self.bridges:
if hasattr(bridge, "publish_job_status"):
bridge.publish_job_status(result_data, item, status, return_info)
bridge.publish_job_status(
{}, item, "failed", serialize_result_info(f"Callback error: {str(e)}", False, {})
)
def cancel_goal(self, goal_uuid: str) -> None:
"""取消目标"""
def cancel_goal(self, goal_uuid: str) -> bool:
"""
取消目标
Args:
goal_uuid: 目标UUID（job_id）
Returns:
bool: 如果找到目标并发起取消请求返回True，否则返回False
"""
if goal_uuid in self._goals:
self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid}")
self._goals[goal_uuid].cancel_goal_async()
self.lab_logger().info(f"[Host Node] Cancelling goal {goal_uuid[:8]}")
goal_handle = self._goals[goal_uuid]
# 发起异步取消请求
cancel_future = goal_handle.cancel_goal_async()
# 添加取消完成的回调
cancel_future.add_done_callback(lambda future: self._cancel_goal_callback(goal_uuid, future))
return True
else:
self.lab_logger().warning(f"[Host Node] Goal {goal_uuid} not found, cannot cancel")
self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} not found in _goals, cannot cancel")
return False
def _cancel_goal_callback(self, goal_uuid: str, future) -> None:
"""取消目标的回调"""
try:
cancel_response = future.result()
if cancel_response.goals_canceling:
self.lab_logger().info(f"[Host Node] Goal {goal_uuid[:8]} cancel request accepted")
else:
self.lab_logger().warning(f"[Host Node] Goal {goal_uuid[:8]} cancel request rejected")
except Exception as e:
self.lab_logger().error(f"[Host Node] Error cancelling goal {goal_uuid[:8]}: {str(e)}")
import traceback
self.lab_logger().error(traceback.format_exc())
def get_goal_status(self, job_id: str) -> int:
"""获取目标状态"""
@@ -1056,11 +1157,12 @@ 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 = self.bridges[-1].resource_tree_get([data["uuid"]], data["with_children"])
http_req = http_client.resource_tree_get([data["uuid"]], data["with_children"])
elif "id" in data and data["id"].startswith("/"):
http_req = self.bridges[-1].resource_get(data["id"], data["with_children"])
http_req = http_client.resource_get(data["id"], data["with_children"])
else:
raise ValueError("没有使用正确的物料 id 或 uuid")
response.response = json.dumps(http_req["data"])
@@ -1270,7 +1372,7 @@ class HostNode(BaseROS2DeviceNode):
def test_resource(
self, resource: ResourceSlot, resources: List[ResourceSlot], device: DeviceSlot, devices: List[DeviceSlot]
):
) -> TestResourceReturn:
return {
"resources": ResourceTreeSet.from_plr_resources([resource, *resources]).dump(),
"devices": [device, *devices],

4
unilabos/ros/nodes/presets/joint_republisher.py
View File

@@ -1,3 +1,4 @@
import uuid
import rclpy,json
from rclpy.node import Node
from sensor_msgs.msg import JointState
@@ -6,7 +7,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
class JointRepublisher(BaseROS2DeviceNode):
def __init__(self,device_id,resource_tracker):
def __init__(self,device_id,resource_tracker, **kwargs):
super().__init__(
driver_instance=self,
device_id=device_id,
@@ -15,6 +16,7 @@ class JointRepublisher(BaseROS2DeviceNode):
hardware_interface={},
print_publish=False,
resource_tracker=resource_tracker,
device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
)
# print('-'*20,device_id)

24
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -1,5 +1,6 @@
from pathlib import Path
import time
import uuid
import rclpy,json
from rclpy.node import Node
from std_msgs.msg import String,Header
@@ -25,7 +26,7 @@ from unilabos.resources.graphio import initialize_resources
from unilabos.registry.registry import lab_registry
class ResourceMeshManager(BaseROS2DeviceNode):
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50):
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50, **kwargs):
"""初始化资源网格管理器节点
Args:
@@ -41,10 +42,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
hardware_interface={},
print_publish=False,
resource_tracker=resource_tracker,
device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
)
self.resource_model = resource_model
self.resource_config_dict = {item['id']: item for item in resource_config}
self.resource_config_dict = {item['uuid']: item for item in resource_config}
self.move_group_ready = False
self.resource_tf_dict = {}
self.tf_broadcaster = TransformBroadcaster(self)
@@ -182,14 +184,16 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.get_logger().info('开始设置资源网格管理器')
#遍历resource_config中的资源配置，判断panent是否在resource_model中，
resource_tf_dict = {}
for resource_id, resource_config in resource_config_dict.items():
for resource_uuid, resource_config in resource_config_dict.items():
parent = None
resource_id = resource_config['id']
if resource_config['parent_uuid'] is not None and resource_config['parent_uuid'] != "":
parent = resource_config_dict[resource_config['parent_uuid']]['id']
parent = resource_config['parent']
parent_link = 'world'
if parent in self.resource_model:
parent_link = parent
elif parent is None and resource_id in self.resource_model:
pass
elif parent is not None and resource_id in self.resource_model:
# parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
@@ -199,9 +203,9 @@ class ResourceMeshManager(BaseROS2DeviceNode):
continue
# 提取位置信息并转换单位
position = {
"x": float(resource_config['position']['x'])/1000,
"y": float(resource_config['position']['y'])/1000,
"z": float(resource_config['position']['z'])/1000
"x": float(resource_config['position']['position']['x'])/1000,
"y": float(resource_config['position']['position']['y'])/1000,
"z": float(resource_config['position']['position']['z'])/1000
}
rotation_dict = {
@@ -210,8 +214,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"z": 0
}
if 'rotation' in resource_config['config']:
rotation_dict = resource_config['config']['rotation']
if 'rotation' in resource_config['position']:
rotation_dict = resource_config['position']['rotation']
# 从欧拉角转换为四元数
q = quaternion_from_euler(

16
unilabos/ros/nodes/presets/workstation.py
View File

@@ -24,7 +24,7 @@ from unilabos.ros.msgs.message_converter import (
convert_from_ros_msg_with_mapping,
)
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, DeviceNodeResourceTracker, ROS2DeviceNode
from unilabos.ros.nodes.resource_tracker import ResourceTreeSet
from unilabos.ros.nodes.resource_tracker import ResourceTreeSet, ResourceDictInstance
from unilabos.utils.type_check import get_result_info_str
if TYPE_CHECKING:
@@ -47,7 +47,7 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
def __init__(
self,
protocol_type: List[str],
children: Dict[str, Any],
children: List[ResourceDictInstance],
*,
driver_instance: "WorkstationBase",
device_id: str,
@@ -81,10 +81,11 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
# 初始化子设备
self.communication_node_id_to_instance = {}
for device_id, device_config in self.children.items():
if device_config.get("type", "device") != "device":
for device_config in self.children:
device_id = device_config.res_content.id
if device_config.res_content.type != "device":
self.lab_logger().debug(
f"[Protocol Node] Skipping type {device_config['type']} {device_id} already existed, skipping."
f"[Protocol Node] Skipping type {device_config.res_content.type} {device_id} already existed, skipping."
)
continue
try:
@@ -101,8 +102,9 @@ class ROS2WorkstationNode(BaseROS2DeviceNode):
self.communication_node_id_to_instance[device_id] = d
continue
for device_id, device_config in self.children.items():
if device_config.get("type", "device") != "device":
for device_config in self.children:
device_id = device_config.res_content.id
if device_config.res_content.type != "device":
continue
# 设置硬件接口代理
if device_id not in self.sub_devices: