mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 13:01:12 +00:00
Dev v0.9.0 (#23)
Add high-level PLR functions Add Laiyu/Zhida driver support Fix ROS node discovery issues Add hostname and resource query support Fix ROS message conversion logic Support configuration via environment variables * Update README and MQTTClient for installation instructions and code improvements * feat: 支持local_config启动 add: 增加对crt path的说明，为传入config.py的相对路径 move: web component * add: registry description * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 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/ * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * fix: device.class possible null * fix: HPLC additions with online service * fix: slave mode spin not working * fix: slave mode spin not working * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * feat: 多ProtocolNode 允许子设备ID相同 feat: 上报发现的ActionClient feat: Host重启动，通过discover机制要求slaveNode重新注册，实现信息及时上报 * feat: 支持env设置config * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 * Device visualization (#14) * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> * fix: missing hostname in devices_names fix: upload_file for model file * fix: missing paho-mqtt package bump version to 0.9.0 * fix startup add ResourceCreateFromOuter.action * fix type hint * update actions * update actions * host node add_resource_from_outer fix cmake list * pass device config to device class * add: bind_parent_ids to resource create action fix: message convert string * fix: host node should not be re_discovered * feat: resource tracker support dict * feat: add more necessary params * feat: fix boolean null in registry action data * feat: add outer resource * 编写mesh添加action * feat: append resource * add action * feat: vis 2d for plr * fix * fix: browser on rviz * fix: cloud bridge error fallback to local * fix: salve auto run rviz * 初始化两个plate * Device visualization (#22) * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model 添加物料模型管理类，遍历物料与resource_model，完成TF数据收集 * 完成TF发布 * 修改模型方向，在yaml中添加变换属性 * 添加物料tf变化时，发送topic到前端 另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 * 编写mesh添加action * add action * fix * fix: browser on rviz * fix: cloud bridge error fallback to local * fix: salve auto run rviz * 初始化两个plate --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> * fix: multi channel * fix: aspirate * fix: aspirate * fix: aspirate * fix: aspirate * 提交 * fix: jobadd * fix: jobadd * fix: msg converter * tijiao --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: q434343 <73513873+q434343@users.noreply.github.com>
This commit is contained in:
Xuwznln
GitHub
@@ -7,11 +7,13 @@ import uuid
|
||||
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, SerialCommand # type: ignore
|
||||
from geometry_msgs.msg import Point
|
||||
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 unilabos_msgs.msg import Resource # type: ignore
|
||||
from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, \
|
||||
SerialCommand # type: ignore
|
||||
from unique_identifier_msgs.msg import UUID
|
||||
|
||||
from unilabos.registry.registry import lab_registry
|
||||
@@ -23,11 +25,9 @@ from unilabos.ros.msgs.message_converter import (
|
||||
convert_from_ros_msg,
|
||||
convert_to_ros_msg,
|
||||
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):
|
||||
@@ -50,7 +50,7 @@ class HostNode(BaseROS2DeviceNode):
|
||||
self,
|
||||
device_id: str,
|
||||
devices_config: Dict[str, Any],
|
||||
resources_config: Any,
|
||||
resources_config: list,
|
||||
physical_setup_graph: Optional[Dict[str, Any]] = None,
|
||||
controllers_config: Optional[Dict[str, Any]] = None,
|
||||
bridges: Optional[List[Any]] = None,
|
||||
@@ -76,7 +76,7 @@ class HostNode(BaseROS2DeviceNode):
|
||||
driver_instance=self,
|
||||
device_id=device_id,
|
||||
status_types={},
|
||||
action_value_mappings={},
|
||||
action_value_mappings=lab_registry.device_type_registry["host_node"]["class"]["action_value_mappings"],
|
||||
hardware_interface={},
|
||||
print_publish=False,
|
||||
resource_tracker=DeviceNodeResourceTracker(), # host node并不是通过initialize 包一层传进来的
|
||||
@@ -97,15 +97,13 @@ class HostNode(BaseROS2DeviceNode):
|
||||
self.bridges = bridges
|
||||
|
||||
# 创建设备、动作客户端和目标存储
|
||||
self.devices_names: Dict[str, str] = {} # 存储设备名称和命名空间的映射
|
||||
self.devices_names: Dict[str, str] = {device_id: self.namespace} # 存储设备名称和命名空间的映射
|
||||
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] = (
|
||||
{}
|
||||
) # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
|
||||
self._action_value_mappings: Dict[str, Dict] = {} # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
|
||||
self._goals: Dict[str, Any] = {} # 用来存储多个目标的状态
|
||||
self._online_devices: Set[str] = set() # 用于跟踪在线设备
|
||||
self._online_devices: Set[str] = {f"{self.namespace}/{device_id}"} # 用于跟踪在线设备
|
||||
self._last_discovery_time = 0.0 # 上次设备发现的时间
|
||||
self._discovery_lock = threading.Lock() # 设备发现的互斥锁
|
||||
self._subscribed_topics = set() # 用于跟踪已订阅的话题
|
||||
@@ -259,16 +257,41 @@ class HostNode(BaseROS2DeviceNode):
|
||||
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_name, {
|
||||
"device_id": edge_device_id,
|
||||
"action_name": action_name,
|
||||
"schema": info_with_schema,
|
||||
})
|
||||
# from unilabos.app.mq import mqtt_client
|
||||
# info_with_schema = ros_action_to_json_schema(action_type)
|
||||
# mqtt_client.publish_actions(action_name, {
|
||||
# "device_id": edge_device_id,
|
||||
# "device_type": "",
|
||||
# "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 add_resource_from_outer(self, resources: list["Resource"], device_ids: list[str], bind_parent_ids: list[str], bind_locations: list[Point], other_calling_params: list[str]):
|
||||
for resource, device_id, bind_parent_id, bind_location, other_calling_param in zip(resources, device_ids, bind_parent_ids, bind_locations, other_calling_params):
|
||||
# 这里要求device_id传入必须是edge_device_id
|
||||
namespace = "/devices/" + device_id
|
||||
srv_address = f"/srv{namespace}/append_resource"
|
||||
sclient = self.create_client(SerialCommand, srv_address)
|
||||
sclient.wait_for_service()
|
||||
request = SerialCommand.Request()
|
||||
request.command = json.dumps({
|
||||
"resource": resource,
|
||||
"namespace": namespace,
|
||||
"edge_device_id": device_id,
|
||||
"bind_parent_id": bind_parent_id,
|
||||
"bind_location": {
|
||||
"x": bind_location.x,
|
||||
"y": bind_location.y,
|
||||
"z": bind_location.z,
|
||||
},
|
||||
"other_calling_param": json.loads(other_calling_param) if other_calling_param else {},
|
||||
}, ensure_ascii=False)
|
||||
response = sclient.call(request)
|
||||
pass
|
||||
pass
|
||||
|
||||
def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
|
||||
"""
|
||||
根据配置初始化设备，
|
||||
@@ -297,13 +320,14 @@ class HostNode(BaseROS2DeviceNode):
|
||||
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 (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_name, {
|
||||
"device_id": device_id,
|
||||
"action_name": action_name,
|
||||
"schema": info_with_schema,
|
||||
})
|
||||
# from unilabos.app.mq import mqtt_client
|
||||
# info_with_schema = ros_action_to_json_schema(action_type)
|
||||
# mqtt_client.publish_actions(action_name, {
|
||||
# "device_id": device_id,
|
||||
# "device_type": device_config["class"],
|
||||
# "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_id
|
||||
@@ -619,7 +643,8 @@ class HostNode(BaseROS2DeviceNode):
|
||||
self.lab_logger().debug(f"[Host Node-Resource] Retrieved from bridge: {len(r)} resources")
|
||||
except Exception as e:
|
||||
self.lab_logger().error(f"[Host Node-Resource] Error retrieving from bridge: {str(e)}")
|
||||
r = []
|
||||
r = [resource for resource in self.resources_config if resource.get("id") == request.id]
|
||||
self.lab_logger().warning(f"[Host Node-Resource] Retrieved from local: {len(r)} resources")
|
||||
else:
|
||||
# 本地物料服务，根据 id 查询物料
|
||||
r = [resource for resource in self.resources_config if resource.get("id") == request.id]
|
||||
|
||||
60
unilabos/ros/nodes/presets/joint_republisher.py
Normal file
60
unilabos/ros/nodes/presets/joint_republisher.py
Normal file
@@ -0,0 +1,60 @@
|
||||
import rclpy,json
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import String
|
||||
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):
|
||||
super().__init__(
|
||||
driver_instance=self,
|
||||
device_id=device_id,
|
||||
status_types={},
|
||||
action_value_mappings={},
|
||||
hardware_interface={},
|
||||
print_publish=False,
|
||||
resource_tracker=resource_tracker,
|
||||
)
|
||||
|
||||
# print('-'*20,device_id)
|
||||
self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
|
||||
# 创建订阅者
|
||||
self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.listener_callback,
|
||||
10,
|
||||
callback_group=ReentrantCallbackGroup()
|
||||
)
|
||||
self.msg = String()
|
||||
|
||||
def listener_callback(self, msg:JointState):
|
||||
|
||||
try:
|
||||
json_dict = {}
|
||||
json_dict["name"] = list(msg.name)
|
||||
json_dict["position"] = list(msg.position)
|
||||
json_dict["velocity"] = list(msg.velocity)
|
||||
json_dict["effort"] = list(msg.effort)
|
||||
|
||||
self.msg.data = str(json_dict)
|
||||
self.joint_repub.publish(self.msg)
|
||||
# print('-'*20)
|
||||
# print(self.msg.data)
|
||||
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
rclpy.init()
|
||||
subscriber = JointRepublisher()
|
||||
rclpy.spin(subscriber)
|
||||
subscriber.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
1139
unilabos/ros/nodes/presets/resource_mesh_manager.py
Normal file
1139
unilabos/ros/nodes/presets/resource_mesh_manager.py
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user