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
