diff --git a/test/experiments/test.json b/test/experiments/test.json index 97ad5ec7..7ab8f87b 100644 --- a/test/experiments/test.json +++ b/test/experiments/test.json @@ -9,8 +9,8 @@ "type": "device", "class": "gripper.mock", "position": { - "x": 1000, - "y": 1000, + "x": 0, + "y": 0, "z": 0 }, "config": { diff --git a/unilabos/app/main.py b/unilabos/app/main.py index caa029c1..e819cb6b 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -71,7 +71,12 @@ def parse_args(): default=True, help="是否在启动时打开信息页", ) - + parser.add_argument( + "--visual", + choices=["rviz", "web","None"], + default="rviz", + help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'", + ) return parser.parse_args() @@ -164,16 +169,63 @@ def main(): signal.signal(signal.SIGTERM, _exit) mqtt_client.start() - resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict) - start_backend(**args_dict) - # print('-'*100) - # print(resource_visualization.resource_model) - # print('-'*100) - server_thread = threading.Thread(target=start_server) - server_thread.start() - - resource_visualization.start() + if args_dict["visual"] != "None": + if args_dict["visual"] == "rviz": + resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict) + elif args_dict["visual"] == "web": + resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict,enable_rviz=False ) + devices_config_add = add_resource_mesh_manager_node(resource_visualization.resource_model, args_dict["resources_config"]) + args_dict["devices_config"] = {**args_dict["devices_config"], **devices_config_add} + + server_thread = threading.Thread(target=start_server) + server_thread.start() + start_backend(**args_dict) + resource_visualization.start() + else: + start_backend(**args_dict) + start_server() +def add_resource_mesh_manager_node( + resource_model, + resource_config , + mesh_manager_device_id = "resource_mesh_manager", + joint_publisher_device_id = "joint_republisher"): + mesh_manager_config ={ + "id": mesh_manager_device_id, + "name": mesh_manager_device_id, + "children": [], + "parent": None, + "type": "device", + "class": "resource.mesh_manager", + "position": { + "x": 620.6111111111111, + "y": 171, + "z": 0 + }, + "config": { + "resource_model": resource_model, + "resource_config": resource_config + }, + "data": { + } + } + joint_publisher_config = { + "id": joint_publisher_device_id, + "name": joint_publisher_device_id, + "children": [], + "parent": None, + "type": "device", + "class": "joint_republisher", + "position": { + "x": 620.6111111111111, + "y": 171, + "z": 0 + }, + "config": {}, + "data": {} + } + return {joint_publisher_config["id"]: joint_publisher_config,mesh_manager_config["id"]: mesh_manager_config} + # return {joint_publisher_config["id"]: joint_publisher_config} if __name__ == "__main__": main() diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py index 997c4316..5cfd1fb2 100644 --- a/unilabos/device_mesh/resource_visalization.py +++ b/unilabos/device_mesh/resource_visalization.py @@ -119,12 +119,12 @@ class ResourceVisualization: }] ) - joint_state_publisher_node = nd( - package='joint_state_publisher_gui', # 或 joint_state_publisher - executable='joint_state_publisher_gui', - name='joint_state_publisher', - output='screen' - ) + # joint_state_publisher_node = nd( + # package='joint_state_publisher_gui', # 或 joint_state_publisher + # executable='joint_state_publisher_gui', + # name='joint_state_publisher', + # output='screen' + # ) # 创建move_group节点 move_group = nd( package='moveit_ros_move_group', @@ -147,7 +147,7 @@ class ResourceVisualization: # 将节点添加到launch描述中 self.launch_description.add_action(robot_state_publisher) - self.launch_description.add_action(joint_state_publisher_node) + # self.launch_description.add_action(joint_state_publisher_node) self.launch_description.add_action(move_group) # 如果启用RViz,添加RViz节点 diff --git a/unilabos/devices/ros_dev/joint_republisher.py b/unilabos/devices/ros_dev/joint_republisher.py index f9797ea9..b731acc7 100644 --- a/unilabos/devices/ros_dev/joint_republisher.py +++ b/unilabos/devices/ros_dev/joint_republisher.py @@ -3,13 +3,22 @@ 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(Node): - def __init__(self,device_id): - super().__init__(device_id) +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'/devices/{device_id}/joint_state_repub',10) + self.joint_repub = self.create_publisher(String,f'joint_state_repub',10) # 创建订阅者 self.create_subscription( JointState, diff --git a/unilabos/devices/ros_dev/resource_mesh_manager.py b/unilabos/devices/ros_dev/resource_mesh_manager.py index ec793892..eb32a0ca 100644 --- a/unilabos/devices/ros_dev/resource_mesh_manager.py +++ b/unilabos/devices/ros_dev/resource_mesh_manager.py @@ -19,18 +19,27 @@ from tf2_ros import TransformBroadcaster, Buffer, TransformListener from rclpy.action import ActionServer from unilabos_msgs.action import SendCmd from rclpy.action.server import ServerGoalHandle +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker -class ResourceMeshManager(Node): - def __init__(self, resource_model: dict, resource_config: list, node_name: str): +class ResourceMeshManager(BaseROS2DeviceNode): + def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager"): """初始化资源网格管理器节点 Args: resource_model (dict): 资源模型字典,包含资源的3D模型信息 resource_config (dict): 资源配置字典,包含资源的配置信息 - node_name (str): 节点名称 + device_id (str): 节点名称 """ - super().__init__(node_name) - + super().__init__( + driver_instance=self, + device_id=device_id, + status_types={}, + action_value_mappings={}, + hardware_interface={}, + print_publish=False, + resource_tracker=resource_tracker, + ) + self.resource_model = resource_model self.resource_config_dict = {item['id']: item for item in resource_config} self.move_group_ready = False @@ -38,7 +47,7 @@ class ResourceMeshManager(Node): self.tf_broadcaster = TransformBroadcaster(self) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - + self.old_resource_pose = {} self.__planning_scene = None @@ -49,7 +58,7 @@ class ResourceMeshManager(Node): callback_group = ReentrantCallbackGroup() self._get_planning_scene_service = self.create_client( srv_type=GetPlanningScene, - srv_name="get_planning_scene", + srv_name="/get_planning_scene", qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, @@ -62,7 +71,7 @@ class ResourceMeshManager(Node): # Create a service for applying the planning scene self._apply_planning_scene_service = self.create_client( srv_type=ApplyPlanningScene, - srv_name="apply_planning_scene", + srv_name="/apply_planning_scene", qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, @@ -73,7 +82,7 @@ class ResourceMeshManager(Node): ) self.resource_pose_publisher = self.create_publisher( - String, f"devices/{node_name}/resource_pose", 10 + String, f"resource_pose", 10 ) self.__collision_object_publisher = self.create_publisher( CollisionObject, "/collision_object", 10 @@ -86,7 +95,7 @@ class ResourceMeshManager(Node): self._action_server = ActionServer( self, SendCmd, - f'devices/{node_name}/tf_update', + f"tf_update", self.tf_update, callback_group=callback_group ) @@ -94,6 +103,7 @@ class ResourceMeshManager(Node): self.resource_mesh_setup() self.create_timer(0.02, self.publish_resource_tf) self.create_timer(1, self.check_resource_pose_changes) + def check_move_group_ready(self): @@ -101,8 +111,9 @@ class ResourceMeshManager(Node): # 获取当前可用的节点列表 - tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time()) - # print(tf_ready) + tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2)) + + # if tf_ready: if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready: self.move_group_ready = True self.add_resource_collision_meshes() @@ -5022,10 +5033,10 @@ if __name__ == '__main__': ''' resource_config = json.loads(config_s.replace("'",'"')) rclpy.init() - resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager') + resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, DeviceNodeResourceTracker()) # resource_mesh_manager.resource_mesh_setup() - resource_mesh_manager.check_move_group_ready() - resource_mesh_manager.publish_resource_tf() + # resource_mesh_manager.check_move_group_ready() + # resource_mesh_manager.publish_resource_tf() # resource_mesh_manager.clear_all_collision_objects() # print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False)) diff --git a/unilabos/registry/devices/sim_nodes.yaml b/unilabos/registry/devices/sim_nodes.yaml new file mode 100644 index 00000000..6f6fa256 --- /dev/null +++ b/unilabos/registry/devices/sim_nodes.yaml @@ -0,0 +1,11 @@ +joint_republisher: + class: + module: unilabos.devices.ros_dev.joint_republisher:JointRepublisher + type: ros2 + +resource.mesh_manager: + class: + module: unilabos.devices.ros_dev.resource_mesh_manager:ResourceMeshManager + type: ros2 + +