def get_frame_in_base_footprint_kdl(self, frame): """ :rtype: PyKDL.Frame """ # if self.T_bf___cam_joint is None: T_bf___cam_joint = msg_to_kdl(lookup_transform('base_footprint', frame)) T_bf___cam_joint.p[2] = 0 T_bf___cam_joint.M = PyKDL.Rotation() return T_bf___cam_joint
def cam_pose_to_base_pose(self, pose, frame): """ :type pose: PoseStamped :rtype: PoseStamped """ T_shelf___cam_joint_g = msg_to_kdl(pose) T_map___bfg = T_shelf___cam_joint_g * self.get_frame_in_base_footprint_kdl(frame).Inverse() base_pose = kdl_to_posestamped(T_map___bfg, pose.header.frame_id) return base_pose