#!/usr/bin/env python3

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 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.
    p0 = geometry_msgs.msg.Point()
    p00 = geometry_msgs.msg.Point()
    p1 = geometry_msgs.msg.Point()
    p2 = geometry_msgs.msg.Point()
    p3 = geometry_msgs.msg.Point()
    p4 = geometry_msgs.msg.Point()

    # in map
    # p1.x = 0.0
    # p1.y = 0.0
    # p2.x = 162.0
    # p2.y = 2.5
    # p3.x = 162.0
    # p3.y = 30.5  #28
    # p4.x = 0.0
    # p4.y = 28

    p0.x = 132.64  # 开始点位
    p0.y = 197.3

    p00.x = p0.x + 5.0  # 终点点位
    p00.y = p0.y

    p1.x = 180.0  #184.93  
    p1.y = 195.7

    p2.x = 19.45   # 维修间14.45
    p2.y = 199.64

    p3.x = 19.81
    p3.y = 172.2

    p4.x = 180.0   #185.08
    p4.y = 168.3

    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()
    tp = []
    goal.goal_type = goal.BACKWARD

    # s = hinge_car_nav_msgs.msg.Segment()
    # s.segment_type = s.LINE
    # start_point = geometry_msgs.msg.Point()
    # start_point.x = 0.0
    # start_point.y = 0.0
    # s.start_point = start_point

    # end_point = geometry_msgs.msg.Point()
    # end_point.x = 162
    # end_point.y = 2.5
    # s.end_point = end_point
    # s.max_vel = 0.45
    # tp.append(s)
    # path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    s.start_point = p0
    s.end_point = p2
    s.max_vel = -0.45
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.CIRCLE
    s.start_point = p2
    s.end_point = p3
    s.center_point.x = 0.5 * (p2.x + p3.x)
    s.center_point.y = 0.5 * (p2.y + p3.y)
    s.max_vel = -0.45
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    s.start_point = p3
    s.end_point = p4
    s.max_vel = -0.45
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.CIRCLE
    s.start_point = p4
    s.end_point = p1
    s.center_point.x = 0.5 * (p1.x + p4.x)
    s.center_point.y = 0.5 * (p1.y + p4.y)
    s.max_vel = -0.45
    path.target_path.append(s)

    s = hinge_car_nav_msgs.msg.Segment()
    s.segment_type = s.LINE
    s.start_point = p1
    s.end_point = p00
    s.max_vel = -0.45
    path.target_path.append(s)


    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)