diff --git a/unilabos/app/main.py b/unilabos/app/main.py index 16667d6c..9c85ea08 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -1,30 +1,24 @@ import argparse import asyncio +import json import os import signal import sys -import json +import threading import time +from copy import deepcopy import yaml -from copy import deepcopy -import threading - -import rclpy -from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker # 首先添加项目根目录到路径 current_dir = os.path.dirname(os.path.abspath(__file__)) -ilabos_dir = os.path.dirname(os.path.dirname(current_dir)) -if ilabos_dir not in sys.path: - sys.path.append(ilabos_dir) +unilabos_dir = os.path.dirname(os.path.dirname(current_dir)) +if unilabos_dir not in sys.path: + sys.path.append(unilabos_dir) -from unilabos.config.config import load_config, BasicConfig, _update_config_from_env +from unilabos.config.config import load_config, BasicConfig from unilabos.utils.banner_print import print_status, print_unilab_banner from unilabos.device_mesh.resource_visalization import ResourceVisualization -from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher -from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager -from rclpy.executors import MultiThreadedExecutor def parse_args(): @@ -83,9 +77,9 @@ def parse_args(): ) parser.add_argument( "--visual", - choices=["rviz", "web","None"], - default="rviz", - help="选择可视化工具: 'rviz' 或 'web' 或 'None',默认'rviz'", + choices=["rviz", "web", "disable"], + default="disable", + help="选择可视化工具: rviz, web", ) return parser.parse_args() @@ -137,7 +131,7 @@ def main(): # 注册表 build_registry(args_dict["registry_path"]) - + devices_and_resources = None if args_dict["graph"] is not None: import unilabos.resources.graphio as graph_res graph_res.physical_setup_graph = ( @@ -186,23 +180,21 @@ def main(): mqtt_client.start() args_dict["resources_mesh_config"] = {} - if args_dict["visual"] != "None": - if args_dict["visual"] == "rviz": - enable_rviz=True - elif args_dict["visual"] == "web": - enable_rviz=False - resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz) - - args_dict["resources_mesh_config"] = resource_visualization.resource_model - # 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 - - start_backend(**args_dict) - server_thread = threading.Thread(target=start_server) - server_thread.start() - asyncio.set_event_loop(asyncio.new_event_loop()) - resource_visualization.start() - while True: - time.sleep(1) + if args_dict["visual"] != "disable": + enable_rviz = args_dict["visual"] == "rviz" + if devices_and_resources is not None: + resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz) + args_dict["resources_mesh_config"] = resource_visualization.resource_model + start_backend(**args_dict) + server_thread = threading.Thread(target=start_server) + server_thread.start() + asyncio.set_event_loop(asyncio.new_event_loop()) + resource_visualization.start() + while True: + time.sleep(1) + else: + start_backend(**args_dict) + start_server() else: start_backend(**args_dict) start_server() diff --git a/unilabos/registry/devices/robot_gripper.yaml b/unilabos/registry/devices/robot_gripper.yaml index 3cb86fed..bae970ac 100644 --- a/unilabos/registry/devices/robot_gripper.yaml +++ b/unilabos/registry/devices/robot_gripper.yaml @@ -19,9 +19,6 @@ gripper.mock: result: position: position effort: torque - model: - type: device - mesh: opentrons_liquid_handler gripper.misumi_rz: description: Misumi RZ gripper diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py index 8560d376..944d4a04 100644 --- a/unilabos/ros/main_slave_run.py +++ b/unilabos/ros/main_slave_run.py @@ -48,14 +48,14 @@ def main( graph: Optional[Dict[str, Any]] = None, controllers_config: Dict[str, Any] = {}, bridges: List[Any] = [], - visual: str = "None", + visual: str = "disable", resources_mesh_config: dict = {}, - args: List[str] = ["--log-level", "debug"], + rclpy_init_args: List[str] = ["--log-level", "debug"], discovery_interval: float = 5.0, ) -> None: """主函数""" - rclpy.init(args=args) + rclpy.init(args=rclpy_init_args) executor = rclpy.__executor = MultiThreadedExecutor() # 创建主机节点 host_node = HostNode( @@ -96,11 +96,13 @@ def slave( graph: Optional[Dict[str, Any]] = None, controllers_config: Dict[str, Any] = {}, bridges: List[Any] = [], - args: List[str] = ["--log-level", "debug"], + visual: str = "disable", + resources_mesh_config: dict = {}, + rclpy_init_args: List[str] = ["--log-level", "debug"], ) -> None: """从节点函数""" if not rclpy.ok(): - rclpy.init(args=args) + rclpy.init(args=rclpy_init_args) executor = rclpy.__executor if not executor: executor = rclpy.__executor = MultiThreadedExecutor() @@ -136,7 +138,7 @@ def slave( logger.info(f"Slave node info updated.") rclient = n.create_client(ResourceAdd, "/resources/add") - rclient.wait_for_service() # FIXME 可能一直等待,加一个参数 + rclient.wait_for_service() request = ResourceAdd.Request() request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources_config] diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 7e032064..f1fb0506 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -438,7 +438,7 @@ class BaseROS2DeviceNode(Node, Generic[T]): action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"]) self.lab_logger().debug(f"接收到原始目标: {action_kwargs}") - # 向Host查询物料当前状态 + # 向Host查询物料当前状态,如果是host本身的增加物料的请求,则直接跳过 for k, v in goal.get_fields_and_field_types().items(): if v in ["unilabos_msgs/Resource", "sequence"]: self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}") diff --git a/unilabos/ros/nodes/presets/host_node.py b/unilabos/ros/nodes/presets/host_node.py index df1f3c25..4209ddff 100644 --- a/unilabos/ros/nodes/presets/host_node.py +++ b/unilabos/ros/nodes/presets/host_node.py @@ -101,9 +101,14 @@ class HostNode(BaseROS2DeviceNode): 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] = { + "add_resrouce": { + "type": ResourceCreateFromOuter, + "goal": { + + } + } + } # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系 self._goals: Dict[str, Any] = {} # 用来存储多个目标的状态 self._online_devices: Set[str] = set() # 用于跟踪在线设备 self._last_discovery_time = 0.0 # 上次设备发现的时间 diff --git a/unilabos_msgs/action/ResourceCreateFromOuter.action b/unilabos_msgs/action/ResourceCreateFromOuter.action new file mode 100644 index 00000000..8519158b --- /dev/null +++ b/unilabos_msgs/action/ResourceCreateFromOuter.action @@ -0,0 +1,5 @@ +Resource[] resources +string[] device_ids +--- +bool success +--- \ No newline at end of file