from geometry_msgs.msg import PoseStamped, Point, PoseWithCovarianceStamped, Pose, Quaternion, Twist
from tf2_geometry_msgs import tf2_geometry_msgs
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import tf2_ros
def  cam2world(msg):
    global ps_out
    buffer = tf2_ros.Buffer()
    ps = tf2_geometry_msgs.PointStamped()
    #设置原坐标相关参数：时间戳、原坐标系、坐标
    ps.header.stamp = rospy.Time()
    ps.header.frame_id = "camera1_color_optical_frame"
    ps.point.x = msg.x
    ps.point.y = msg.y
    ps.point.z = msg.z
    #tf坐标转换：指定转换后坐标系
    ps_out = buffer.transform(ps,"map")
    x=(ps_out.point.x-(-8.05))/0.05
    y=(ps_out.point.y-(-9.52))/0.05
    return x,y