fix startup

add ResourceCreateFromOuter.action

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()

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

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]

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<unilabos_msgs/Resource>"]:
                     self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")

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  # 上次设备发现的时间

unilabos_msgs/action/ResourceCreateFromOuter.action

@@ -0,0 +1,5 @@
+Resource[] resources
+string[] device_ids
+---
+bool success
+---