diff --git a/unilabos/app/main.py b/unilabos/app/main.py index 4bc65cf5..f7bd6f20 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -70,16 +70,20 @@ def parse_args(): help="信息页web服务的启动端口", ) parser.add_argument( - "--open_browser", - type=bool, - default=True, - help="是否在启动时打开信息页", + "--disable_browser", + action='store_true', + 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__": diff --git a/unilabos/config/config.py b/unilabos/config/config.py index 12ed4f6e..0cf999e6 100644 --- a/unilabos/config/config.py +++ b/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配置 diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index 912c6745..45daf454 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/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: diff --git a/unilabos/ros/utils/driver_creator.py b/unilabos/ros/utils/driver_creator.py index 4e018e7e..1218725e 100644 --- a/unilabos/ros/utils/driver_creator.py +++ b/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)