zps.py 文件源码

def get_position(self):
        """ Where are we... """
        world = self.zarj.transform.lookup_transform('world',
                                                     self.zarj.walk.lfname,
                                                     rospy.Time())
        chest_position = self.zarj.transform.lookup_transform(
            'pelvis', 'torso', rospy.Time()).transform

        pose = PoseStamped()
        pose.header.frame_id = 'pelvis'
        pose.header.stamp = rospy.Time()
        pose.pose.position = world.transform.translation
        pose.pose.orientation = chest_position.rotation

        return pose