#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
from tf.transformations import quaternion_from_euler


def callback(data):
    # rospy.loginfo(rospy.get_caller_id() + 'I heard %s', )
    rospy.loginfo('received move base action goal')
    # rospy.loginfo(data.goal_id)


def done_cb(data, data1):
    print("action done cb:%d,%s" % (data, data1))


def active_cb():
    print("active_cb")


def feedback_cb(data):
    pass
    # print("feedback_cb x:%f,y%f", data.pose.position.x,data.pose.position.y)
    # print("feedback_cb ", data.base_position.pose.position.x )


if __name__ == '__main__':
    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('init_pose_pub', anonymous=True)

    # rospy.Subscriber('/move_base/goal',MoveBaseActionGoal, callback)
    # rospy.Subscriber('/move_base/goal',MoveBaseActionGoal, callback)

    goal_pub = rospy.Publisher("/Pose", PoseStamped, queue_size=10)
    init_pose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped,
                                    queue_size=10)
    r = rospy.Rate(10)
    rospy.sleep(2.0)
    # while (1):
    # x = 45.5  # 35
    # y = -7.8
    # z = 2.287
    # yaw = 0.0
    x = 0.0  # 35
    y = 0.0
    z = 0.0
    yaw = 0.0
    pose_msg = PoseStamped()
    q = quaternion_from_euler(0.0, 0, yaw)

    initPose = PoseWithCovarianceStamped()
    initPose.pose.pose.orientation.x = q[0]
    initPose.pose.pose.orientation.y = q[1]
    initPose.pose.pose.orientation.z = q[2]
    initPose.pose.pose.orientation.w = q[3]
    initPose.pose.pose.position.x = x
    initPose.pose.pose.position.y = y
    initPose.pose.pose.position.z = z
    initPose.header.frame_id = "map"
    initPose.header.stamp = rospy.Time.now()
    init_pose_pub.publish(initPose)
    print("pub init pose")
    # rospy.spin()
    # pose_msg.pose.position.x = x
    # pose_msg.pose.position.y = y
    # pose_msg.pose.position.z = z
    # pose_msg.pose.orientation.x = q[0]
    # pose_msg.pose.orientation.y = q[1]
    # pose_msg.pose.orientation.z = q[2]
    # pose_msg.pose.orientation.w = q[3]
    # pose_msg.header.stamp = rospy.Time.now()
    # pose_msg.header.frame_id = "map"
    # goal_pub.publish(pose_msg)
    # print("send msg")
    # r.sleep()