def on_model_states_msg(model_states_msg):
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
for (model_idx, modelinstance_name) in enumerate(model_states_msg.name):
#print(model_idx, 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]:
print('Loaded model: %s' % model_cache[model_name].name)
else:
print('Unable to load model: %s' % model_name)
model = model_cache[model_name]
if not model: # Not an SDF model
continue
#print('model:', model)
model.for_all_links(publish_link_marker,
model_name=model_name,
instance_name=modelinstance_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_model_states_msg(model_states_msg):
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
for (model_idx, modelinstance_name) in enumerate(model_states_msg.name):
#print(model_idx, modelinstance_name)
model_name = pysdf.name2modelname(modelinstance_name)
#print('model_name:', model_name)
if model_name == 'ground_plane' or model_name == 'anymal' or model_name == 'dodgeball': # hardcoded: stuff we don`t want the parser to stream
continue
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]
if not model: # Not an SDF model
continue
#print('model:', model)
model.for_all_links(publish_link_marker,
model_name=model_name,
instance_name=modelinstance_name)
def on_model_states_msg(model_states_msg):
global lastUpdateTime
global updatePeriod
global sdf2moveit
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
for (model_idx, modelinstance_name) in enumerate(model_states_msg.name):
model_name = pysdf.name2modelname(modelinstance_name)
if not modelinstance_name in model_cache:
# Add new collision object
model_cache[
modelinstance_name] = sdf2moveit.add_new_collision_object(
model_name, modelinstance_name)
# Move existing object
model = model_cache[modelinstance_name]
sdf2moveit.update_collision_object_with_pose(
model, modelinstance_name, model_states_msg.pose[model_idx])
for modelinstance_name in list(model_cache):
if modelinstance_name not in model_states_msg.name:
# Object has been deleted in gazebo so it needs to be deleted in rviz
rospy.loginfo("Object %s deleted from gazebo" % modelinstance_name)
sdf2moveit.delete_collision_object(modelinstance_name)
del model_cache[modelinstance_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 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_model_states_msg(model_states_msg):
global message_count
if max_messages <= 0 or message_count < max_messages:
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
for (model_idx,
modelinstance_name) in enumerate(model_states_msg.name):
try:
model_name = modelinstance_name
if (submodelsToBeIncluded
and model_name not in submodelsToBeIncluded
) or model_name in submodelsToBeIgnored:
rospy.logdebug(
"gazebo2marker_node: Model instance {} not included".
format(model_name))
continue
model = load_model(model_name)
assert (model)
except Exception:
try:
model_name = pysdf.name2modelname(modelinstance_name)
if (submodelsToBeIncluded
and model_name not in submodelsToBeIncluded
) or model_name in submodelsToBeIgnored:
rospy.logdebug(
"gazebo2marker_node: Model {} not included".format(
model_name))
continue
model = load_model(model_name)
assert (model)
except Exception as e:
rospy.logwarn(
'gazebo2marker_node: Failed to load model {}, '
'model instance {}: {}'.format(model_name,
modelinstance_name,
repr(e)))
continue
rospy.logdebug(
'gazebo2marker_node: Successfully loaded model: {}'.format(
modelinstance_name))
model.for_all_links(publish_link_marker,
model_name=model_name,
instance_name=modelinstance_name)
message_count += 1
else:
return
def publish_marker(self):
for (model_idx,
modelinstance_name) in enumerate(self.model_states_msg.name):
# print(model_idx, modelinstance_name)
model_name = pysdf.name2modelname(modelinstance_name)
# print('model_name:', model_name)
if not model_name in self.model_cache:
sdf = pysdf.SDF(model=model_name)
self.model_cache[model_name] = sdf.world.models[0] if len(
sdf.world.models) >= 1 else None
if not self.model_cache[model_name]:
print('Unable to load model: %s' % model_name)
model = self.model_cache[model_name]
if not model: # Not an SDF model
continue
# print('model:', model)
model.for_all_links(self.publish_link_marker,
model_name=model_name,
instance_name=modelinstance_name)
def on_model_states_msg(model_states_msg):
global lastUpdateTime
sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
if sinceLastUpdateDuration.to_sec() < updatePeriod:
return
lastUpdateTime = rospy.get_rostime()
for (model_idx, modelinstance_name) in enumerate(model_states_msg.name):
#print(model_idx, 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]
if not model: # Not an SDF model
continue
#print('model:', model)
model.for_all_links(publish_link_marker, model_name=model_name, instance_name=modelinstance_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))
