def on_link_states_msg(link_states_msg):
"""
Publish tf for each model in current Gazebo world
"""
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
poses = {'gazebo_world': identity_matrix()}
for (link_idx, link_name) in enumerate(link_states_msg.name):
poses[link_name] = pysdf.pose_msg2homogeneous(
link_states_msg.pose[link_idx])
#print('%s:\n%s' % (link_name, poses[link_name]))
for (link_idx, link_name) in enumerate(link_states_msg.name):
#print(link_idx, link_name)
modelinstance_name = link_name.split('::')[0]
#print('modelinstance_name:', modelinstance_name)
model_name = pysdf.name2modelname(modelinstance_name)
#print('model_name:', model_name)
if not model_name in model_cache:
sdf = pysdf.SDF(model=model_name)
model_cache[model_name] = sdf.world.models[0] if len(
sdf.world.models) >= 1 else None
if model_cache[model_name]:
rospy.loginfo('Loaded model: %s' %
model_cache[model_name].name)
else:
rospy.loginfo('Unable to load model: %s' % model_name)
model = model_cache[model_name]
link_name_in_model = link_name.replace(modelinstance_name + '::', '')
if model:
link = model.get_link(link_name_in_model)
if link.tree_parent_joint:
parent_link = link.tree_parent_joint.tree_parent_link
parent_link_name = parent_link.get_full_name()
#print('parent:', parent_link_name)
parentinstance_link_name = parent_link_name.replace(
model_name, modelinstance_name, 1)
else: # direct child of world
parentinstance_link_name = 'gazebo_world'
else: # Not an SDF model
parentinstance_link_name = 'gazebo_world'
#print('parentinstance:', parentinstance_link_name)
if is_ignored(parentinstance_link_name):
rospy.loginfo("Ignoring TF %s -> %s" %
(parentinstance_link_name, link_name))
continue
pose = poses[link_name]
parent_pose = poses[parentinstance_link_name]
rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
translation, quaternion = pysdf.homogeneous2translation_quaternion(
rel_tf)
#print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion))
tfBroadcaster.sendTransform(translation, quaternion,
rospy.get_rostime(),
pysdf.sdf2tfname(link_name),
pysdf.sdf2tfname(parentinstance_link_name))
def publish_tf(self):
model_cache = {}
poses = {'gazebo_world': identity_matrix()}
for (link_idx, link_name) in enumerate(self.link_states_msg.name):
poses[link_name] = pysdf.pose_msg2homogeneous(
self.link_states_msg.pose[link_idx])
# print('%s:\n%s' % (link_name, poses[link_name]))
for (link_idx, link_name) in enumerate(self.link_states_msg.name):
# print(link_idx, link_name)
modelinstance_name = link_name.split('::')[0]
# print('modelinstance_name:', modelinstance_name)
model_name = pysdf.name2modelname(modelinstance_name)
# print('model_name:', model_name)
if not model_name in model_cache:
sdf = pysdf.SDF(model=model_name)
model_cache[model_name] = sdf.world.models[0] if len(
sdf.world.models) >= 1 else None
if not model_cache[model_name]:
print('Unable to load model: %s' % model_name)
model = model_cache[model_name]
link_name_in_model = link_name.replace(modelinstance_name + '::',
'')
if model:
link = model.get_link(link_name_in_model)
if link.tree_parent_joint:
parent_link = link.tree_parent_joint.tree_parent_link
parent_link_name = parent_link.get_full_name()
# print('parent:', parent_link_name)
parentinstance_link_name = parent_link_name.replace(
model_name, modelinstance_name, 1)
else: # direct child of world
parentinstance_link_name = 'gazebo_world'
else: # Not an SDF model
parentinstance_link_name = 'gazebo_world'
# print('parentinstance:', parentinstance_link_name)
pose = poses[link_name]
#parent_pose = pysdf.pose_msg2homogeneous(self.model_states_msg.pose[1])
parent_pose = poses[parentinstance_link_name]
rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
translation, quaternion = pysdf.homogeneous2translation_quaternion(
rel_tf)
# print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion))
self.tfBroadcaster.sendTransform(
translation, quaternion, rospy.get_rostime(),
pysdf.sdf2tfname(link_name),
pysdf.sdf2tfname(parentinstance_link_name))
if self.link_odometry == pysdf.sdf2tfname(link_name):
self.publish_odometry(
translation, quaternion, pysdf.sdf2tfname(link_name),
pysdf.sdf2tfname(parentinstance_link_name))
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 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 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 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 on_link_states_msg(link_states_msg):
"""
Publish tf for each model in current Gazebo world
"""
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
poses = {'gazebo_world': identity_matrix()}
for (link_idx, link_name) in enumerate(link_states_msg.name):
poses[link_name] = pysdf.pose_msg2homogeneous(link_states_msg.pose[link_idx])
#print('%s:\n%s' % (link_name, poses[link_name]))
for (link_idx, link_name) in enumerate(link_states_msg.name):
#print(link_idx, link_name)
modelinstance_name = link_name.split('::')[0]
#print('modelinstance_name:', modelinstance_name)
model_name = pysdf.name2modelname(modelinstance_name)
#print('model_name:', model_name)
if not model_name in model_cache:
sdf = pysdf.SDF(model=model_name)
model_cache[model_name] = sdf.world.models[0] if len(sdf.world.models) >= 1 else None
if model_cache[model_name]:
rospy.loginfo('Loaded model: %s' % model_cache[model_name].name)
else:
rospy.loginfo('Unable to load model: %s' % model_name)
model = model_cache[model_name]
link_name_in_model = link_name.replace(modelinstance_name + '::', '')
if model:
link = model.get_link(link_name_in_model)
if link.tree_parent_joint:
parent_link = link.tree_parent_joint.tree_parent_link
parent_link_name = parent_link.get_full_name()
#print('parent:', parent_link_name)
parentinstance_link_name = parent_link_name.replace(model_name, modelinstance_name, 1)
else: # direct child of world
parentinstance_link_name = 'gazebo_world'
else: # Not an SDF model
parentinstance_link_name = 'gazebo_world'
#print('parentinstance:', parentinstance_link_name)
if is_ignored(parentinstance_link_name):
rospy.loginfo("Ignoring TF %s -> %s" % (parentinstance_link_name, link_name))
continue
pose = poses[link_name]
parent_pose = poses[parentinstance_link_name]
rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf)
#print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion))
tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
def update_collision_object(self, link, full_linkname, **kwargs):
if 'name' in kwargs:
modelinstance_name = kwargs['name']
full_linkname_mod = modelinstance_name + "::" + full_linkname.split(
"::")[1]
link_root = pysdf.sdf2tfname(full_linkname_mod)
self.collision_objects_updated[link_root] = CollisionObject()
self.collision_objects_updated[link_root].id = link_root
self.collision_objects_updated[
link_root].operation = CollisionObject.MOVE
if 'pose' in kwargs:
updated_pose = kwargs['pose']
self.move_collision_object(
self.collision_objects_updated[link_root],
self.collision_objects[link_root], updated_pose)
def prepare_markers(prefix):
world.for_all_links(prepare_link_marker)
for marker in markers:
marker.header.frame_id = prefix + pysdf.sdf2tfname(
marker.header.frame_id)
marker.ns = prefix + pysdf.sdf2tfname(marker.ns)
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 calculate_tfs(prefix):
world.for_all_joints(calculate_joint_tf)
for tf in tfs:
tf[0] = prefix + pysdf.sdf2tfname(tf[0])
tf[1] = prefix + pysdf.sdf2tfname(tf[1])
def calculate_tfs(prefix):
world.for_all_joints(calculate_joint_tf)
for tf in tfs:
tf[0] = prefix + pysdf.sdf2tfname(tf[0])
tf[1] = prefix + pysdf.sdf2tfname(tf[1])
def on_link_states_msg(link_states_msg):
"""
Publish tf for each model in current Gazebo world
"""
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
poses = {'gazebo_world': identity_matrix()}
for (link_idx, link_name) in enumerate(link_states_msg.name):
poses[link_name] = pysdf.pose_msg2homogeneous(
link_states_msg.pose[link_idx])
for (link_idx, link_name) in enumerate(link_states_msg.name):
modelinstance_name = link_name.split('::')[0]
try:
model_name = modelinstance_name
if (submodelsToBeIncluded
and model_name not in submodelsToBeIncluded
) or model_name in submodelsToBeIgnored:
rospy.logdebug(
"gazebo2tf_node: Model instance {} not included".format(
model_name))
continue
else:
rospy.logdebug(
"gazebo2tf_node: Model instance {} included".format(
model_name))
model = load_model(model_name)
parentinstance_link_name = get_parentinstance_link_name(
model, model_name, modelinstance_name, link_name)
assert (parentinstance_link_name)
except Exception:
try:
model_name = pysdf.name2modelname(modelinstance_name)
if (submodelsToBeIncluded
and model_name not in submodelsToBeIncluded
) or model_name in submodelsToBeIgnored:
rospy.logdebug(
"gazebo2tf_node: Model {} not included".format(
model_name))
continue
else:
rospy.logdebug(
"gazebo2tf_node: Model {} included".format(model_name))
model = load_model(model_name)
parentinstance_link_name = get_parentinstance_link_name(
model, model_name, modelinstance_name, link_name)
assert (parentinstance_link_name)
except Exception as e:
rospy.logdebug(
'gazebo2tf_node: Failed to find parent instance link name for '
'model {}, model instance {}: {}'.format(
model_name, modelinstance_name, repr(e)))
continue
if not is_included(parentinstance_link_name) or is_ignored(
parentinstance_link_name):
rospy.loginfo('Ignoring TF {} -> {}'.format(
parentinstance_link_name, link_name))
continue
pose = poses[link_name]
parent_pose = poses[parentinstance_link_name]
rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
translation, quaternion = pysdf.homogeneous2translation_quaternion(
rel_tf)
tfBroadcaster.sendTransform(translation, quaternion,
rospy.get_rostime(),
pysdf.sdf2tfname(link_name),
pysdf.sdf2tfname(parentinstance_link_name))
def prepare_markers(prefix):
world.for_all_links(prepare_link_marker)
for marker in markers:
marker.header.frame_id = prefix + pysdf.sdf2tfname(marker.header.frame_id)
marker.ns = prefix + pysdf.sdf2tfname(marker.ns)
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)
