beihang/public/test3d.py

import mysql.connector
import time
import threading
from queue import Queue
from trilateration3d import trilateration3d
import numpy as np
import matplotlib.pyplot as plt

# 基站列表及坐标
reader_ids = ['10001', '10006', '10005', '10007']
# 基站坐标
MapBSList = {'10001': [8.43, 14.35, 11.5], '10006': [5.59, -4.65, 1.5], '10005': [13.85, 0, 1.5], '10007': [0, 0, 1.5]}
# 场地坐标
field_vertices = {'begin_x': 0, 'begin_y': -10, 'begin_z': 0, 'end_x': 20, 'end_y': 20, 'end_z': 30}

x = []
y1 = []
y2 = []
y3 = []
y4 = []
loc_x = []
loc_y = []
loc_z = []


class DbThread(threading.Thread):
    def __init__(self, dist_queue, interval):
        threading.Thread.__init__(self)
        self.dist_queue = dist_queue
        self.interval = interval

    def run(self):
        # while True:
        self.conn = mysql.connector.connect(
            # host="172.31.239.56",
            host="121.42.8.157",
            user="root",
            # port=13306,
            port=13307,
            password="~cs3x@2022#123~",
            database="xcjkdba"
        )
        self.cursor = self.conn.cursor()
        # now = datetime.now()  # 获取当前时间
        # two_seconds_ago = (now - timedelta(seconds=2)).strftime('%Y-%m-%d %H:%M:%S')  # 获取两秒之前的时间
        # one_second_ago = (now - timedelta(seconds=1)).strftime('%Y-%m-%d %H:%M:%S')
        # sqlstr = "SELECT card_id, loc_time, distances FROM his_location_cell_card_tof_aodi where card_id = 0001 and loc_time >= '" + two_seconds_ago + "' AND loc_time < '" + one_second_ago + "'"  # + "' limit 1"
        sqlstr = """SELECT card_id, loc_time, distances FROM his_location_cell_card_tof_aodi WHERE card_id = '0001' AND loc_time BETWEEN '2023-05-06 17:19:00' AND '2023-05-06 18:21:00' """
        self.cursor.execute(sqlstr)
        results = self.cursor.fetchall()
        # print(results)
        for row in results:
            # print(row)
            templist = row[2].split(",")
            dist_data = {}
            for i in range(0, len(templist), 2):
                dist_data[templist[i + 1]] = float(templist[i])
            self.dist_queue.put(dist_data)
        self.cursor.close()
        self.conn.close()
        time.sleep(self.interval)


def update_dist_data(dist_data):
    if "10001" in dist_data:
        y1.append(dist_data['10001'])
    else:
        y1.append(None)
    if "10006" in dist_data:
        y3.append(dist_data['10006'])
    else:
        y3.append(None)
    if "10005" in dist_data:
        y4.append(dist_data['10005'])
    else:
        y4.append(None)
    if "10007" in dist_data:
        y2.append(dist_data['10007'])
    else:
        y2.append(None)

    if len(y1) > 100:
        x.pop(0)
        y1.pop(0)
        y3.pop(0)
        y4.pop(0)
        y2.pop(0)
        x.append(x[-1] + 1)
    else:
        x.append(len(x) + 1)


def update_loc_data(dist_data):
    if len(dist_data) >= 4:
        center = trilateration3d(MapBSList, dist_data)
        if center is not None:
            loc_x.append(center.x)
            loc_y.append(center.y)
            loc_z.append(center.z)
            print(center.x, center.y, center.z)
            return center.x, center.y, center.z
        else:
            print("center is None")
            return None

    if len(loc_y) > 10:
        loc_x.pop(0)
        loc_y.pop(0)
        loc_z.pop(0)


dist_queue = Queue()
loc_queue = Queue()
db_thread = DbThread(dist_queue, interval=0.5)
db_thread.start()

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.subplots_adjust(left=0.05, right=0.95, bottom=0.05, top=0.95)
ax.set_title(" 3D Scatter Plot", size=15)
ax.set_xticks(np.arange(0, 10, 1))
ax.set_yticks(np.arange(0, 10, 1))
ax.set_zticks(np.arange(0, 10, 1))
ax.tick_params(labelsize=8)

ax.set_xlim(0, 10)
ax.set_ylim(0, 10)
ax.set_zlim(0, 10)

# 初始化一个空的散点图对象
sc = ax.scatter([], [], [], c='r', marker='o')

# 初始化数据
x_data = []
y_data = []
z_data = []

while True:
    if not dist_queue.empty():
        dist_data = dist_queue.get()
        update_dist_data(dist_data)
        uld = update_loc_data(dist_data)
        if uld:
            x_data.append(uld[0])
            y_data.append(uld[1])
            z_data.append(uld[2])
            if len(x_data) > 5:
                x_data.pop(0)
                y_data.pop(0)
                z_data.pop(0)
            # 更新散点图的数据
            # scatter._offsets3d = (x_data, y_data, z_data)
            sc.remove()
            sc = ax.scatter(x_data, y_data, z_data, c='r', marker='o', s=5)
            # 重新绘制图形
            plt.draw()
            plt.pause(0.25)

    time.sleep(0.25)