Merge remote-tracking branch 'origin/dev' into device_visualization

unilabos/app/main.py

@@ -70,16 +70,20 @@ def parse_args():
         help="信息页web服务的启动端口",
     )
     parser.add_argument(
-        "--open_browser",
+        "--disable_browser",
-        type=bool,
+        action='store_true',
-        default=True,
+        help="是否在启动时关闭信息页",
-        help="是否在启动时打开信息页",
+    )
+    parser.add_argument(
+        "--2d_vis",
+        action='store_true',
+        help="是否在pylabrobot实例启动时，同时启动可视化",
     )
     parser.add_argument(
         "--visual",
-        choices=["rviz", "web", "deck", "disable"],
+        choices=["rviz", "web", "disable"],
         default="disable",
-        help="选择可视化工具: rviz, web, deck(2D bird view)",
+        help="选择可视化工具: rviz, web",
     )
     return parser.parse_args()
 
@@ -111,6 +115,7 @@ def main():
     machine_name = os.popen("hostname").read().strip()
     machine_name = "".join([c if c.isalnum() or c == "_" else "_" for c in machine_name])
     BasicConfig.machine_name = machine_name
+    BasicConfig.vis_2d_enable = args_dict["2d_vis"]
 
     from unilabos.resources.graphio import (
         read_node_link_json,
@@ -186,7 +191,7 @@ def main():
             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 = threading.Thread(target=start_server, args=(not args_dict["disable_browser"],))
             server_thread.start()
             asyncio.set_event_loop(asyncio.new_event_loop())
             resource_visualization.start()
@@ -194,10 +199,10 @@ def main():
                 time.sleep(1)
         else:
             start_backend(**args_dict)
-            start_server()
+            start_server(open_browser=not args_dict["disable_browser"])
     else:
         start_backend(**args_dict)
-        start_server()
+        start_server(open_browser=not args_dict["disable_browser"])
 
 
 if __name__ == "__main__":

unilabos/config/config.py

@@ -13,6 +13,7 @@ class BasicConfig:
     is_host_mode = True  # 从registry.py移动过来
     slave_no_host = False  # 是否跳过rclient.wait_for_service()
     machine_name = "undefined"
+    vis_2d_enable = False
 
 
 # MQTT配置

unilabos/ros/nodes/base_device_node.py

@@ -10,14 +10,16 @@ import asyncio
 
 import rclpy
 from rclpy.node import Node
-from rclpy.action import ActionServer
+from rclpy.action import ActionServer, ActionClient
 from rclpy.action.server import ServerGoalHandle
 from rclpy.client import Client
 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.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr
+from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr, \
+    initialize_resources
 from unilabos.ros.msgs.message_converter import (
     convert_to_ros_msg,
     convert_from_ros_msg,
@@ -317,25 +319,50 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             location = command_json["bind_location"]
             other_calling_param = command_json["other_calling_param"]
             resources = command_json["resource"]
+            initialize_full = other_calling_param.pop("initialize_full", False)
             # 本地拿到这个物料，可能需要先做初始化?
             if isinstance(resources, list):
+                if initialize_full:
+                    resources = initialize_resources(resources)
                 request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources]
             else:
+                if initialize_full:
+                    resources = initialize_resources([resources])
                 request.resources = [convert_to_ros_msg(Resource, resources)]
             response = rclient.call(request)
-            # 应该本地先add_resource？
+            # 应该先add_resource了
             res.response = "OK"
             # 接下来该根据bind_parent_id进行assign了，目前只有plr可以进行assign，不然没有办法输入到物料系统中
             resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
+            request.resources = [convert_to_ros_msg(Resource, resources)]
+
             try:
                 from pylabrobot.resources.resource import Resource as ResourcePLR
                 from pylabrobot.resources.deck import Deck
                 from pylabrobot.resources import Coordinate
+                from pylabrobot.resources import OTDeck
                 contain_model = not isinstance(resource, Deck)
                 if isinstance(resource, ResourcePLR):
                     # resources.list()
                     plr_instance = resource_ulab_to_plr(resources, contain_model)
+                    if isinstance(resource, OTDeck) and "slot" in other_calling_param:
+                        resource.assign_child_at_slot(plr_instance, **other_calling_param)
                     resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param)
+                # 发送给ResourceMeshManager
+                action_client = ActionClient(
+                    self, SendCmd, "/devices/resource_mesh_manager/add_resource_mesh", callback_group=self.callback_group
+                )
+                goal = SendCmd.Goal()
+                goal.command = json.dumps({
+                    "resources": resources,
+                    "bind_parent_id": bind_parent_id,
+                })
+                future = action_client.send_goal_async(goal, goal_uuid=uuid.uuid4())
+
+                def done_cb(*args):
+                    self.lab_logger().info(f"向meshmanager发送新增resource完成")
+
+                future.add_done_callback(done_cb)
             except ImportError:
                 self.lab_logger().error("Host请求添加物料时，本环境并不存在pylabrobot")
             except Exception as e:

unilabos/ros/utils/driver_creator.py

@@ -226,6 +226,14 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
             from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
             def done_cb(*args):
                 logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成")
+                from unilabos.config.config import BasicConfig
+                if BasicConfig.vis_2d_enable:
+                    from pylabrobot.visualizer.visualizer import Visualizer
+                    vis = Visualizer(resource=self.device_instance, open_browser=True)
+                    def vis_done_cb(*args):
+                        logger.info(f"PyLabRobot设备实例开启了Visualizer {self.device_instance}")
+                    ROS2DeviceNode.run_async_func(vis.setup).add_done_callback(vis_done_cb)
+                    logger.debug(f"PyLabRobot设备实例提交开启Visualizer {self.device_instance}")
             ROS2DeviceNode.run_async_func(getattr(self.device_instance, "setup")).add_done_callback(done_cb)