LISHUZUOXUN_yangjiang/Exercise3/running_muwb.py

415 lines
17 KiB
Python
Raw Normal View History

2024-09-23 14:54:15 +08:00
# coding=gb2312
import json
import sys
from Database.manager_database import *
from Speaker.speak_base import SpeakServer, beep
from UWB.positioning_standalone import *
FINISH_LINE_SIZE1 = 0
FINISH_LINE_SIZE2 = 1
RUNNING_COUNTING = "cnt"
RUNNING_STATUS = "stat"
RUNNING_MES = "mes"
STATUS_ACROSS = "across"
STATUS_EXIT = "exit"
RUNNING_LAST_STATUS_TIME = "lst"
RUNNING_COST = "cost"
RUNNING_DONE = 'done'
PATH = sys.path[0]
def score_compute(age, cost, score_data):
my_age_index = 0
for age_index in range(len(score_data["age"])):
candidate_age = score_data["age"][age_index]
if candidate_age[0] <= age <= candidate_age[1]:
my_age_index = age_index
break
my_score_index = len(score_data["values"]) - 1
for value_index in range(len(score_data["values"]) - 1, 0 - 1, -1):
value = score_data["values"][value_index][my_age_index]
if cost <= value[0] * 60 + value[1]:
my_score_index = value_index
else:
break
if cost > score_data["values"][-1][my_age_index][0] * 60 + score_data["values"][-1][my_age_index][1]:
my_base_score = 0
else:
my_base_score = score_data["score"][my_score_index]
if my_base_score == 100:
value = score_data["values"][my_score_index][my_age_index]
base_score = value[0] * 60 + value[1]
my_score = my_base_score + (base_score - cost) // 5 * 1
else:
my_score = my_base_score
return my_score
class Running:
def __init__(self, positioning: Positioning, round_num=3, min_round_time=30) -> None:
self.min_round_time = float(min_round_time)
self.round_num = int(round_num)
self.positioning = positioning
self.positioning_tag = None
self.finish_line = {}
# <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
self.start_time = 0
# <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
self.stop_time = 0
# <20>ܲ<EFBFBD><DCB2><EFBFBD>¼
self.running_record = {}
# <20>ܲ<EFBFBD><DCB2><EFBFBD>ˮ<EFBFBD><CBAE>¼
self.running_record_list = {}
# <20>ܲ<EFBFBD>Ȧʱ<C8A6><CAB1>¼
self.round_record = {}
# <20><>¼<EFBFBD>ֻ<EFBFBD>
self.test_tag = []
# <20>ֻ<EFBFBD><D6BB><EFBFBD>Ӧ<EFBFBD><D3A6>Ա<EFBFBD><D4B1>Ϣ
self.person_mes = {}
# <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>״̬
self.__start_running = threading.Event()
self.__start_running.clear()
# ˵<><CBB5>
self.speak_driver = SpeakServer()
self.speak_driver.start()
# <20>ɼ<EFBFBD><C9BC>ļ<EFBFBD>
file = open(f"{PATH}/Exercise3/running_score.json")
self.score_data = json.load(file)
# <20>ܲ<EFBFBD><DCB2><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>
self.final_data = {}
# <20>Ƿ񲥱<C7B7>
self.is_play = False
def set_play(self, is_play):
self.is_play = is_play
def update_and_play(self, tag, new_record):
if self.is_play:
this_record = self.running_record[tag]
if this_record[RUNNING_COUNTING] != new_record[RUNNING_COUNTING]:
person_name = self.person_mes[tag][NAME]
person_id = self.person_mes[tag][ID]
if new_record[RUNNING_DONE]:
self.speak_driver.add_speak(f"{person_name}<EFBFBD><EFBFBD><EFBFBD>ɿ<EFBFBD><EFBFBD>ԣ<EFBFBD>")
else:
self.speak_driver.add_speak(f"{person_name}ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>{new_record[RUNNING_COUNTING]}Ȧ<EFBFBD><EFBFBD>")
self.running_record[tag] = new_record
def set_config(self, round_num, min_round_time):
self.min_round_time = float(min_round_time)
self.round_num = int(round_num)
def add_tag(self, tag):
self.test_tag.append(tag)
def del_tag(self, tag):
self.test_tag.remove(tag)
del self.person_mes[tag]
def add_tag_mes(self, tag, mes):
self.person_mes[tag] = mes
def reset(self):
self.test_tag.clear()
self.person_mes.clear()
def start(self):
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
self.__start_running.clear()
self.round_record.clear()
self.positioning.clear_information()
self.positioning.resume()
self.speak_driver.add_speak("׼<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD>ԣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>")
counting = 5
for i in range(counting):
start_time = time.time()
self.speak_driver.add_speak(counting - i)
while time.time() - start_time < 1:
time.sleep(0.05)
self.speak_driver.wait_4_speak()
self.start_time = time.time()
self.positioning.set_valid_time(self.start_time)
duration = 700 # millisecond
freq = 640 # Hz
beep(freq, duration)
self.__start_running.set()
# <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>¼
self.running_record = {
tag: {
RUNNING_COUNTING: -1,
RUNNING_STATUS: STATUS_EXIT,
RUNNING_MES: "<EFBFBD>",
RUNNING_COST: 0,
RUNNING_DONE: False,
}
for tag in self.test_tag
}
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
threading.Thread(target=self._thread_running, daemon=True).start()
threading.Thread(target=self._thread_processing, daemon=True).start()
def kill(self):
# <20><>¼<EFBFBD>ֻ<EFBFBD>
self.test_tag.clear()
self.running_record_list.clear()
self.__start_running.clear()
def stop(self):
# # <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
self.stop_time = time.time()
# self.start_time = 0
self.kill()
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ճɼ<D5B3>
self.final_data.clear()
raw_score = self.get_score()
for score in raw_score:
band_id = score["band_id"]
person_mes = self.person_mes[band_id]
tag_mes = self.running_record[band_id]
score.update(person_mes)
score.update(tag_mes)
person_id = score["id"]
round_time = self.round_record.get(person_id)
round_time_record = round_time if round_time else []
score.update({"round_time": round_time_record})
score.update({
"fix": False, "final_result": round_time_record
})
self.final_data[person_id] = score
# <20><><EFBFBD>õ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>гɼ<D0B3>
def get_all_score(self):
return list(self.final_data.values())
# <20>ɼ<EFBFBD><C9BC>޸<EFBFBD>
def fix_score(self, person_id):
if not self.final_data[person_id][RUNNING_DONE]:
detected_round = len(self.final_data[person_id]["round_time"])
lacked_round = self.round_num - detected_round + 1
result = [
self.final_data[person_id]["round_time"][i]
for i in range(detected_round)
]
# <20><><EFBFBD><EFBFBD>û<EFBFBD>гɼ<D0B3><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>
if len(result) <= 1:
total_time = self.stop_time - self.start_time
result = [total_time / (self.round_num + 1) for i in range(self.round_num + 1)]
# <20><><EFBFBD><EFBFBD><EFBFBD>гɼ<D0B3>
else:
for _ in range(lacked_round):
max_result = max(result)
index = result.index(max_result)
result = result[0:index:] + [max_result / 2, max_result / 2] + result[index + 1::]
self.final_data[person_id]["final_result"] = {
i: result[i] for i in range(len(result))
}
self.final_data[person_id]["fix"] = True
self.final_data[person_id]["total_time"] = sum(result)
# <20>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>
def fix_withdraw(self, person_id):
self.final_data[person_id]["final_result"] = self.final_data[person_id]["round_time"]
self.final_data[person_id]["fix"] = False
def _thread_processing(self):
while self.__start_running.is_set():
data = self.positioning.get_last_information()
try:
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
if data:
# print(data)
record_time, mes = data
# print(self.start_time - record_time, len(self.positioning.tag_information))
if record_time < self.start_time - 3:
continue
tag = mes[TAG]
if tag not in self.test_tag:
continue
avg_distance = mes[DIST]
avg_rssi = mes[RSSI]
self.running_record_list.setdefault(tag, [])
self.running_record_list[tag].append(
{RECORD: record_time, DIST: avg_distance, RSSI: avg_rssi}
)
except Exception as e:
traceback.format_exc()
print(f"<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>{e.args}")
def _thread_running(self):
# <20><>ǰ<EFBFBD><C7B0>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
bias = 3
# <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
judge_window_time = 6
detect_time_range = judge_window_time / 2
# <20><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
effective_distance = 300
effective_rssi = -80
# <20><>ʼ<EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
judge_distance = 700
judge_rssi = -82
while self.__start_running.is_set():
this_time = time.time()
for tag, record in self.running_record_list.copy().items():
if tag not in self.test_tag or self.running_record[tag][RUNNING_DONE]:
continue
# <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Աid
person_id = self.person_mes[tag]["id"]
sorted_record = sorted(record, key=lambda x: x[RECORD])
this_tag = {
RUNNING_COUNTING: -1,
RUNNING_STATUS: STATUS_EXIT,
RUNNING_MES: "<EFBFBD>",
RUNNING_COST: 0,
RUNNING_DONE: False
}
# Cache
last_status_time = -1
last_enter_time = self.start_time
for one_record in sorted_record:
try:
record_time = one_record[RECORD]
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
if record_time < self.start_time - bias or this_time - record_time < detect_time_range:
continue
distance = one_record[DIST]
rssi = one_record[RSSI]
# <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><E4B4B0><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>
frontward_window = list(
filter(
lambda x: -detect_time_range <= record_time - x[RECORD] < 0,
sorted_record
)
)
# <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (
(
# CASE1 δ<><CEB4>û<EFBFBD>м<EFBFBD><D0BC>ٵ<EFBFBD><D9B5><EFBFBD><EFBFBD><EFBFBD>
len(frontward_window) == 0
# δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EBB6BC><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD>
or min([fw[DIST] for fw in frontward_window]) > distance
# δ<><CEB4><EFBFBD>źŶ<C5BA><C5B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
or max([fw[RSSI] for fw in frontward_window]) < rssi
) or (
# CASE2 С<><D0A1><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
distance <= effective_distance
or rssi >= effective_rssi
)
) and (
# <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>Ȧʱ<C8A6><EFBFBD><E4A3AC><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EBBFAA><EFBFBD><EFBFBD><EFBFBD>Ҵﵽ<D2B4>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
record_time - last_status_time > self.min_round_time
and this_tag[RUNNING_STATUS] == STATUS_EXIT
and (distance < judge_distance or rssi >= judge_rssi)
):
last_status_time = record_time
this_tag[RUNNING_STATUS] = STATUS_ACROSS
this_tag[RUNNING_MES] = "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>"
this_tag[RUNNING_COUNTING] += 1
# Ȧ<><C8A6>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>¼
self.round_record.setdefault(person_id, {})
self.round_record[person_id][
this_tag[RUNNING_COUNTING]
] = 0 if record_time - last_enter_time < 0 else record_time - last_enter_time
last_enter_time += self.round_record[person_id][
this_tag[RUNNING_COUNTING]
]
# <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if this_tag[RUNNING_COUNTING] == self.round_num:
this_tag[RUNNING_DONE] = True
this_tag[RUNNING_COST] = record_time - self.start_time
# <20><EFBFBD>ж<EFBFBD>1
elif (
(
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD>֮ǰ<D6AE>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
len(frontward_window) > 0
and
(
min([fw[DIST] for fw in frontward_window]) > judge_distance
or max([fw[RSSI] for fw in frontward_window]) < judge_rssi
)
) and (
# <20><><EFBFBD>ҵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EBBFAA>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD>ǰ״̬Ϊ<CCAC><CEAA><EFBFBD>ߣ<EFBFBD><DFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><CFB4>ж<EFBFBD>ʱ<EFBFBD><EFBFBD><E4B3AC>һ<EFBFBD><D2BB><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
(
distance > effective_distance
or rssi < effective_rssi
)
and this_tag[RUNNING_STATUS] == STATUS_ACROSS
and record_time - last_status_time > detect_time_range
)
):
last_status_time = record_time
this_tag[RUNNING_STATUS] = STATUS_EXIT
this_tag[RUNNING_MES] = "<EFBFBD>"
# <20><EFBFBD>ж<EFBFBD>2
if (
(
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
len(frontward_window) == 0
) and (
# <20><><EFBFBD>ҵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EBBFAA>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD>ǰ״̬Ϊ<CCAC><CEAA><EFBFBD><EFBFBD>
(distance > 0 or rssi < -60)
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
or this_time - record_time > detect_time_range
)
and this_tag[RUNNING_STATUS] == STATUS_ACROSS
):
last_status_time = record_time
this_tag[RUNNING_STATUS] = STATUS_EXIT
this_tag[RUNNING_MES] = "<EFBFBD>"
except Exception as e:
traceback.format_exc()
print(f"<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:{e.args}")
if this_time - last_status_time > self.min_round_time and this_tag[RUNNING_STATUS] == STATUS_EXIT:
this_tag[RUNNING_MES] = "<EFBFBD>س<EFBFBD>"
# <20><><EFBFBD>¼<EFBFBD>¼
self.update_and_play(tag=tag, new_record=this_tag)
# <20><>Ϣһ<CFA2><D2BB>
time.sleep(1)
def get_person_round_time(self, person_id):
if person_id not in self.final_data.keys():
return {}
return self.final_data[person_id]["final_result"]
def get_valid_score(self):
result = []
for person_id, data in self.final_data.items():
if data["done"] or data["fix"]:
data["score"] = score_compute(data["age"], data["total_time"], self.score_data)
result.append(data)
return result
# <20><>ȡ<EFBFBD>ɼ<EFBFBD><C9BC>ӿ<EFBFBD>
def get_score(self):
score = []
this_cost_time = time.time() - self.start_time
for tag, record in self.running_record.items():
if record[RUNNING_COUNTING] < 0:
finish_status = "δ<EFBFBD><EFBFBD>ʼ"
else:
if record[RUNNING_COUNTING] == 0:
finish_status = f"<EFBFBD>ѿ<EFBFBD>ʼ({record[RUNNING_MES]})"
else:
finish_status = f"<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>({record[RUNNING_MES]})"
if record[RUNNING_COUNTING] == self.round_num:
finish_status = "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɿ<EFBFBD><EFBFBD><EFBFBD>"
age = self.person_mes[tag]["age"]
one = {
"band_id": tag,
"score": score_compute(age, record[RUNNING_COST], self.score_data)
if record[RUNNING_COUNTING] == self.round_num else 0,
"total_time": record[RUNNING_COST] if record[RUNNING_DONE] else this_cost_time,
"round": record[RUNNING_COUNTING] if record[RUNNING_COUNTING] > 0 else 0,
"finish": finish_status,
"percentage": record[RUNNING_COUNTING] / self.round_num
if record[RUNNING_COUNTING] > 0 else 0
}
score.append(one)
return score