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