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