open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

add action

Browse Source
This commit is contained in:
zhangshixiang
2025-05-06 23:27:42 +08:00
parent d199fda9a5
commit 3d607db49a
1 changed files with 28 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -117,8 +117,33 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.create_timer(1/self.rate, self.publish_resource_tf)
self.create_timer(1/self.rate, self.check_resource_pose_changes)
def check_move_group_ready(self):
"""检查move_group节点是否已初始化完成"""
def add_resource_mesh_callback(self,resource_config_str:str):
# 获取当前可用的节点列表
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.publish_resource_tf()
self.add_resource_collision_meshes(self.resource_tf_dict)
# time.sleep(1)
def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
tf_update_msg = goal_handle.request
try:
self.add_resource_mesh(tf_update_msg.command)
except Exception as e:
self.get_logger().error(f"添加资源失败: {e}")
goal_handle.abort()
return SendCmd.Result(success=False)
goal_handle.succeed()
return SendCmd.Result(success=True)
def add_resource_mesh(self,resource_config_str:str):
"""刷新资源配置"""
registry = lab_registry
@@ -147,22 +172,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.add_resource_collision_meshes(tf_dict)
def check_move_group_ready(self):
"""检查move_group节点是否已初始化完成"""
# 获取当前可用的节点列表
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.publish_resource_tf()
self.add_resource_collision_meshes(self.resource_tf_dict)
# time.sleep(1)
def resource_mesh_setup(self, resource_config_dict:dict):
"""move_group初始化完成后的设置"""
self.get_logger().info('开始设置资源网格管理器')