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 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 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 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))
