LISHUZUOXUN_yangjiang/Exercise3/running_muwb_pure_detection.py

396 lines
15 KiB
Python
Raw Normal View History

2024-09-23 14:54:15 +08:00
# coding=gb2312
import json
import os.path
import sys
from Database.manager_database import *
from Speaker.speak_base import SpeakServer, beep
from UWB.positioning_standalone_v2 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]
ANCHOR_RSSI_ADJUSTMENT_PATH = f"{PATH}/Exercise3/anchor_adjustment.json"
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><>վУ׼<D0A3><D7BC><EFBFBD><EFBFBD>
anchor_list = list(positioning.multi_uwb.uwb_driver_table.keys())
default_effective_rssi = -84
if os.path.exists(ANCHOR_RSSI_ADJUSTMENT_PATH):
with open(ANCHOR_RSSI_ADJUSTMENT_PATH, "r", encoding="utf-8_sig") as file:
self.anchor_rssi_adjustment = json.load(file)
if len(self.anchor_rssi_adjustment) < len(anchor_list):
self.anchor_rssi_adjustment = {
anchor: default_effective_rssi
for anchor in anchor_list
}
with open(ANCHOR_RSSI_ADJUSTMENT_PATH, "w", encoding="utf-8_sig") as file:
json.dump(self.anchor_rssi_adjustment, file)
else:
self.anchor_rssi_adjustment = {
anchor: default_effective_rssi
for anchor in anchor_list
}
with open(ANCHOR_RSSI_ADJUSTMENT_PATH, "w", encoding="utf-8_sig") as file:
json.dump(self.anchor_rssi_adjustment, 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()
self.start_time = 0
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()
# <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>¼
self.running_record = {
tag: {
RUNNING_COUNTING: -1,
RUNNING_STATUS: STATUS_EXIT,
RUNNING_MES: "δ<EFBFBD><EFBFBD>ʼ",
RUNNING_COST: 0,
RUNNING_DONE: False,
}
for tag in self.test_tag
}
# <20><><EFBFBD>ܲ<EFBFBD><DCB2><EFBFBD>
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)
beep(duration=700, freq=640)
self.__start_running.set()
# <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:
record_time, mes = data
tag = mes[TAG]
if tag not in self.test_tag:
continue
distance = mes[DIST]
rssi = mes[RSSI]
anchor = mes[ANCHOR_ID]
self.running_record_list.setdefault(tag, [])
self.running_record_list[tag].append(
{RECORD: record_time, DIST: distance, RSSI: rssi, ANCHOR_ID: anchor}
)
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>ж<EFBFBD>ʱ<EFBFBD><CAB1>
leave_detect_time = 10
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
# <20>ϴν<CFB4><CEBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
last_enter_time = self.start_time
# <20>ϴ<EFBFBD><CFB4><EFBFBD><EBBFAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
last_leave_time = -1
# <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>¼<EFBFBD><C2BC>ʱ<EFBFBD><CAB1>
record_time = self.start_time
for one_record in sorted_record:
try:
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
if one_record[RECORD] < self.start_time - bias:
continue
record_time = one_record[RECORD]
# <20><EFBFBD>ж<EFBFBD>1
if (
# <20><><EFBFBD><EFBFBD><EFBFBD>ϴν<CFB4><CEBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD>һ<EFBFBD><D2BB>ʱ<EFBFBD><CAB1>
record_time - last_leave_time > leave_detect_time
and this_tag[RUNNING_STATUS] == STATUS_ACROSS
):
this_tag[RUNNING_STATUS] = STATUS_EXIT
this_tag[RUNNING_MES] = "<EFBFBD>"
# <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (
# <20><><EFBFBD>ҵ<EFBFBD>ǰʱ<C7B0><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><CFB4>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD>ڵ<EFBFBD>Ȧʱ<C8A6><EFBFBD><E4A3AC><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EBBFAA>
record_time - last_leave_time > self.min_round_time
and this_tag[RUNNING_STATUS] == STATUS_EXIT
):
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, {})
this_round_cost = 0 if record_time - last_enter_time < 0 else record_time - last_enter_time
self.round_record[person_id][this_tag[RUNNING_COUNTING]] = this_round_cost
last_enter_time += this_round_cost
# <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
break
last_leave_time = record_time
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD><DAB3><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD>Ҿ<EFBFBD><D2BE><EFBFBD><EFBFBD>ϴμ<CFB4><CEBC><EFBFBD>ʱ<EFBFBD><EFBFBD><E4B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD>³<EFBFBD><C2B3><EFBFBD>ʱ<EFBFBD><CAB1>
if (
this_tag[RUNNING_STATUS] == STATUS_ACROSS
# and record_time - last_leave_time < detect_time_range
):
last_leave_time = record_time
except Exception as e:
print(traceback.format_exc())
print(f"<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:{e.args}")
# <20><EFBFBD>ж<EFBFBD>2
if (
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EBBFAA>ʱ<EFBFBD><CAB1><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ʱ<EFBFBD><CAB1>
this_time - last_leave_time > leave_detect_time
# <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><DDBE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>ʱ<EFBFBD><CAB1>
and this_time - record_time > leave_detect_time
and this_tag[RUNNING_STATUS] == STATUS_ACROSS
):
this_tag[RUNNING_STATUS] = STATUS_EXIT
this_tag[RUNNING_MES] = "<EFBFBD>"
if this_time - last_leave_time > self.min_round_time and this_tag[RUNNING_STATUS] == STATUS_EXIT:
this_tag[RUNNING_MES] = "<EFBFBD>س<EFBFBD>"
# print(this_tag)
# <20><><EFBFBD>¼<EFBFBD>¼
self.update_and_play(tag=tag, new_record=this_tag)
# <20><>Ϣһ<CFA2><D2BB>
time.sleep(0.5)
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
# print(self.running_record)
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] or self.start_time == 0 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 * 100
if record[RUNNING_COUNTING] > 0 else 0
}
score.append(one)
# print(score)
return score