#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
# from std_msgs.msg import String
# from hinge_car_nav_msgs.msg import HingeCarNavActionGoal
from hinge_car_nav_msgs.msg import hinge_car_nav_msgs
import geometry_msgs.msg
# import sys
# import tf
from tf.transformations import quaternion_from_euler
# import numpy as np
# import sys
# Brings in the SimpleActionClient
# import yaml
import actionlib

# 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('action_client', anonymous=True)

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

    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient(
        'hinge_car_nav', hinge_car_nav_msgs.msg.HingeCarNavAction)

    # Waits until the action server has started up and started
    # listening for goals.

    x = 30.0
    y = 207.0
    yaw = -1.57
    p0 = geometry_msgs.msg.PoseStamped()
    p0.pose.position.x = x
    p0.pose.position.y = y
    # q = quaternion_from_euler(0.0 , 0.0 , yaw)
    # p0.pose.orientation.x = q[0]
    # p0.pose.orientation.y = q[1]
    # p0.pose.orientation.z = q[2]
    # p0.pose.orientation.w = q[3]
    # 定义欧拉角
    roll, pitch = 0.0, 0.0  # 用实际值替换
    q = quaternion_from_euler(roll, pitch, yaw)
    # 创建geometry_msgs/Quaternion类型的四元数
    quaternion_msg = geometry_msgs.msg.Quaternion(*q)
    p0.pose.orientation = quaternion_msg
    print("before client connected")
    client.wait_for_server()
    print("client connected")
    goal = hinge_car_nav_msgs.msg.HingeCarNavGoal()
    path = hinge_car_nav_msgs.msg.HingeCarPath()
    goal.goal_type = goal.UNLOAD_SLAGPOT
    # goal.slagpot_pose.header.frame_id = "world"
    # goal.slagpot_pose.header.stamp = rospy.Time.now()
    # goal.slagpot_pose.pose.orientation = p0.pose.orientation
    # goal.slagpot_pose.pose.position = p0.pose.position
    path.slagpot_pose = p0
    goal.path = path
    client.send_goal(goal, done_cb, active_cb, feedback_cb)
    # Sends the goal to the action server.
    # Waits for the server to finish performing the action.

    client.wait_for_result()
    client.get_result()
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        result = client.get_result()
    except rospy.ROSInterruptException:
        print("program interrupted before completion")
    rospy.sleep(5.0)