gazebo2rviz.py 文件源码

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

# Main function.