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