baxter_scan_object.py 文件源码

def update_camera_transform(self):
        t = self.tl.getLatestCommonTime("/root", "/camera_link")
        position, quaternion = self.tl.lookupTransform("/root",
                                                       "/camera_link",
                                                       t)
        position, quaternion = self.update_transformation(list(position), list(quaternion))

        #get ICP to find rotation and translation
        #multiply old position and orientation by rotation and translation
        #publish new pose to /update_camera_alignment
        self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))