#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

#import roslib
import rospy
import numpy as np
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
#import cv2
cam0_path = '/home/camellia/pic/'  # 已经建立好的存储cam0 文件的目录

n = 0

class PatrolNav():
    def __init__(self):

        # 节点名称
        rospy.init_node('patrol_nav_node', anonymous=True)

        # 终端按下Ctrl+C后可以终止节点
        rospy.on_shutdown(self.shutdown)

        # From launch file get parameters
        # 从patrol_nav.launch文件中得到的参数
        self.rest_time = rospy.get_param("~rest_time", 5)
        self.keep_patrol = rospy.get_param("~keep_patrol", False)
        self.random_patrol = rospy.get_param("~random_patrol", False)
        self.patrol_type = rospy.get_param("~patrol_type", 0)
        self.patrol_loop = rospy.get_param("~patrol_loop", 2)
        self.patrol_time = rospy.get_param("~patrol_time", 5)

        # set all navigation target pose
        # 设置所有的导航目标点，其中还包括了最终的朝向
        # 可以使用Rviz中的"2D Nav Goal"功能来获取到地图上坐标点的坐标和朝向：rostopic echo /move_base_simple/goal
        self.locations = dict()
        self.locations['one'] = Pose(Point(-1.9553, 1.0021, 0.00),Quaternion(0.000, 0.000, 0.73, 0.66))
        self.locations['two'] = Pose(Point(1.7781, -0.2908, 0.00),Quaternion(0.000, 0.000, 0.000, 1.000))
        self.locations['three'] = Pose(Point(1.30, 4.7651, 0.00),Quaternion(0.000, 0.000, 0.000, 1.000))
        self.locations['four'] = Pose(Point(-2.9921, 3.7542, 0.00),Quaternion(0.000, 0.000, 0.70, 0.71))
        # self.locations['five'] = Pose(Point(3.5146985054016113, -1.628450870513916, 0.00),Quaternion(0.000, 0.000, 0.000, 1.000))
        # self.locations['six'] = Pose(Point(1.765, 2.084, 0.000), Quaternion(0.000, 0.000, -0.005, 0.999))

        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
                       'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST']

        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        self.move_base.wait_for_server(rospy.Duration(30))
        rospy.loginfo("Connected to move base server")

        n_successes = 0
        target_num = 0
        location = ""
        start_time = rospy.Time.now()
        locations_cnt = len(self.locations)
        sequeue = ['one', 'two', 'three', 'four', 'five']

        rospy.loginfo("Starting position navigation ")
        # Begin the main loop and run through a sequence of locations
        while not rospy.is_shutdown():
            # target_num = random.randint(0, locations_cnt-1)

            # Get the next location in the current sequence
            # 取出坐标点向move_base发送，move_base发送导航坐标点的函数为self.send_goal
            location = sequeue[target_num]

            rospy.loginfo("target_num_value:" + str(target_num))  # 打印target_num的值

            rospy.loginfo("Going to: " + str(location))
            self.send_goal(location)

            # 我的理解是一次导航给300秒的时间，返回的是此次导航结束的时间，如果finished_within_time=0 说明时间到了导航还没结束，如果finished_within_time！=0说明，导航到了，在300秒以内
            # 有个问题就是这段时间机器人是在运动的，程序会停在这里等机器人运动
            # finished_within_time是个bool值，就是说明有没有超时的意思，超时时间为300秒，如果300秒内没有完成任务就是超时了，finished_within_time是个bool值就是false，如果完成任务了就是true
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.logerr("ERROR:Timed out achieving goal")
            else:
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    global n
                    n += 1
                    print("Successful",n)
                    #message = rospy.wait_for_message('/camera/rgb/image_raw', Image, 5)
                    #cv_img = bridge.imgmsg_to_cv2(message, "bgr8")

                    #image_name = "Image" + str(n) + ".jpg"  # 图像命名
                    #cv2.imwrite(cam0_path + image_name, cv_img)  # 保存；
                    target_num += 1

		#if target_num > 4:
                # shutdown()


    def send_goal(self, locate):
        self.goal = MoveBaseGoal()
        self.goal.target_pose.pose = self.locations[locate]
        self.goal.target_pose.header.frame_id = 'map'
        self.goal.target_pose.header.stamp = rospy.Time.now()
        self.move_base.send_goal(self.goal)  # send goal to move_base

    def shutdown(self):
        rospy.logwarn("Stopping the patrol...")


if __name__ == '__main__':
    try:
        bridge = CvBridge()
        PatrolNav()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.logwarn("patrol navigation exception finished.")