mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-03-24 05:42:10 +00:00
1349 lines
52 KiB
Python
1349 lines
52 KiB
Python
from pathlib import Path
|
||
import time
|
||
import uuid
|
||
import rclpy,json
|
||
from rclpy.node import Node
|
||
from std_msgs.msg import String,Header
|
||
import numpy as np
|
||
from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene
|
||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
|
||
from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry, RobotState, PlanningScene
|
||
from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive
|
||
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped
|
||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
|
||
from rclpy.task import Future
|
||
import copy
|
||
from typing import Tuple, Optional, Union, Any, List
|
||
from tf_transformations import quaternion_from_euler
|
||
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
|
||
from unilabos.resources.graphio import initialize_resources
|
||
from unilabos.resources.resource_tracker import EXTRA_CLASS
|
||
from unilabos.registry.registry import lab_registry
|
||
|
||
class ResourceMeshManager(BaseROS2DeviceNode):
|
||
def __init__(
|
||
self,
|
||
resource_model: Optional[dict] = None,
|
||
resource_config: Optional[list] = None,
|
||
resource_tracker=None,
|
||
device_id: str = "resource_mesh_manager",
|
||
registry_name: str = "",
|
||
rate=50,
|
||
**kwargs,
|
||
):
|
||
"""初始化资源网格管理器节点
|
||
|
||
Args:
|
||
resource_model: 资源模型字典（可选，为 None 时自动从 registry 构建）
|
||
resource_config: 资源配置列表（可选，为 None 时启动后通过 ActionServer 或 load_from_resource_tree 加载）
|
||
resource_tracker: 资源追踪器
|
||
device_id: 节点名称
|
||
rate: TF 发布频率
|
||
"""
|
||
if resource_tracker is None:
|
||
resource_tracker = DeviceNodeResourceTracker()
|
||
|
||
super().__init__(
|
||
driver_instance=self,
|
||
device_id=device_id,
|
||
registry_name=registry_name,
|
||
status_types={},
|
||
action_value_mappings={},
|
||
hardware_interface={},
|
||
print_publish=False,
|
||
resource_tracker=resource_tracker,
|
||
device_uuid=kwargs.get("uuid", str(uuid.uuid4())),
|
||
)
|
||
|
||
self.resource_model = resource_model if resource_model is not None else {}
|
||
self.resource_config_dict = (
|
||
{item['uuid']: item for item in resource_config} if resource_config else {}
|
||
)
|
||
self.move_group_ready = False
|
||
self.resource_tf_dict = {}
|
||
self.tf_broadcaster = TransformBroadcaster(self)
|
||
self.tf_buffer = Buffer()
|
||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||
self.rate = rate
|
||
self.zero_count = 0
|
||
|
||
self.old_resource_pose = {}
|
||
self.__planning_scene = PlanningScene()
|
||
self.__old_planning_scene = None
|
||
self.__old_allowed_collision_matrix = None
|
||
self.mesh_path = Path(__file__).parent.parent.parent.parent.absolute()
|
||
self.msg_type = 'resource_status'
|
||
self.resource_status_dict = {}
|
||
|
||
callback_group = ReentrantCallbackGroup()
|
||
self._get_planning_scene_service = self.create_client(
|
||
srv_type=GetPlanningScene,
|
||
srv_name="/get_planning_scene",
|
||
qos_profile=QoSProfile(
|
||
durability=QoSDurabilityPolicy.VOLATILE,
|
||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||
history=QoSHistoryPolicy.KEEP_LAST,
|
||
depth=1,
|
||
),
|
||
callback_group=callback_group,
|
||
)
|
||
|
||
self._apply_planning_scene_service = self.create_client(
|
||
srv_type=ApplyPlanningScene,
|
||
srv_name="/apply_planning_scene",
|
||
qos_profile=QoSProfile(
|
||
durability=QoSDurabilityPolicy.VOLATILE,
|
||
reliability=QoSReliabilityPolicy.RELIABLE,
|
||
history=QoSHistoryPolicy.KEEP_LAST,
|
||
depth=1,
|
||
),
|
||
callback_group=callback_group,
|
||
)
|
||
|
||
self.resource_pose_publisher = self.create_publisher(
|
||
String, f"resource_pose", 10
|
||
)
|
||
self.__collision_object_publisher = self.create_publisher(
|
||
CollisionObject, "/collision_object", 10
|
||
)
|
||
self.__planning_scene_publisher = self.create_publisher(
|
||
PlanningScene, "/planning_scene", 10
|
||
)
|
||
self.__attached_collision_object_publisher = self.create_publisher(
|
||
AttachedCollisionObject, "/attached_collision_object", 0
|
||
)
|
||
|
||
self._action_server = ActionServer(
|
||
self,
|
||
SendCmd,
|
||
f"tf_update",
|
||
self.tf_update,
|
||
callback_group=callback_group,
|
||
)
|
||
|
||
self._add_resource_mesh_action_server = ActionServer(
|
||
self,
|
||
SendCmd,
|
||
f"add_resource_mesh",
|
||
self.add_resource_mesh_callback,
|
||
callback_group=callback_group,
|
||
)
|
||
|
||
self._reload_resource_mesh_action_server = ActionServer(
|
||
self,
|
||
SendCmd,
|
||
f"reload_resource_mesh",
|
||
self._reload_resource_mesh_callback,
|
||
callback_group=callback_group,
|
||
)
|
||
|
||
if self.resource_config_dict:
|
||
self.resource_tf_dict = self.resource_mesh_setup(self.resource_config_dict)
|
||
else:
|
||
self.get_logger().info("未提供 resource_config，将通过 ActionServer 或 load_from_resource_tree 加载")
|
||
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节点是否已初始化完成"""
|
||
|
||
# 获取当前可用的节点列表
|
||
if len(self.resource_tf_dict) == 0:
|
||
return
|
||
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)
|
||
|
||
|
||
def _build_resource_model_for_config(self, resource_config_dict: dict):
|
||
"""从 registry 中为给定的资源配置自动构建 resource_model（mesh 信息）"""
|
||
registry = lab_registry
|
||
for _uuid, res_cfg in resource_config_dict.items():
|
||
resource_id = res_cfg.get('id', '')
|
||
resource_class = res_cfg.get('class', '')
|
||
if not resource_class:
|
||
continue
|
||
if resource_class not in registry.resource_type_registry:
|
||
continue
|
||
reg_entry = registry.resource_type_registry[resource_class]
|
||
if 'model' not in reg_entry:
|
||
continue
|
||
model_config = reg_entry['model']
|
||
if model_config.get('type') != 'resource':
|
||
continue
|
||
if resource_id in self.resource_model:
|
||
continue
|
||
self.resource_model[resource_id] = {
|
||
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
|
||
'mesh_tf': model_config['mesh_tf'],
|
||
}
|
||
if model_config.get('children_mesh') is not None:
|
||
self.resource_model[f"{resource_id}_"] = {
|
||
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
|
||
'mesh_tf': model_config['children_mesh_tf'],
|
||
}
|
||
|
||
def load_from_resource_tree(self):
|
||
"""从 resource_tracker 中读取资源树，自动构建 resource_config_dict / resource_model 并刷新 TF"""
|
||
new_config_dict: dict = {}
|
||
|
||
def _collect_plr_resource(res, parent_uuid: Optional[str] = None):
|
||
res_uuid = getattr(res, 'unilabos_uuid', None)
|
||
if not res_uuid:
|
||
res_uuid = str(uuid.uuid4())
|
||
extra = getattr(res, 'unilabos_extra', {}) or {}
|
||
resource_class = extra.get(EXTRA_CLASS, '')
|
||
|
||
location = getattr(res, 'location', None)
|
||
pos_x = float(location.x) if location else 0.0
|
||
pos_y = float(location.y) if location else 0.0
|
||
pos_z = float(location.z) if location else 0.0
|
||
|
||
rotation = extra.get('rotation', {'x': 0, 'y': 0, 'z': 0})
|
||
|
||
new_config_dict[res_uuid] = {
|
||
'id': res.name,
|
||
'uuid': res_uuid,
|
||
'class': resource_class,
|
||
'parent_uuid': parent_uuid,
|
||
'pose': {
|
||
'position': {'x': pos_x, 'y': pos_y, 'z': pos_z},
|
||
'rotation': rotation,
|
||
},
|
||
}
|
||
|
||
for child in getattr(res, 'children', []) or []:
|
||
_collect_plr_resource(child, res_uuid)
|
||
|
||
for resource in self.resource_tracker.resources:
|
||
root_parent_uuid = None
|
||
plr_parent = getattr(resource, 'parent', None)
|
||
if plr_parent is not None:
|
||
root_parent_uuid = getattr(plr_parent, 'unilabos_uuid', None)
|
||
_collect_plr_resource(resource, root_parent_uuid)
|
||
|
||
if not new_config_dict:
|
||
self.get_logger().warning("resource_tracker 中没有找到任何资源")
|
||
return
|
||
|
||
self.resource_config_dict = {**self.resource_config_dict, **new_config_dict}
|
||
self._build_resource_model_for_config(new_config_dict)
|
||
|
||
tf_dict = self.resource_mesh_setup(new_config_dict)
|
||
self.resource_tf_dict = {**self.resource_tf_dict, **tf_dict}
|
||
self.publish_resource_tf()
|
||
if self.move_group_ready:
|
||
self.add_resource_collision_meshes(tf_dict)
|
||
self.get_logger().info(f"从资源树加载了 {len(new_config_dict)} 个资源")
|
||
|
||
def _reload_resource_mesh_callback(self, goal_handle: ServerGoalHandle):
|
||
"""ActionServer 回调：重新从资源树加载所有 mesh"""
|
||
try:
|
||
self.load_from_resource_tree()
|
||
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_callback(self, goal_handle: ServerGoalHandle):
|
||
tf_update_msg = goal_handle.request
|
||
try:
|
||
parsed = json.loads(tf_update_msg.command.replace("'", '"'))
|
||
if 'resources' in parsed:
|
||
for res_config in parsed['resources']:
|
||
self.add_resource_mesh(json.dumps(res_config))
|
||
else:
|
||
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):
|
||
"""添加单个资源的 mesh 配置"""
|
||
registry = lab_registry
|
||
resource_config = json.loads(resource_config_str.replace("'", '"'))
|
||
|
||
if resource_config['id'] in self.resource_config_dict:
|
||
self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
|
||
return
|
||
resource_class = resource_config.get('class', '')
|
||
if resource_class and resource_class in registry.resource_type_registry:
|
||
reg_entry = registry.resource_type_registry[resource_class]
|
||
if 'model' in reg_entry:
|
||
model_config = reg_entry['model']
|
||
if model_config.get('type') == 'resource':
|
||
self.resource_model[resource_config['id']] = {
|
||
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
|
||
'mesh_tf': model_config['mesh_tf'],
|
||
}
|
||
if model_config.get('children_mesh') is not None:
|
||
self.resource_model[f"{resource_config['id']}_"] = {
|
||
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
|
||
'mesh_tf': model_config['children_mesh_tf'],
|
||
}
|
||
resources = initialize_resources([resource_config])
|
||
resource_dict = {item['id']: item for item in resources}
|
||
self.resource_config_dict = {**self.resource_config_dict, **resource_dict}
|
||
tf_dict = self.resource_mesh_setup(resource_dict)
|
||
self.resource_tf_dict = {**self.resource_tf_dict, **tf_dict}
|
||
self.publish_resource_tf()
|
||
self.add_resource_collision_meshes(tf_dict)
|
||
|
||
def resource_mesh_setup(self, resource_config_dict: dict):
|
||
"""根据资源配置字典设置 TF 关系"""
|
||
self.get_logger().info('开始设置资源网格管理器')
|
||
resource_tf_dict = {}
|
||
for resource_uuid, resource_config in resource_config_dict.items():
|
||
parent = None
|
||
resource_id = resource_config['id']
|
||
parent_uuid = resource_config.get('parent_uuid')
|
||
if parent_uuid is not None and parent_uuid != "":
|
||
parent_entry = resource_config_dict.get(parent_uuid) or self.resource_config_dict.get(parent_uuid)
|
||
parent = parent_entry['id'] if parent_entry else None
|
||
|
||
parent_link = 'world'
|
||
if parent in self.resource_model:
|
||
parent_link = parent
|
||
elif parent is None and resource_id in self.resource_model:
|
||
pass
|
||
elif parent is not None and resource_id in self.resource_model:
|
||
# parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
|
||
parent_link = f"{parent}_device_link".replace("None_","")
|
||
|
||
else:
|
||
continue
|
||
# 提取位置信息并转换单位
|
||
position = {
|
||
"x": float(resource_config['pose']['position']['x'])/1000,
|
||
"y": float(resource_config['pose']['position']['y'])/1000,
|
||
"z": float(resource_config['pose']['position']['z'])/1000
|
||
}
|
||
|
||
rotation_dict = {
|
||
"x": 0,
|
||
"y": 0,
|
||
"z": 0
|
||
}
|
||
|
||
if 'rotation' in resource_config['pose']:
|
||
rotation_dict = resource_config['pose']['rotation']
|
||
|
||
# 从欧拉角转换为四元数
|
||
q = quaternion_from_euler(
|
||
float(rotation_dict['x']),
|
||
float(rotation_dict['y']),
|
||
float(rotation_dict['z'])
|
||
)
|
||
|
||
rotation = {
|
||
"x": q[0],
|
||
"y": q[1],
|
||
"z": q[2],
|
||
"w": q[3]
|
||
}
|
||
|
||
# 更新资源TF字典
|
||
resource_tf_dict[resource_id] = {
|
||
"parent": parent_link,
|
||
"position": position,
|
||
"rotation": rotation
|
||
}
|
||
|
||
return resource_tf_dict
|
||
|
||
|
||
def publish_resource_tf(self):
|
||
"""
|
||
发布资源之间的TF关系
|
||
|
||
遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
|
||
发布key和parent之间的tf关系
|
||
"""
|
||
|
||
transforms = []
|
||
|
||
# 遍历资源TF字典
|
||
resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
|
||
for resource_id, tf_info in resource_tf_dict.items():
|
||
parent = tf_info['parent']
|
||
position = tf_info['position']
|
||
rotation = tf_info['rotation']
|
||
|
||
# 创建静态变换消息
|
||
|
||
transform = TransformStamped()
|
||
transform.header.stamp = self.get_clock().now().to_msg()
|
||
transform.header.frame_id = parent
|
||
transform.child_frame_id = resource_id
|
||
|
||
# 设置位置
|
||
transform.transform.translation.x = float(position['x'])
|
||
transform.transform.translation.y = float(position['y'])
|
||
transform.transform.translation.z = float(position['z'])
|
||
|
||
# 设置旋转
|
||
transform.transform.rotation.x = rotation['x']
|
||
transform.transform.rotation.y = rotation['y']
|
||
transform.transform.rotation.z = rotation['z']
|
||
transform.transform.rotation.w = rotation['w']
|
||
|
||
transforms.append(transform)
|
||
|
||
# 一次性发布所有静态变换
|
||
if transforms:
|
||
self.tf_broadcaster.sendTransform(transforms)
|
||
# self.check_resource_pose_changes()
|
||
# self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
|
||
|
||
|
||
def check_resource_pose_changes(self):
|
||
"""
|
||
遍历资源TF字典，计算每个资源相对于world的变换，
|
||
与旧的位姿比较，记录发生变化的资源，并更新旧位姿记录。
|
||
|
||
Returns:
|
||
dict: 包含发生位姿变化的资源ID及其新位姿
|
||
"""
|
||
if not self.move_group_ready:
|
||
self.check_move_group_ready()
|
||
return
|
||
changed_poses = {}
|
||
resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
|
||
if self.msg_type == 'resource_pose':
|
||
for resource_id in resource_tf_dict.keys():
|
||
try:
|
||
# 获取从resource_id到world的转换
|
||
|
||
transform = self.tf_buffer.lookup_transform(
|
||
"world",
|
||
resource_id,
|
||
rclpy.time.Time(seconds=0),
|
||
# rclpy.duration.Duration(seconds=5)
|
||
)
|
||
|
||
# 提取当前位姿信息
|
||
current_pose = {
|
||
"position": {
|
||
"x": transform.transform.translation.x,
|
||
"y": transform.transform.translation.y,
|
||
"z": transform.transform.translation.z
|
||
},
|
||
"rotation": {
|
||
"x": transform.transform.rotation.x,
|
||
"y": transform.transform.rotation.y,
|
||
"z": transform.transform.rotation.z,
|
||
"w": transform.transform.rotation.w
|
||
}
|
||
}
|
||
|
||
# 检查是否存在旧位姿记录
|
||
if resource_id not in self.old_resource_pose:
|
||
# 如果没有旧记录，则认为是新资源，记录变化
|
||
changed_poses[resource_id] = current_pose
|
||
self.old_resource_pose[resource_id] = current_pose
|
||
else:
|
||
# 比较当前位姿与旧位姿
|
||
old_pose = self.old_resource_pose[resource_id]
|
||
if (not self._is_pose_equal(current_pose, old_pose)):
|
||
# 如果位姿发生变化，记录新位姿
|
||
changed_poses[resource_id] = current_pose
|
||
self.old_resource_pose[resource_id] = current_pose
|
||
|
||
except Exception as e:
|
||
self.get_logger().warning(f"获取资源 {resource_id} 的世界坐标变换失败: {e}")
|
||
|
||
elif self.msg_type == 'resource_status':
|
||
for resource_id, resource_status in resource_tf_dict.items():
|
||
if resource_id not in self.old_resource_pose:
|
||
# 如果没有旧记录，则认为是新资源，记录变化
|
||
changed_poses[resource_id] = resource_status['parent']
|
||
self.old_resource_pose[resource_id] = resource_status['parent']
|
||
else:
|
||
# 比较当前位姿与旧位姿
|
||
old_pose = self.old_resource_pose[resource_id]
|
||
if resource_status['parent'] != old_pose:
|
||
# 如果位姿发生变化，记录新位姿
|
||
changed_poses[resource_id] = resource_status['parent']
|
||
self.old_resource_pose[resource_id] = resource_status['parent']
|
||
|
||
|
||
if changed_poses != {}:
|
||
self.zero_count = 0
|
||
changed_poses_msg = String()
|
||
changed_poses_msg.data = json.dumps(changed_poses)
|
||
self.resource_pose_publisher.publish(changed_poses_msg)
|
||
else:
|
||
if self.zero_count > self.rate:
|
||
self.zero_count = 0
|
||
changed_poses_msg = String()
|
||
changed_poses_msg.data = json.dumps(changed_poses)
|
||
self.resource_pose_publisher.publish(changed_poses_msg)
|
||
self.zero_count += 1
|
||
|
||
|
||
def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
|
||
"""
|
||
比较两个位姿是否相等（考虑浮点数精度）
|
||
|
||
Args:
|
||
pose1: 第一个位姿
|
||
pose2: 第二个位姿
|
||
tolerance: 浮点数比较的容差
|
||
|
||
Returns:
|
||
bool: 如果位姿相等返回True，否则返回False
|
||
"""
|
||
# 比较位置
|
||
pos1 = pose1["position"]
|
||
pos2 = pose2["position"]
|
||
if (abs(pos1["x"] - pos2["x"]) > tolerance or
|
||
abs(pos1["y"] - pos2["y"]) > tolerance or
|
||
abs(pos1["z"] - pos2["z"]) > tolerance):
|
||
return False
|
||
|
||
# 比较旋转
|
||
rot1 = pose1["rotation"]
|
||
rot2 = pose2["rotation"]
|
||
if (abs(rot1["x"] - rot2["x"]) > tolerance or
|
||
abs(rot1["y"] - rot2["y"]) > tolerance or
|
||
abs(rot1["z"] - rot2["z"]) > tolerance or
|
||
abs(rot1["w"] - rot2["w"]) > tolerance):
|
||
return False
|
||
|
||
return True
|
||
|
||
def tf_update(self, goal_handle : ServerGoalHandle):
|
||
tf_update_msg = goal_handle.request
|
||
|
||
try:
|
||
cmd_dict = json.loads(tf_update_msg.command.replace("'",'"'))
|
||
self.__planning_scene = self._get_planning_scene_service.call(
|
||
GetPlanningScene.Request()
|
||
).scene
|
||
self.__planning_scene.is_diff = True
|
||
planning_scene = PlanningScene()
|
||
planning_scene.is_diff = True
|
||
planning_scene.robot_state.is_diff = True
|
||
# time_start = self.get_clock().now()
|
||
time_start = rclpy.time.Time(seconds=0)
|
||
count = 0
|
||
|
||
for resource_id, target_parent in cmd_dict.items():
|
||
parent_id = target_parent
|
||
if target_parent == '__trash':
|
||
parent_id = 'world'
|
||
# 获取从resource_id到target_parent的转换
|
||
transform = self.tf_buffer.lookup_transform(
|
||
parent_id,
|
||
resource_id,
|
||
time_start,
|
||
timeout=rclpy.duration.Duration(seconds=10)
|
||
)
|
||
|
||
# 提取转换中的位置和旋转信息
|
||
position = {
|
||
"x": transform.transform.translation.x,
|
||
"y": transform.transform.translation.y,
|
||
"z": transform.transform.translation.z
|
||
}
|
||
|
||
rotation = {
|
||
"x": transform.transform.rotation.x,
|
||
"y": transform.transform.rotation.y,
|
||
"z": transform.transform.rotation.z,
|
||
"w": transform.transform.rotation.w
|
||
}
|
||
|
||
self.resource_tf_dict[resource_id] = {
|
||
"parent": parent_id,
|
||
"position": position,
|
||
"rotation": rotation
|
||
}
|
||
|
||
|
||
# self.attach_collision_object(id=resource_id,link_name=target_parent)
|
||
# time.sleep(0.02)
|
||
operation_attach = CollisionObject.ADD
|
||
operation_world = CollisionObject.REMOVE
|
||
if target_parent == 'world':
|
||
operation_attach = CollisionObject.REMOVE
|
||
operation_world = CollisionObject.ADD
|
||
elif target_parent == '__trash':
|
||
operation_attach = CollisionObject.REMOVE
|
||
|
||
world_object = CollisionObject(
|
||
id=resource_id,
|
||
operation=operation_world
|
||
)
|
||
if target_parent != '__trash':
|
||
planning_scene.world.collision_objects.append(world_object)
|
||
|
||
|
||
collision_object = AttachedCollisionObject(
|
||
object=CollisionObject(
|
||
id=resource_id,
|
||
operation=operation_attach
|
||
)
|
||
)
|
||
if target_parent != 'world' and target_parent != '__trash':
|
||
collision_object.link_name = target_parent
|
||
planning_scene.robot_state.attached_collision_objects.append(collision_object)
|
||
|
||
count += 1
|
||
|
||
if count > 30:
|
||
req = ApplyPlanningScene.Request()
|
||
req.scene = planning_scene
|
||
self.publish_resource_tf()
|
||
self._apply_planning_scene_service.call(req)
|
||
self.__planning_scene_publisher.publish(planning_scene)
|
||
count = 0
|
||
|
||
planning_scene = PlanningScene()
|
||
planning_scene.is_diff = True
|
||
planning_scene.robot_state.is_diff = True
|
||
|
||
req = ApplyPlanningScene.Request()
|
||
req.scene = planning_scene
|
||
self.publish_resource_tf()
|
||
self._apply_planning_scene_service.call(req)
|
||
self.__planning_scene_publisher.publish(planning_scene)
|
||
|
||
# self.__collision_object_publisher.publish(CollisionObject())
|
||
|
||
|
||
except Exception as e:
|
||
self.get_logger().error(f"更新资源TF字典失败: {e}")
|
||
goal_handle.abort()
|
||
return SendCmd.Result(success=False)
|
||
goal_handle.succeed()
|
||
return SendCmd.Result(success=True)
|
||
|
||
|
||
|
||
def add_resource_collision_meshes(self,resource_tf_dict:dict):
|
||
"""
|
||
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
|
||
|
||
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
|
||
|
||
"""
|
||
self.get_logger().info('开始添加资源碰撞网格')
|
||
|
||
self.__planning_scene = self._get_planning_scene_service.call(
|
||
GetPlanningScene.Request()
|
||
).scene
|
||
planning_scene = PlanningScene()
|
||
planning_scene.is_diff = True
|
||
count = 0
|
||
for resource_id, tf_info in resource_tf_dict.items():
|
||
|
||
if resource_id in self.resource_model:
|
||
# 获取位置信息
|
||
|
||
position = [
|
||
float(self.resource_model[resource_id]['mesh_tf'][0]),
|
||
float(self.resource_model[resource_id]['mesh_tf'][1]),
|
||
float(self.resource_model[resource_id]['mesh_tf'][2])
|
||
]
|
||
|
||
# 获取旋转信息并转换为四元数
|
||
|
||
q = quaternion_from_euler(
|
||
float(self.resource_model[resource_id]['mesh_tf'][3]),
|
||
float(self.resource_model[resource_id]['mesh_tf'][4]),
|
||
float(self.resource_model[resource_id]['mesh_tf'][5])
|
||
)
|
||
|
||
# 添加碰撞网格
|
||
collision_object = self.get_collision_mesh(
|
||
filepath=self.resource_model[resource_id]['mesh'],
|
||
id=resource_id,
|
||
position=position,
|
||
quat_xyzw=q,
|
||
frame_id=resource_id
|
||
)
|
||
count += 1
|
||
planning_scene.world.collision_objects.append(collision_object)
|
||
elif f"{tf_info['parent']}_" in self.resource_model:
|
||
# 获取资源的父级框架ID
|
||
id_ = f"{tf_info['parent']}_"
|
||
|
||
# 获取位置信息
|
||
position = [
|
||
float(self.resource_model[id_]['mesh_tf'][0]),
|
||
float(self.resource_model[id_]['mesh_tf'][1]),
|
||
float(self.resource_model[id_]['mesh_tf'][2])
|
||
]
|
||
|
||
# 获取旋转信息并转换为四元数
|
||
|
||
q = quaternion_from_euler(
|
||
float(self.resource_model[id_]['mesh_tf'][3]),
|
||
float(self.resource_model[id_]['mesh_tf'][4]),
|
||
float(self.resource_model[id_]['mesh_tf'][5])
|
||
)
|
||
|
||
# 添加碰撞网格
|
||
collision_object = self.get_collision_mesh(
|
||
filepath=self.resource_model[id_]['mesh'],
|
||
id=resource_id,
|
||
position=position,
|
||
quat_xyzw=q,
|
||
frame_id=resource_id
|
||
)
|
||
count += 1
|
||
planning_scene.world.collision_objects.append(collision_object)
|
||
|
||
if count > 30:
|
||
req = ApplyPlanningScene.Request()
|
||
req.scene = planning_scene
|
||
self.publish_resource_tf()
|
||
self._apply_planning_scene_service.call(req)
|
||
self.__planning_scene_publisher.publish(planning_scene)
|
||
count = 0
|
||
|
||
planning_scene = PlanningScene()
|
||
planning_scene.is_diff = True
|
||
|
||
req = ApplyPlanningScene.Request()
|
||
req.scene = planning_scene
|
||
self.publish_resource_tf()
|
||
self._apply_planning_scene_service.call(req)
|
||
self.__planning_scene_publisher.publish(planning_scene)
|
||
|
||
self.get_logger().info('资源碰撞网格添加完成')
|
||
|
||
|
||
def add_collision_primitive(
|
||
self,
|
||
id: str,
|
||
primitive_type: int,
|
||
dimensions: Tuple[float, float, float],
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
):
|
||
"""
|
||
Add collision object with a primitive geometry specified by its dimensions.
|
||
|
||
`primitive_type` can be one of the following:
|
||
- `SolidPrimitive.BOX`
|
||
- `SolidPrimitive.SPHERE`
|
||
- `SolidPrimitive.CYLINDER`
|
||
- `SolidPrimitive.CONE`
|
||
"""
|
||
|
||
if (pose is None) and (position is None or quat_xyzw is None):
|
||
raise ValueError(
|
||
"Either `pose` or `position` and `quat_xyzw` must be specified!"
|
||
)
|
||
|
||
if isinstance(pose, PoseStamped):
|
||
pose_stamped = pose
|
||
elif isinstance(pose, Pose):
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=pose,
|
||
)
|
||
else:
|
||
if not isinstance(position, Point):
|
||
position = Point(
|
||
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||
)
|
||
if not isinstance(quat_xyzw, Quaternion):
|
||
quat_xyzw = Quaternion(
|
||
x=float(quat_xyzw[0]),
|
||
y=float(quat_xyzw[1]),
|
||
z=float(quat_xyzw[2]),
|
||
w=float(quat_xyzw[3]),
|
||
)
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=Pose(position=position, orientation=quat_xyzw),
|
||
)
|
||
|
||
msg = CollisionObject(
|
||
header=pose_stamped.header,
|
||
id=id,
|
||
operation=operation,
|
||
pose=pose_stamped.pose,
|
||
)
|
||
|
||
msg.primitives.append(
|
||
SolidPrimitive(type=primitive_type, dimensions=dimensions)
|
||
)
|
||
|
||
self.__collision_object_publisher.publish(msg)
|
||
|
||
def add_collision_box(
|
||
self,
|
||
id: str,
|
||
size: Tuple[float, float, float],
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
):
|
||
"""
|
||
Add collision object with a box geometry specified by its size.
|
||
"""
|
||
|
||
assert len(size) == 3, "Invalid size of the box!"
|
||
|
||
self.add_collision_primitive(
|
||
id=id,
|
||
primitive_type=SolidPrimitive.BOX,
|
||
dimensions=size,
|
||
pose=pose,
|
||
position=position,
|
||
quat_xyzw=quat_xyzw,
|
||
frame_id=frame_id,
|
||
operation=operation,
|
||
)
|
||
|
||
def add_collision_sphere(
|
||
self,
|
||
id: str,
|
||
radius: float,
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
):
|
||
"""
|
||
Add collision object with a sphere geometry specified by its radius.
|
||
"""
|
||
|
||
if quat_xyzw is None:
|
||
quat_xyzw = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
|
||
|
||
self.add_collision_primitive(
|
||
id=id,
|
||
primitive_type=SolidPrimitive.SPHERE,
|
||
dimensions=[
|
||
radius,
|
||
],
|
||
pose=pose,
|
||
position=position,
|
||
quat_xyzw=quat_xyzw,
|
||
frame_id=frame_id,
|
||
operation=operation,
|
||
)
|
||
|
||
def add_collision_cylinder(
|
||
self,
|
||
id: str,
|
||
height: float,
|
||
radius: float,
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
):
|
||
"""
|
||
Add collision object with a cylinder geometry specified by its height and radius.
|
||
"""
|
||
|
||
self.add_collision_primitive(
|
||
id=id,
|
||
primitive_type=SolidPrimitive.CYLINDER,
|
||
dimensions=[height, radius],
|
||
pose=pose,
|
||
position=position,
|
||
quat_xyzw=quat_xyzw,
|
||
frame_id=frame_id,
|
||
operation=operation,
|
||
)
|
||
|
||
def add_collision_cone(
|
||
self,
|
||
id: str,
|
||
height: float,
|
||
radius: float,
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
):
|
||
"""
|
||
Add collision object with a cone geometry specified by its height and radius.
|
||
"""
|
||
|
||
self.add_collision_primitive(
|
||
id=id,
|
||
primitive_type=SolidPrimitive.CONE,
|
||
dimensions=[height, radius],
|
||
pose=pose,
|
||
position=position,
|
||
quat_xyzw=quat_xyzw,
|
||
frame_id=frame_id,
|
||
operation=operation,
|
||
)
|
||
|
||
def get_collision_mesh(
|
||
self,
|
||
filepath: Optional[str],
|
||
id: str,
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
scale: Union[float, Tuple[float, float, float]] = 1.0,
|
||
mesh: Optional[Any] = None,
|
||
):
|
||
"""
|
||
Add collision object with a mesh geometry. Either `filepath` must be
|
||
specified or `mesh` must be provided.
|
||
Note: This function required 'trimesh' Python module to be installed.
|
||
"""
|
||
|
||
# Load the mesh
|
||
try:
|
||
import trimesh
|
||
except ImportError as err:
|
||
raise ImportError(
|
||
"Python module 'trimesh' not found! Please install it manually in order "
|
||
"to add collision objects into the MoveIt 2 planning scene."
|
||
) from err
|
||
|
||
# Check the parameters
|
||
if (pose is None) and (position is None or quat_xyzw is None):
|
||
raise ValueError(
|
||
"Either `pose` or `position` and `quat_xyzw` must be specified!"
|
||
)
|
||
if (filepath is None and mesh is None) or (
|
||
filepath is not None and mesh is not None
|
||
):
|
||
raise ValueError("Exactly one of `filepath` or `mesh` must be specified!")
|
||
if mesh is not None and not isinstance(mesh, trimesh.Trimesh):
|
||
raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!")
|
||
|
||
if isinstance(pose, PoseStamped):
|
||
pose_stamped = pose
|
||
elif isinstance(pose, Pose):
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=pose,
|
||
)
|
||
else:
|
||
if not isinstance(position, Point):
|
||
position = Point(
|
||
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||
)
|
||
if not isinstance(quat_xyzw, Quaternion):
|
||
quat_xyzw = Quaternion(
|
||
x=float(quat_xyzw[0]),
|
||
y=float(quat_xyzw[1]),
|
||
z=float(quat_xyzw[2]),
|
||
w=float(quat_xyzw[3]),
|
||
)
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=Pose(position=position, orientation=quat_xyzw),
|
||
)
|
||
|
||
msg = CollisionObject(
|
||
header=pose_stamped.header,
|
||
id=id,
|
||
operation=operation,
|
||
pose=pose_stamped.pose,
|
||
)
|
||
|
||
if filepath is not None:
|
||
mesh = trimesh.load(filepath)
|
||
|
||
# Scale the mesh
|
||
if isinstance(scale, float):
|
||
scale = (scale, scale, scale)
|
||
if not (scale[0] == scale[1] == scale[2] == 1.0):
|
||
# If the mesh was passed in as a parameter, make a copy of it to
|
||
# avoid transforming the original.
|
||
if filepath is not None:
|
||
mesh = mesh.copy()
|
||
# Transform the mesh
|
||
transform = np.eye(4)
|
||
np.fill_diagonal(transform, scale)
|
||
mesh.apply_transform(transform)
|
||
|
||
msg.meshes.append(
|
||
Mesh(
|
||
triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],
|
||
vertices=[
|
||
Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices
|
||
],
|
||
)
|
||
)
|
||
|
||
# self.__collision_object_publisher.publish(msg)
|
||
return msg
|
||
|
||
def add_collision_mesh(
|
||
self,
|
||
filepath: Optional[str],
|
||
id: str,
|
||
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||
quat_xyzw: Optional[
|
||
Union[Quaternion, Tuple[float, float, float, float]]
|
||
] = None,
|
||
frame_id: Optional[str] = None,
|
||
operation: int = CollisionObject.ADD,
|
||
scale: Union[float, Tuple[float, float, float]] = 1.0,
|
||
mesh: Optional[Any] = None,
|
||
):
|
||
"""
|
||
Add collision object with a mesh geometry. Either `filepath` must be
|
||
specified or `mesh` must be provided.
|
||
Note: This function required 'trimesh' Python module to be installed.
|
||
"""
|
||
|
||
# Load the mesh
|
||
try:
|
||
import trimesh
|
||
except ImportError as err:
|
||
raise ImportError(
|
||
"Python module 'trimesh' not found! Please install it manually in order "
|
||
"to add collision objects into the MoveIt 2 planning scene."
|
||
) from err
|
||
|
||
# Check the parameters
|
||
if (pose is None) and (position is None or quat_xyzw is None):
|
||
raise ValueError(
|
||
"Either `pose` or `position` and `quat_xyzw` must be specified!"
|
||
)
|
||
if (filepath is None and mesh is None) or (
|
||
filepath is not None and mesh is not None
|
||
):
|
||
raise ValueError("Exactly one of `filepath` or `mesh` must be specified!")
|
||
if mesh is not None and not isinstance(mesh, trimesh.Trimesh):
|
||
raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!")
|
||
|
||
if isinstance(pose, PoseStamped):
|
||
pose_stamped = pose
|
||
elif isinstance(pose, Pose):
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=pose,
|
||
)
|
||
else:
|
||
if not isinstance(position, Point):
|
||
position = Point(
|
||
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||
)
|
||
if not isinstance(quat_xyzw, Quaternion):
|
||
quat_xyzw = Quaternion(
|
||
x=float(quat_xyzw[0]),
|
||
y=float(quat_xyzw[1]),
|
||
z=float(quat_xyzw[2]),
|
||
w=float(quat_xyzw[3]),
|
||
)
|
||
pose_stamped = PoseStamped(
|
||
header=Header(
|
||
stamp=self.get_clock().now().to_msg(),
|
||
frame_id=(
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
),
|
||
),
|
||
pose=Pose(position=position, orientation=quat_xyzw),
|
||
)
|
||
|
||
msg = CollisionObject(
|
||
header=pose_stamped.header,
|
||
id=id,
|
||
operation=operation,
|
||
pose=pose_stamped.pose,
|
||
)
|
||
|
||
if filepath is not None:
|
||
mesh = trimesh.load(filepath)
|
||
|
||
# Scale the mesh
|
||
if isinstance(scale, float):
|
||
scale = (scale, scale, scale)
|
||
if not (scale[0] == scale[1] == scale[2] == 1.0):
|
||
# If the mesh was passed in as a parameter, make a copy of it to
|
||
# avoid transforming the original.
|
||
if filepath is not None:
|
||
mesh = mesh.copy()
|
||
# Transform the mesh
|
||
transform = np.eye(4)
|
||
np.fill_diagonal(transform, scale)
|
||
mesh.apply_transform(transform)
|
||
|
||
msg.meshes.append(
|
||
Mesh(
|
||
triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],
|
||
vertices=[
|
||
Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices
|
||
],
|
||
)
|
||
)
|
||
|
||
self.__collision_object_publisher.publish(msg)
|
||
|
||
def remove_collision_object(self, id: str):
|
||
"""
|
||
Remove collision object specified by its `id`.
|
||
"""
|
||
|
||
msg = CollisionObject()
|
||
msg.id = id
|
||
msg.operation = CollisionObject.REMOVE
|
||
msg.header.stamp = self.get_clock().now().to_msg()
|
||
self.__collision_object_publisher.publish(msg)
|
||
|
||
def remove_collision_mesh(self, id: str):
|
||
"""
|
||
Remove collision mesh specified by its `id`.
|
||
Identical to `remove_collision_object()`.
|
||
"""
|
||
|
||
self.remove_collision_object(id)
|
||
|
||
def attach_collision_object(
|
||
self,
|
||
id: str,
|
||
link_name: Optional[str] = None,
|
||
touch_links: List[str] = [],
|
||
weight: float = 0.0,
|
||
):
|
||
"""
|
||
Attach collision object to the robot.
|
||
"""
|
||
|
||
msg = AttachedCollisionObject(
|
||
object=CollisionObject(id=id, operation=CollisionObject.ADD)
|
||
)
|
||
msg.link_name = link_name
|
||
msg.touch_links = touch_links
|
||
msg.weight = weight
|
||
|
||
self.__attached_collision_object_publisher.publish(msg)
|
||
|
||
def detach_collision_object(self, id: int):
|
||
"""
|
||
Detach collision object from the robot.
|
||
"""
|
||
|
||
msg = AttachedCollisionObject(
|
||
object=CollisionObject(id=id, operation=CollisionObject.REMOVE)
|
||
)
|
||
self.__attached_collision_object_publisher.publish(msg)
|
||
|
||
def detach_all_collision_objects(self):
|
||
"""
|
||
Detach collision object from the robot.
|
||
"""
|
||
|
||
msg = AttachedCollisionObject(
|
||
object=CollisionObject(operation=CollisionObject.REMOVE)
|
||
)
|
||
self.__attached_collision_object_publisher.publish(msg)
|
||
|
||
def move_collision(
|
||
self,
|
||
id: str,
|
||
position: Union[Point, Tuple[float, float, float]],
|
||
quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
|
||
frame_id: Optional[str] = None,
|
||
):
|
||
"""
|
||
Move collision object specified by its `id`.
|
||
"""
|
||
|
||
msg = CollisionObject()
|
||
|
||
if not isinstance(position, Point):
|
||
position = Point(
|
||
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||
)
|
||
if not isinstance(quat_xyzw, Quaternion):
|
||
quat_xyzw = Quaternion(
|
||
x=float(quat_xyzw[0]),
|
||
y=float(quat_xyzw[1]),
|
||
z=float(quat_xyzw[2]),
|
||
w=float(quat_xyzw[3]),
|
||
)
|
||
|
||
pose = Pose()
|
||
pose.position = position
|
||
pose.orientation = quat_xyzw
|
||
msg.pose = pose
|
||
msg.id = id
|
||
msg.operation = CollisionObject.MOVE
|
||
msg.header.frame_id = (
|
||
frame_id if frame_id is not None else self.__base_link_name
|
||
)
|
||
msg.header.stamp = self.get_clock().now().to_msg()
|
||
|
||
self.__collision_object_publisher.publish(msg)
|
||
|
||
def update_planning_scene(self) -> bool:
|
||
"""
|
||
Gets the current planning scene. Returns whether the service call was
|
||
successful.
|
||
"""
|
||
|
||
if not self._get_planning_scene_service.service_is_ready():
|
||
self.get_logger().warn(
|
||
f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||
)
|
||
return False
|
||
self.__planning_scene = self._get_planning_scene_service.call(
|
||
GetPlanningScene.Request()
|
||
).scene
|
||
return True
|
||
|
||
def allow_collisions(self, id: str, allow: bool) -> Optional[Future]:
|
||
"""
|
||
Takes in the ID of an element in the planning scene. Modifies the allowed
|
||
collision matrix to (dis)allow collisions between that object and all other
|
||
object.
|
||
|
||
If `allow` is True, a plan will succeed even if the robot collides with that object.
|
||
If `allow` is False, a plan will fail if the robot collides with that object.
|
||
Returns whether it successfully updated the allowed collision matrix.
|
||
|
||
Returns the future of the service call.
|
||
"""
|
||
# Update the planning scene
|
||
if not self.update_planning_scene():
|
||
return None
|
||
allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix
|
||
self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix)
|
||
|
||
# Get the location in the allowed collision matrix of the object
|
||
j = None
|
||
if id not in allowed_collision_matrix.entry_names:
|
||
allowed_collision_matrix.entry_names.append(id)
|
||
else:
|
||
j = allowed_collision_matrix.entry_names.index(id)
|
||
# For all other objects, (dis)allow collisions with the object with `id`
|
||
for i in range(len(allowed_collision_matrix.entry_values)):
|
||
if j is None:
|
||
allowed_collision_matrix.entry_values[i].enabled.append(allow)
|
||
elif i != j:
|
||
allowed_collision_matrix.entry_values[i].enabled[j] = allow
|
||
# For the object with `id`, (dis)allow collisions with all other objects
|
||
allowed_collision_entry = AllowedCollisionEntry(
|
||
enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))]
|
||
)
|
||
if j is None:
|
||
allowed_collision_matrix.entry_values.append(allowed_collision_entry)
|
||
else:
|
||
allowed_collision_matrix.entry_values[j] = allowed_collision_entry
|
||
|
||
# Apply the new planning scene
|
||
if not self._apply_planning_scene_service.service_is_ready():
|
||
self.get_logger().warn(
|
||
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||
)
|
||
return None
|
||
return self._apply_planning_scene_service.call_async(
|
||
ApplyPlanningScene.Request(scene=self.__planning_scene)
|
||
)
|
||
|
||
def process_allow_collision_future(self, future: Future) -> bool:
|
||
"""
|
||
Return whether the allow collision service call is done and has succeeded
|
||
or not. If it failed, reset the allowed collision matrix to the old one.
|
||
"""
|
||
if not future.done():
|
||
return False
|
||
|
||
# Get response
|
||
resp = future.result()
|
||
|
||
# If it failed, restore the old planning scene
|
||
if not resp.success:
|
||
self.__planning_scene.allowed_collision_matrix = (
|
||
self.__old_allowed_collision_matrix
|
||
)
|
||
|
||
return resp.success
|
||
|
||
def clear_all_collision_objects(self) -> Optional[Future]:
|
||
"""
|
||
Removes all attached and un-attached collision objects from the planning scene.
|
||
|
||
Returns a future for the ApplyPlanningScene service call.
|
||
"""
|
||
# Update the planning scene
|
||
if not self.update_planning_scene():
|
||
return None
|
||
self.__old_planning_scene = copy.deepcopy(self.__planning_scene)
|
||
|
||
# Remove all collision objects from the planning scene
|
||
self.__planning_scene.world.collision_objects = []
|
||
self.__planning_scene.robot_state.attached_collision_objects = []
|
||
|
||
# Apply the new planning scene
|
||
if not self._apply_planning_scene_service.service_is_ready():
|
||
self.get_logger().warn(
|
||
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||
)
|
||
return None
|
||
return self._apply_planning_scene_service.call_async(
|
||
ApplyPlanningScene.Request(scene=self.__planning_scene)
|
||
)
|