def move_collision_object(self, sink_collision_object,
source_collision_object, updated_pose):
link_world = pysdf.pose_msg2homogeneous(updated_pose)
for pose in source_collision_object.primitive_poses:
primitive_pose_in_link = pysdf.pose_msg2homogeneous(pose)
primitive_pose_in_world = pysdf.homogeneous2pose_msg(
concatenate_matrices(link_world, primitive_pose_in_link))
sink_collision_object.primitive_poses.extend(
[primitive_pose_in_world])
for pose in source_collision_object.mesh_poses:
mesh_pose_in_link = pysdf.pose_msg2homogeneous(pose)
mesh_pose_in_world = pysdf.homogeneous2pose_msg(
concatenate_matrices(link_world, mesh_pose_in_link))
sink_collision_object.mesh_poses.extend([mesh_pose_in_world])
for pose in source_collision_object.plane_poses:
plane_pose_in_link = pysdf.pose_msg2homogeneous(pose)
plane_pose_in_world = pysdf.homogeneous2pose_msg(
concatenate_matrices(link_world, plane_pose_in_link))
sink_collision_object.plane_poses.extend([plane_pose_in_world])
def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(
concatenate_matrices(link.pose_world,
inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." %
(full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val) for val in linkpart.geometry_data['scale'].split())
mesh_path = linkpart.geometry_data['uri'].replace(
'model://', pysdf.models_path)
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped,
mesh_path, scale)
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(2.0 * float(linkpart.geometry_data['radius']),
float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(float(val) for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
test_mesh_path = linkpart.geometry_data['uri'].replace('model://', models_path)
if os.path.isfile(test_mesh_path):
mesh_path = test_mesh_path
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale)
else:
print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname))
elif linkpart.geometry_type == 'box':
scale = tuple(float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple((2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
def link2marker_msg(link,
full_linkname,
use_collision=False,
lifetime=rospy.Duration(0)):
marker_msg = None
linkpart = None
if use_collision:
linkpart = getattr(link, 'collision')
else: # visual
linkpart = getattr(link, 'visual')
if not linkpart.geometry_type in supported_geometry_types:
return
marker_msg = copy.deepcopy(protoMarkerMsg)
marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
marker_msg.header.stamp = rospy.get_rostime()
marker_msg.lifetime = lifetime
marker_msg.ns = pysdf.sdf2tfname(full_linkname)
marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
if linkpart.geometry_type == 'mesh':
marker_msg.type = Marker.MESH_RESOURCE
marker_msg.mesh_resource = linkpart.geometry_data['uri'].replace(
'model://', 'file://' + pysdf.models_path)
scale = (float(val) for val in linkpart.geometry_data['scale'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
else:
marker_msg.color.a = 1
marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
if linkpart.geometry_type == 'box':
marker_msg.type = Marker.CUBE
scale = (float(val) for val in linkpart.geometry_data['size'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
elif linkpart.geometry_type == 'sphere':
marker_msg.type = Marker.SPHERE
marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(
linkpart.geometry_data['radius'])
elif linkpart.geometry_type == 'cylinder':
marker_msg.type = Marker.CYLINDER
marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(
linkpart.geometry_data['radius'])
marker_msg.scale.z = float(linkpart.geometry_data['length'])
#print(marker_msg)
return marker_msg
def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)):
marker_msg = None
linkpart = None
if use_collision:
linkpart = getattr(link, 'collision')
else: # visual
linkpart = getattr(link, 'visual')
if not linkpart.geometry_type in supported_geometry_types:
return
marker_msg = copy.deepcopy(protoMarkerMsg)
marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
marker_msg.header.stamp = rospy.get_rostime()
marker_msg.lifetime = lifetime
marker_msg.ns = pysdf.sdf2tfname(full_linkname)
marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
if linkpart.geometry_type == 'mesh':
marker_msg.type = Marker.MESH_RESOURCE
marker_msg.mesh_resource = linkpart.geometry_data['uri'].replace('model://', 'file://' + pysdf.models_path)
scale = (float(val) for val in linkpart.geometry_data['scale'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
else:
marker_msg.color.a = 1
marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
if linkpart.geometry_type == 'box':
marker_msg.type = Marker.CUBE
scale = (float(val) for val in linkpart.geometry_data['size'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
elif linkpart.geometry_type == 'sphere':
marker_msg.type = Marker.SPHERE
marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius'])
elif linkpart.geometry_type == 'cylinder':
marker_msg.type = Marker.CYLINDER
marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius'])
marker_msg.scale.z = float(linkpart.geometry_data['length'])
#print(marker_msg)
return marker_msg
def link2marker_msg(link,
full_linkname,
use_collision=False,
lifetime=rospy.Duration(0)):
marker_msg = None
linkpart = None
if use_collision:
linkparts = getattr(link, 'collisions')
else: # visual
linkparts = getattr(link, 'visuals')
msgs = []
for linkpart in linkparts:
if not linkpart.geometry_type in supported_geometry_types:
if linkpart.geometry_type:
print(
"Element %s with geometry type %s not supported. Ignored."
% (full_linkname, linkpart.geometry_type))
return None
marker_msg = copy.deepcopy(protoMarkerMsg)
marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
marker_msg.header.stamp = rospy.get_rostime()
marker_msg.lifetime = lifetime
marker_msg.ns = pysdf.sdf2tfname(full_linkname + "::" + linkpart.name)
marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
if linkpart.geometry_type == 'mesh':
marker_msg.type = Marker.MESH_RESOURCE
# print('linkpart.geometry_data: %s' % linkpart.geometry_data['uri'])
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace(
'model://', models_path + '/')
# print('resource: %s' % resource)
if os.path.isfile(resource):
marker_msg.mesh_resource = 'file://' + resource
# print('found resource %s at %s' % (linkpart.geometry_data['uri'], resource))
break
# support URDF-like resource paths starting with model://
if not marker_msg.mesh_resource and linkpart.geometry_data[
'uri'].startswith('model://'):
stripped_uri = linkpart.geometry_data['uri'].replace(
'model://', '')
uri_parts = stripped_uri.split('/', 1)
if len(uri_parts) == 2:
package_name = uri_parts[0]
try:
package_path = gazebo_rospack.get_path(package_name)
mesh_path = os.path.join(package_path, uri_parts[1])
if os.path.isfile(mesh_path):
marker_msg.mesh_resource = 'file://' + mesh_path
except ResourceNotFound, e:
pass
if not marker_msg.mesh_resource:
print('ERROR! could not find resource: %s' %
linkpart.geometry_data['uri'])
return None
scale = (float(val)
for val in linkpart.geometry_data['scale'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
else:
marker_msg.color.a = 1
marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
if linkpart.geometry_type == 'box':
marker_msg.type = Marker.CUBE
scale = (float(val)
for val in linkpart.geometry_data['size'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
elif linkpart.geometry_type == 'sphere':
marker_msg.type = Marker.SPHERE
marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(
linkpart.geometry_data['radius'])
elif linkpart.geometry_type == 'cylinder':
marker_msg.type = Marker.CYLINDER
marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(
linkpart.geometry_data['radius'])
marker_msg.scale.z = float(linkpart.geometry_data['length'])
# print(marker_msg)
msgs.append(marker_msg)
def link_to_collision_object(self, link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if self.is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print(
"Element %s with geometry type %s not supported. Ignored."
% (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val)
for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(resource):
mesh_path = resource
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = pysdf.homogeneous2pose_msg(
linkpart.pose)
if not self.make_mesh(collision_object, link_pose_stamped,
mesh_path, scale):
return None
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val)
for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [float(linkpart.geometry_data['radius'])]
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(float(linkpart.geometry_data['length']),
float(linkpart.geometry_data['radius'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
return collision_object
def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)):
marker_msg = None
linkpart = None
if use_collision:
linkparts = getattr(link, 'collisions')
else: # visual
linkparts = getattr(link, 'visuals')
msgs = []
for linkpart in linkparts:
if not linkpart.geometry_type in supported_geometry_types:
if linkpart.geometry_type:
print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type))
return None
marker_msg = copy.deepcopy(protoMarkerMsg)
marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
marker_msg.header.stamp = rospy.get_rostime()
marker_msg.lifetime = lifetime
marker_msg.ns = pysdf.sdf2tfname(full_linkname + "::" + linkpart.name)
marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
if linkpart.geometry_type == 'mesh':
marker_msg.type = Marker.MESH_RESOURCE
#print('linkpart.geometry_data: %s' % linkpart.geometry_data['uri'])
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace('model://', models_path + '/')
#print('resource: %s' % resource)
if os.path.isfile(resource):
marker_msg.mesh_resource = 'file://' + resource
#print('found resource %s at %s' % (linkpart.geometry_data['uri'], resource))
break
# support URDF-like resource paths starting with model://
if not marker_msg.mesh_resource and linkpart.geometry_data['uri'].startswith('model://'):
stripped_uri = linkpart.geometry_data['uri'].replace('model://', '')
uri_parts = stripped_uri.split('/', 1)
if len(uri_parts) == 2:
package_name = uri_parts[0]
try:
package_path = gazebo_rospack.get_path(package_name)
mesh_path = os.path.join(package_path, uri_parts[1])
if os.path.isfile(mesh_path):
marker_msg.mesh_resource = 'file://' + mesh_path
except ResourceNotFound, e:
pass
if not marker_msg.mesh_resource:
print('ERROR! could not find resource: %s' % linkpart.geometry_data['uri'])
return None
scale = (float(val) for val in linkpart.geometry_data['scale'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
else:
marker_msg.color.a = 1
marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
if linkpart.geometry_type == 'box':
marker_msg.type = Marker.CUBE
scale = (float(val) for val in linkpart.geometry_data['size'].split())
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
elif linkpart.geometry_type == 'sphere':
marker_msg.type = Marker.SPHERE
marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius'])
elif linkpart.geometry_type == 'cylinder':
marker_msg.type = Marker.CYLINDER
marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius'])
marker_msg.scale.z = float(linkpart.geometry_data['length'])
#print(marker_msg)
msgs.append(marker_msg)
