def add_new_collision_object(self, model_name, modelinstance_name):
sdf = pysdf.SDF(model=model_name)
num_collision_objects = len(self.collision_objects)
model = sdf.world.models[0] if len(sdf.world.models) >= 1 else None
if model:
model.for_all_links(self.convert_to_collision_object,
name=modelinstance_name)
if len(self.collision_objects) == num_collision_objects:
rospy.logerr('Unable to load model: %s' % model_name)
return None
planning_scene_msg = PlanningScene()
planning_scene_msg.is_diff = True
for (collision_object_root,
collision_object) in self.collision_objects.iteritems():
if collision_object_root in self.ignored_submodels:
pass
else:
planning_scene_msg.world.collision_objects.append(
collision_object)
planning_scene_msg.world.collision_objects[
-1].header.frame_id = 'world'
self.planning_scene_pub.publish(planning_scene_msg)
rospy.loginfo('Loaded model: %s' % modelinstance_name)
return model
else:
rospy.logerr('Unable to load model: %s' % model_name)
return None
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('-p',
'--prefix',
default='',
help='Publish with prefix')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('sdf2tfstatic')
global ignored_submodels
ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';')
rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels)
global tfBroadcaster
tfBroadcaster = tf.TransformBroadcaster()
global world
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
calculate_tfs(args.prefix)
rospy.loginfo('Spinning')
r = rospy.Rate(args.freq)
while not rospy.is_shutdown():
publish_tf()
r.sleep()
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_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_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=2, help='Frequency Markers are published (default: 2 Hz)')
parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
parser.add_argument('-c', '--collision', action='store_true', help='Publish collision instead of visual elements')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('sdf2marker')
global submodelsToBeIgnored
submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of', '').split(';')
rospy.loginfo('Ignoring submodels of: ' + str(submodelsToBeIgnored))
global updatePeriod
updatePeriod = 1. / args.freq
global use_collision
use_collision = args.collision
global markerPub
markerPub = rospy.Publisher('/gazebo2rviz_models_visual', Marker, queue_size=10)
global world
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
prepare_markers(args.prefix)
rospy.loginfo('Spinning')
r = rospy.Rate(args.freq)
while not rospy.is_shutdown():
publishMarkers();
r.sleep()
def main():
parser = argparse.ArgumentParser()
#parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)')
#parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('sdf2moveit_collision')
global ignored_submodels
ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';')
rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels)
planning_scene_pub = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
sdf = pysdf.SDF(model=args.sdf)
world = sdf.world
world.for_all_links(convert_to_collision_object)
planning_scene_msg = PlanningScene()
planning_scene_msg.is_diff = True
for (collision_object_root,
collision_object) in collision_objects.iteritems():
if collision_object_root in ignored_submodels:
print('TODO2') # attached object instead of collision object
else:
planning_scene_msg.world.collision_objects.append(collision_object)
planning_scene_msg.world.collision_objects[
-1].header.frame_id = 'world'
while planning_scene_pub.get_num_connections() < 1:
rospy.sleep(0.1)
planning_scene_pub.publish(planning_scene_msg)
def load_model(model_name):
if not model_name in model_cache:
model_cache[model_name] = None
sdf = pysdf.SDF(model=model_name,
ignore_submodels=submodelsToBeIgnored)
model_cache[model_name] = sdf.world.models[0] if len(
sdf.world.models) >= 1 else None
model = model_cache[model_name]
return model
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 main():
parser = argparse.ArgumentParser()
parser.add_argument('-f',
'--freq',
type=float,
default=2,
help='Frequency Markers are published (default: 2 Hz)')
parser.add_argument('-c',
'--collision',
action='store_true',
help='Publish collision instead of visual elements')
parser.add_argument('-w',
'--worldfile',
type=str,
help='Read models from this world file')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('gazebo2marker')
global submodelsToBeIgnored
submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of',
'').split(';')
rospy.loginfo('Ignoring submodels of: ' + str(submodelsToBeIgnored))
if submodelsToBeIgnored:
rospy.logerr(
'ignore_submodels_of is currently not supported and will thus have no effect'
)
global updatePeriod
updatePeriod = 1. / args.freq
global use_collision
use_collision = args.collision
if args.worldfile:
global worldsdf
worldsdf = pysdf.SDF(file=args.worldfile)
global markerPub
markerPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
rospy.sleep(rospy.Duration(0, 100 * 1000))
global lastUpdateTime
lastUpdateTime = rospy.get_rostime()
modelStatesSub = rospy.Subscriber('gazebo/model_states', ModelStates,
on_model_states_msg)
rospy.loginfo('Spinning')
rospy.spin()
def load_model(model_name):
if not model_name in model_cache:
model_cache[model_name] = None
if worldsdf:
for model in worldsdf.world.models:
if model.name == model_name:
model_cache[model_name] = model
break
else:
sdf = pysdf.SDF(model=model_name,
ignore_submodels=submodelsToBeIgnored)
if len(sdf.world.models) >= 1:
model_cache[model_name] = sdf.world.models[0]
model = model_cache[model_name]
return model
def main():
parser = argparse.ArgumentParser()
parser.add_argument('sdf', help='SDF file to convert')
parser.add_argument('urdf', help='Resulting URDF file to be written')
parser.add_argument('-p', '--plot', nargs=1, help='Plot SDF to file')
parser.add_argument('--no-prefix', action='store_true', help='Do not use model name as name prefix')
args = parser.parse_args()
sdf = pysdf.SDF(file=args.sdf)
world = sdf.world
if args.plot:
world.plot_to_file(args.plot[0])
if len(world.models) != 1:
print('SDF contains %s instead of exactly one model. Aborting.' % len(world.models))
sys.exit(1)
model = world.models[0]
print(model)
model.save_urdf(args.urdf, prefix=None if args.no_prefix else '')
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)
parser.add_argument('--prefix',
action='store_true',
help='Use model name as name prefix')
args = parser.parse_args()
plugins = [
"""
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>%s</robotNamespace>
<topicName>ros_control</topicName>
</plugin>
</gazebo>
"""
]
sdf = pysdf.SDF(args.project, file=args.sdf)
world = sdf.world
if args.plot:
world.plot_to_file(args.plot[0])
if len(world.models) != 1:
print('SDF contains %s instead of exactly one model. Aborting.' %
len(world.models))
sys.exit(1)
model = world.models[0]
#print(model)
model.save_urdf_with_plugin(args.urdf,
plugins,
prefix='' if args.prefix else None)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-f',
'--freq',
type=float,
default=2,
help='Frequency Markers are published (default: 2 Hz)')
parser.add_argument('-c',
'--collision',
action='store_true',
help='Publish collision instead of visual elements')
parser.add_argument('-l',
'--latch',
action='store_true',
help='Latch publisher.')
parser.add_argument(
'-lf',
'--lifetime-forever',
action='store_true',
help='Set marker message duration lifetime to forever.')
parser.add_argument(
'-m',
'--max-messages',
type=int,
default=-1,
help=
'Maximum number of messages to publish per marker (default: -1 = infinite)'
)
parser.add_argument('-w',
'--worldfile',
type=str,
help='Read models from this world file')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('gazebo2marker')
global submodelsToBeIncluded
submodelsToBeIncluded = rospy.get_param('~include_submodels',
'').split(';')
if submodelsToBeIncluded == ['']:
submodelsToBeIncluded = []
else:
rospy.loginfo('gazebo2marker_node: Including submodels: ' +
str(submodelsToBeIncluded))
global submodelsToBeIgnored
submodelsToBeIgnored = rospy.get_param('~ignore_submodels', '').split(';')
if submodelsToBeIgnored == ['']:
submodelsToBeIgnored = []
else:
rospy.loginfo('gazebo2marker_node: Ignoring submodels: ' +
str(submodelsToBeIgnored))
global updatePeriod
updatePeriod = 1. / args.freq
global use_collision
use_collision = args.collision
global latch
if args.latch:
latch = True
global max_messages
max_messages = args.max_messages
global lifetime
if args.lifetime_forever:
lifetime = rospy.Duration(0)
else:
lifetime = rospy.Duration(2 * updatePeriod)
if args.worldfile:
global worldsdf
global submodelsToBeIgnored
rospy.loginfo('gazebo2marker_node: Loading world model: {}'.format(
args.worldfile))
worldsdf = pysdf.SDF(file=args.worldfile,
ignore_submodels=submodelsToBeIgnored)
if worldsdf:
rospy.logdebug(
'gazebo2marker_node: Sucessfully loaded world model {}'.format(
args.worldfile))
else:
rospy.logwarn(
'gazebo2marker_node: Failed to load world model {}'.format(
args.worldfile))
global markerPub
markerPub = rospy.Publisher('/visualization_marker',
Marker,
queue_size=10,
latch=latch)
rospy.sleep(rospy.Duration(0, 100 * 1000))
global lastUpdateTime
lastUpdateTime = rospy.get_rostime()
modelStatesSub = rospy.Subscriber('gazebo/model_states', ModelStates,
on_model_states_msg)
rospy.loginfo('gazebo2marker_node: Spinning')
rospy.spin()
