import pymysql
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import PoseStamped
import json
import threading
from time import sleep as sl
from datetime import datetime as dt


class CowdungCart(object):
    def __init__(self):
        self.db = pymysql.connect(host='52.69.200.169', port=3306, user='cowdungcart', password='skyeye', database='CowdungCart', charset='utf8')
        self.cursor = self.db.cursor()


    def queryMySQL(self):
        while True:
            sl(60)
            # self.cursor.close()
            # self.db.close()
            # self.db = pymysql.connect(host='52.69.200.169', port=3306, user='cowdungcart', password='skyeye', database='CowdungCart', charset='utf8')
            # self.cursor = self.db.cursor()
            sql = "select time1, time2, time3, time4 from schedule order by datetime desc limit 1"
            self.cursor.execute(sql)
            schedules = self.cursor.fetchall()[0]
            current_time = dt.now()
            time = dt.strftime(current_time, "%H:%M")
            print(time)
            for schedule in schedules:
                print(schedule)
                if schedule == time:
                    sql = "select goal_change_x, goal_change_y from goal"
                    self.cursor.execute(sql)
                    goals = self.cursor.fetchall()
                    for goal in goals:
                        self.goalPublish(goal)
                        sl(600)


    def goalPublish(self, goal):
        # 定义发布器pub，发布的题目是chatter，消息类型是String，然后queue_size是在订阅者接受消息不够快的时候保留的消息的数量，如果对qos要求低的话可以设为0，不设置的话会出个报警，不过不会出错
        # pub = rospy.Publisher('chatter', String, queue_size=10)
        pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
        # 接下来这里初始化了一个名为talker的节点，节点的名字不能以‘ / ’开头，因为之后会自动给你加一个。anonymous参数在为True的时候会在原本节点名字的后面加一串随机数，来保证可以同时开启多个同样的节点，如果为false的话就只能开一个
        rospy.init_node('goalPublish', anonymous=True)
        # 这里初始化一个Rate对象，通过后面的sleep()可以设定循环的频率
        rate = rospy.Rate(10)  # 10hz

        ps = PoseStamped()
        ps.header.seq = 0
        ps.header.stamp.secs = 0
        ps.header.stamp.nsecs = 0
        ps.header.frame_id = 'map'
        ps.pose.position.x = float(goal[0])
        ps.pose.position.y = float(goal[1])
        ps.pose.position.z = 0.0
        ps.pose.orientation.x = 0.0
        ps.pose.orientation.y = 0.0
        ps.pose.orientation.z = 0.2
        ps.pose.orientation.w = 0.1
        # rospy.loginfo(ps)
        # pub.publish(ps)
        # while not rospy.is_shutdown():
        #     # hello_str = "hello world %s" % rospy.get_time()
        #     rospy.loginfo(ps)
        #     #这里把hello_str发布出去了
        #     pub.publish(ps)
        #     rate.sleep()
        i = 0
        while i < 2:
            rospy.loginfo(ps)
            pub.publish(ps)
            rate.sleep()
            i += 1



    def workOn(self):

        self.queryMySQL()
        self.cursor.close()
        self.db.close()

if __name__ == '__main__':
    cowdungcart = CowdungCart()
    cowdungcart.workOn()