295 lines
14 KiB
Python
295 lines
14 KiB
Python
import json
|
||
import threading
|
||
import time
|
||
import pandas
|
||
import numpy
|
||
import math
|
||
import csv
|
||
from PySide6.QtCore import Signal, QThread
|
||
from codes.common import clibs
|
||
|
||
|
||
class DoFactoryTest(QThread):
|
||
output = Signal(str, str)
|
||
curve_map = {
|
||
"周期内平均转矩": ["device_servo_trq_feedback", ],
|
||
"周期内最大速度": ["hw_joint_vel_feedback", ],
|
||
}
|
||
|
||
def __init__(self, dir_path, interval, procs, /):
|
||
super().__init__()
|
||
self.dir_path = dir_path
|
||
self.interval = int(interval) if interval != "" else clibs.CYCLE
|
||
self.procs = procs
|
||
self.idx = 6
|
||
self.curves = []
|
||
|
||
def logger(self, level, module, content, color="black", error="", flag="both"):
|
||
flag = "cursor" if level.upper() == "DEBUG" else "both"
|
||
clibs.logger(level, module, content, color, flag, signal=self.output)
|
||
if level.upper() == "ERROR":
|
||
raise Exception(f"{error} | {content}")
|
||
|
||
def initialization(self, data_dirs, data_files):
|
||
@clibs.handle_exception
|
||
def check_files():
|
||
for proc_name, is_enabled in self.procs.items():
|
||
if is_enabled:
|
||
self.curves.extend(self.curve_map[proc_name])
|
||
|
||
if len(self.curves) == 0:
|
||
self.logger("ERROR", "factory", "未查询到需要记录数据的曲线，至少选择一个！", "red", "CurveNameError")
|
||
|
||
if len(data_dirs) != 0 or len(data_files) != 1:
|
||
self.logger("ERROR", "factory", "初始路径下不允许有文件夹，且初始路径下只能存在一个工程文件 —— *.zip，确认后重新运行！", "red", "InitFileError")
|
||
|
||
if not data_files[0].endswith(".zip"):
|
||
self.logger("ERROR", "factory", f"{data_files[0]} 不是一个有效的工程文件，需确认！", "red", "ProjectFileError")
|
||
|
||
return data_files[0]
|
||
|
||
@clibs.handle_exception
|
||
def get_configs():
|
||
robot_type, records = None, None
|
||
msg_id = clibs.c_hr.execution("controller.get_params")
|
||
records = clibs.c_hr.get_from_id(msg_id)
|
||
for record in records:
|
||
if "请求发送成功" not in record[0]:
|
||
robot_type = eval(record[0])["data"]["robot_type"]
|
||
server_file = f"/home/luoshi/bin/controller/robot_cfg/{robot_type}/{robot_type}.cfg"
|
||
local_file = self.dir_path + f"/{robot_type}.cfg"
|
||
clibs.c_pd.pull_file_from_server(server_file, local_file)
|
||
|
||
try:
|
||
with open(local_file, mode="r", encoding="utf-8") as f_config:
|
||
configs = json.load(f_config)
|
||
except Exception as Err:
|
||
self.logger("ERROR", "factory", f"无法打开 {local_file}<br>{Err}", "red", "OpenFileError")
|
||
|
||
# 最大角速度，额定电流，减速比，额定转速
|
||
version = configs["VERSION"]
|
||
m_avs = configs["MOTION"]["JOINT_MAX_SPEED"]
|
||
self.logger("INFO", "factory", f"get_configs: 机型文件版本 {robot_type}_{version}")
|
||
self.logger("INFO", "factory", f"get_configs: 各关节角速度 {m_avs}")
|
||
return m_avs
|
||
|
||
self.logger("INFO", "factory", "正在做初始化校验和配置，这可能需要一点时间......", "green")
|
||
prj_file = check_files()
|
||
if prj_file is None:
|
||
return
|
||
avs = get_configs()
|
||
params = {
|
||
"prj_file": prj_file,
|
||
"interval": self.interval,
|
||
"avs": avs,
|
||
}
|
||
self.logger("INFO", "factory", "数据目录合规性检查结束，未发现问题......", "green")
|
||
return params
|
||
|
||
@clibs.handle_exception
|
||
def change_curve_state(self, stat):
|
||
display_pdo_params = [{"name": name, "channel": chl} for name in self.curves for chl in range(6)]
|
||
clibs.c_hr.execution("diagnosis.open", open=stat, display_open=stat)
|
||
clibs.c_hr.execution("diagnosis.set_params", display_pdo_params=display_pdo_params)
|
||
|
||
@clibs.handle_exception
|
||
def run_rl(self, params):
|
||
prj_file, interval = params["prj_file"], params["interval"]
|
||
# 1. 关闭诊断曲线，触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电
|
||
self.change_curve_state(False)
|
||
clibs.c_md.r_soft_estop(0)
|
||
clibs.c_md.r_soft_estop(1)
|
||
clibs.c_md.r_reset_estop()
|
||
clibs.c_md.r_clear_alarm()
|
||
clibs.c_md.write_act(False)
|
||
time.sleep(1) # 让曲线彻底关闭
|
||
|
||
# 2. reload工程后，pp2main，并且自动模式和上电
|
||
prj_name = ".".join(prj_file.split("/")[-1].split(".")[:-1])
|
||
prj_path = f"{prj_name}/_build/{prj_name}.prj"
|
||
clibs.c_hr.execution("overview.reload", prj_path=prj_path, tasks=["factory"])
|
||
clibs.c_hr.execution("rl_task.pp_to_main", tasks=["factory"])
|
||
clibs.c_hr.execution("state.switch_auto")
|
||
clibs.c_hr.execution("state.switch_motor_on")
|
||
|
||
# 3. 开始运行程序
|
||
self.logger("INFO", "factory", f"正在采集场景工程的周期，大概1min左右......", "blue")
|
||
clibs.c_hr.execution("rl_task.set_run_params", loop_mode=True, override=1.0)
|
||
clibs.c_hr.execution("rl_task.run", tasks=["factory"])
|
||
t_start = time.time()
|
||
while True:
|
||
if clibs.c_md.read_ready_to_go() == 1:
|
||
clibs.c_md.write_act(True)
|
||
break
|
||
else:
|
||
if (time.time() - t_start) > 15:
|
||
self.logger("ERROR", "factory", "15s 内未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", "red", "ReadySignalTimeoutError")
|
||
else:
|
||
time.sleep(clibs.INTERVAL)
|
||
|
||
# 4. 获取初始数据，周期时间，首次的各轴平均电流值，打开诊断曲线，并执行采集
|
||
time.sleep(clibs.INTERVAL*10) # 等待 RL 程序中 scenario_time 初始化
|
||
t_start = time.time()
|
||
while True:
|
||
scenario_time = float(f"{float(clibs.c_md.read_scenario_time()):.2f}")
|
||
if scenario_time != 0:
|
||
self.logger("INFO", "factory", f"耐久工程的周期时间：{scenario_time}s | 单轮次执行时间：{scenario_time+interval}~{scenario_time*2+interval}")
|
||
break
|
||
else:
|
||
time.sleep(clibs.INTERVAL)
|
||
if (time.time() - t_start) > 900:
|
||
self.logger("ERROR", "factory", f"900s内未收到耐久工程的周期时间，需要确认RL程序和工具通信交互是否正常执行（支持最长工程周期时间为300s）......", "red", "GetScenarioTimeError")
|
||
|
||
# 6. 准备数据保存文件
|
||
for proc_name, is_enabled in self.procs.items():
|
||
if not is_enabled:
|
||
continue
|
||
|
||
with open(f"{self.dir_path}/{proc_name}.csv", mode="a+", newline="") as f_csv:
|
||
for curve in self.curve_map[proc_name]:
|
||
titles = [f"{curve}_{i}" for i in range(6)]
|
||
titles.insert(0, "time")
|
||
csv_writer = csv.writer(f_csv)
|
||
csv_writer.writerow(titles)
|
||
|
||
# 7. 开始采集
|
||
count = 0
|
||
while clibs.running[self.idx]:
|
||
this_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
|
||
next_time_1 = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()+scenario_time+interval+1))
|
||
next_time_2 = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()+scenario_time+interval+1+scenario_time))
|
||
self.logger("INFO", "factory", f"[{this_time}] 当前次数：{count:09d} | 预计下次数据更新时间：{next_time_1}~{next_time_2}", "#008B8B")
|
||
count += 1
|
||
# 固定间隔，更新一次数据，打开曲线，获取周期内电流，关闭曲线
|
||
time.sleep(interval)
|
||
while True:
|
||
capture_start = clibs.c_md.read_capture_start()
|
||
if capture_start == 1:
|
||
break
|
||
else:
|
||
time.sleep(clibs.INTERVAL/10)
|
||
|
||
self.change_curve_state(True)
|
||
time.sleep(scenario_time)
|
||
end_time = time.time()
|
||
start_time = end_time - scenario_time
|
||
self.change_curve_state(False)
|
||
# 保留数据并处理输出
|
||
self.gen_results(params, start_time, end_time)
|
||
else:
|
||
self.change_curve_state(False)
|
||
self.logger("INFO", "factory", "后台数据清零完成，现在可以重新运行其他程序。", "green")
|
||
|
||
@clibs.handle_exception
|
||
def gen_results(self, params, start_time, end_time):
|
||
s_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(start_time))
|
||
e_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(end_time))
|
||
try:
|
||
clibs.lock.acquire(True)
|
||
clibs.cursor.execute(f"SELECT content FROM logs WHERE timestamp BETWEEN '{s_time}' AND '{e_time}' AND content LIKE '%diagnosis.result%' ORDER BY id ASC")
|
||
records = clibs.cursor.fetchall()
|
||
finally:
|
||
clibs.lock.release()
|
||
|
||
self.data_proc(records, params)
|
||
|
||
@clibs.handle_exception
|
||
def data_proc(self, records, params):
|
||
for proc_name, is_enabled in self.procs.items():
|
||
if not is_enabled:
|
||
continue
|
||
|
||
if proc_name == "周期内平均转矩":
|
||
# get_avg_trq(records, params, w2t)
|
||
t = threading.Thread(target=self.get_avg_trq, args=(records, params, proc_name))
|
||
t.daemon = True
|
||
t.start()
|
||
elif proc_name == "周期内最大速度":
|
||
# get_joint_max_vel(records, params, w2t)
|
||
t = threading.Thread(target=self.get_joint_max_vel, args=(records, params, proc_name))
|
||
t.daemon = True
|
||
t.start()
|
||
|
||
@clibs.handle_exception
|
||
def get_avg_trq(self, records, params, proc_name):
|
||
d_trq, results = [[], [], [], [], [], []], [time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))]
|
||
for record in records:
|
||
data = eval(record[0])["data"]
|
||
for item in data:
|
||
d_item = reversed(item["value"])
|
||
for axis in range(6):
|
||
if item.get("channel", None) == axis and item.get("name", None) == "device_servo_trq_feedback":
|
||
d_trq[axis].extend(d_item)
|
||
|
||
for axis in range(6):
|
||
df = pandas.DataFrame.from_dict({"device_servo_trq_feedback": d_trq[axis]})
|
||
_ = math.sqrt(numpy.square(df[df.columns[0]].values * 1.27 / 1000).sum() / len(df))
|
||
results.append(_)
|
||
|
||
path = "/".join(params["prj_file"].split("/")[:-1])
|
||
with open(f"{path}/{proc_name}.csv", mode="a+", newline="") as f_csv:
|
||
csv_writer = csv.writer(f_csv)
|
||
csv_writer.writerow(results)
|
||
|
||
@clibs.handle_exception
|
||
def get_joint_max_vel(self, records, params, proc_name):
|
||
d_trq, results = [[], [], [], [], [], []], [time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))]
|
||
for record in records:
|
||
data = eval(record[0])["data"]
|
||
for item in data:
|
||
d_item = reversed(item["value"])
|
||
for axis in range(6):
|
||
if item.get("channel", None) == axis and item.get("name", None) == "hw_joint_vel_feedback":
|
||
d_trq[axis].extend(d_item)
|
||
|
||
for axis in range(6):
|
||
df = pandas.DataFrame.from_dict({"hw_joint_vel_feedback": d_trq[axis]})
|
||
_ = df.max().iloc[0]
|
||
results.append(_)
|
||
|
||
path = "/".join(params["prj_file"].split("/")[:-1])
|
||
with open(f"{path}/{proc_name}.csv", mode="a+", newline="") as f_csv:
|
||
csv_writer = csv.writer(f_csv)
|
||
csv_writer.writerow(results)
|
||
|
||
@staticmethod
|
||
def detect_db_size():
|
||
@clibs.db_lock
|
||
@clibs.handle_exception
|
||
def release_memory():
|
||
line_number = 20000
|
||
leftover = 4000 # 200s
|
||
clibs.cursor.execute("SELECT COUNT(id) FROM logs")
|
||
len_records = clibs.cursor.fetchone()[0]
|
||
if len_records > line_number:
|
||
del_num = len_records - leftover + 1
|
||
clibs.cursor.execute(f"DELETE FROM logs WHERE id < {del_num}")
|
||
clibs.cursor.execute(f"UPDATE logs SET id=(id-{del_num - 1}) WHERE id > {del_num - 1}")
|
||
clibs.cursor.execute(f"UPDATE sqlite_sequence SET seq = {leftover + 1} WHERE name = 'logs' ")
|
||
clibs.cursor.execute("VACUUM")
|
||
|
||
while True:
|
||
release_memory()
|
||
time.sleep(clibs.INTERVAL*10)
|
||
|
||
@clibs.handle_exception
|
||
def processing(self):
|
||
time_start = time.time()
|
||
clibs.running[self.idx] = 1
|
||
if clibs.status["hmi"] != 1 or clibs.status["md"] != 1:
|
||
self.logger("ERROR", "factory", "processing: 需要在网络设置中连接HMI以及Modbus通信！", "red", "NetworkError")
|
||
|
||
t = threading.Thread(target=self.detect_db_size)
|
||
t.daemon = True
|
||
t.start()
|
||
|
||
data_dirs, data_files = clibs.traversal_files(self.dir_path, self.output)
|
||
params = self.initialization(data_dirs, data_files)
|
||
clibs.c_pd.push_prj_to_server(params["prj_file"])
|
||
self.run_rl(params)
|
||
|
||
self.logger("INFO", "factory", "-"*60 + "<br>全部处理完毕<br>", "purple")
|
||
time_total = time.time() - time_start
|
||
msg = f"处理时间：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s"
|
||
self.logger("INFO", "factory", msg)
|