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 main():
parser = argparse.ArgumentParser()
parser.add_argument('-f',
'--freq',
type=float,
default=10,
help='Frequency TFs are published (default: 10 Hz)')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
parser.add_argument('source_from_tf', help='Base Frame from SDF')
parser.add_argument('source_to_tf', help='Target Frame from SDF')
parser.add_argument('target_from_tf',
nargs='?',
help='Published Base Frame')
parser.add_argument('target_to_tf',
nargs='?',
help='Published Target Frame')
args = parser.parse_args(rospy.myargv()[1:])
source_from_tf, source_to_tf = args.source_from_tf, args.source_to_tf
target_from_tf = args.target_from_tf if args.target_from_tf else source_from_tf
target_to_tf = args.target_to_tf if args.target_to_tf else source_to_tf
rospy.init_node('sdf2extract_tfstatic')
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
from_link = world.get_link(source_from_tf)
if not from_link:
rospy.logerr('SDF %s does not contain source_from_tf %s' %
(args.sdf, source_from_tf))
return 1
to_link = world.get_link(source_to_tf)
if not to_link:
rospy.logerr('SDF %s does not contain source_to_tf %s' %
(args.sdf, source_to_tf))
return 2
from_to_tf = concatenate_matrices(inverse_matrix(from_link.pose_world),
to_link.pose_world)
translation, quaternion = pysdf.homogeneous2translation_quaternion(
from_to_tf)
tfBroadcaster = tf.TransformBroadcaster()
rospy.loginfo(
'Publishing TF %s -> %s from SDF %s as TF %s -> %s: t=%s q=%s' %
(source_from_tf, source_to_tf, args.sdf, target_from_tf, target_to_tf,
translation, quaternion))
rospy.loginfo('Spinning')
r = rospy.Rate(args.freq)
while not rospy.is_shutdown():
tfBroadcaster.sendTransform(translation, quaternion,
rospy.get_rostime(), target_to_tf,
target_from_tf)
r.sleep()
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 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 calculate_joint_tf(joint, full_jointname):
full_prefix = full_jointname.replace(joint.name, '')
if is_ignored(joint.parent_model):
print("Ignoring TF %s -> %s" %
(full_prefix + joint.parent, full_prefix + joint.child))
return
rel_tf = concatenate_matrices(
inverse_matrix(joint.tree_parent_link.pose_world),
joint.tree_child_link.pose_world)
translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf)
tfs.append([
full_prefix + joint.parent, full_prefix + joint.child, translation,
quaternion
])
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-f', '--freq', type=float, default=10, help='Frequency TFs are published (default: 10 Hz)')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
parser.add_argument('source_from_tf', help='Base Frame from SDF')
parser.add_argument('source_to_tf', help='Target Frame from SDF')
parser.add_argument('target_from_tf', nargs='?', help='Published Base Frame')
parser.add_argument('target_to_tf', nargs='?', help='Published Target Frame')
args = parser.parse_args(rospy.myargv()[1:])
source_from_tf, source_to_tf = args.source_from_tf, args.source_to_tf
target_from_tf = args.target_from_tf if args.target_from_tf else source_from_tf
target_to_tf = args.target_to_tf if args.target_to_tf else source_to_tf
rospy.init_node('sdf2extract_tfstatic')
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
from_link = world.get_link(source_from_tf)
if not from_link:
rospy.logerr('SDF %s does not contain source_from_tf %s' % (args.sdf, source_from_tf))
return 1
to_link = world.get_link(source_to_tf)
if not to_link:
rospy.logerr('SDF %s does not contain source_to_tf %s' % (args.sdf, source_to_tf))
return 2
from_to_tf = concatenate_matrices(inverse_matrix(from_link.pose_world), to_link.pose_world)
translation, quaternion = pysdf.homogeneous2translation_quaternion(from_to_tf)
tfBroadcaster = tf.TransformBroadcaster()
rospy.loginfo('Publishing TF %s -> %s from SDF %s as TF %s -> %s: t=%s q=%s' % (source_from_tf, source_to_tf, args.sdf, target_from_tf, target_to_tf, translation, quaternion))
rospy.loginfo('Spinning')
r = rospy.Rate(args.freq)
while not rospy.is_shutdown():
tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), target_to_tf, target_from_tf)
r.sleep()
def calculate_joint_tf(joint, full_jointname): full_prefix = full_jointname.replace(joint.name, '') rel_tf = concatenate_matrices(inverse_matrix(joint.tree_parent_link.pose_world), joint.tree_child_link.pose_world) translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf) tfs.append([full_prefix + joint.parent, full_prefix + joint.child, translation, quaternion])
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))
