import rospy
from std_msgs.msg import Header
from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovariance, PoseWithCovarianceStamped
from tf.transformations import quaternion_from_euler


def publish_pose(x, y, z, yaw):
    pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
    point = Point(x, y, z)
    q = quaternion_from_euler(0.0, 0.0, yaw).tolist()
    q = Quaternion(0, 0, q[2], q[3])
    data = PoseWithCovarianceStamped(
        Header(0, rospy.Time().now(), '/map'),
        PoseWithCovariance(
            Pose(point, q),
            [0] * 36
        )
    )
    pub.publish(data)


if __name__ == '__main__':
    pub_pose = PubPose()
    pub_pose.publish_pose(0, 0, 0, 0, 0, 0)