from time import sleep, time, strftime, localtime from sys import argv from os import mkdir from paramiko import SSHClient, AutoAddPolicy from json import loads from openpyxl import load_workbook from pandas import DataFrame, concat from commons import clibs tab_name = clibs.tab_names['at'] logger = clibs.log_prod def check_files(path, loadsel, data_dirs, data_files, w2t): if len(data_dirs) != 0 or len(data_files) != 5: w2t('初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！', 0, 0, 'red', tab_name) w2t(' 1. configs.xlsx\n 2. reach33/reach66/reach100_xxxx.xlsx\n 3. xxxx.zip', 0, 1, 'red', tab_name) config_file = reach33 = reach66 = reach100 = prj_file = None for data_file in data_files: filename = data_file.split('\\')[-1] if filename == 'configs.xlsx': config_file = data_file elif filename.startswith('reach33_') and filename.endswith('.xlsx'): reach33 = data_file elif filename.startswith('reach66_') and filename.endswith('.xlsx'): reach66 = data_file elif filename.startswith('reach100_') and filename.endswith('.xlsx'): reach100 = data_file elif filename.endswith('.zip'): prj_file = data_file else: w2t('初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！', 0, 0, 'red', tab_name) w2t(' 1. configs.xlsx\n 2. reach33/reach66/reach100_xxxx.xlsx\n 3. xxxx.zip', 0, 2, 'red', tab_name) if config_file and reach33 and reach66 and reach100 and prj_file: result_dirs = [] mkdir(f"{path}\\j1") mkdir(f"{path}\\j2") mkdir(f"{path}\\j3") for _reach in ['reach33', 'reach66', 'reach100']: for _load in [f'load{loadsel.removeprefix("tool")}']: for _speed in ['speed33', 'speed66', 'speed100']: dir_name = '_'.join([_reach, _load, _speed]) result_dirs.append(dir_name) mkdir(f"{path}\\j1\\{dir_name}") mkdir(f"{path}\\j2\\{dir_name}") if _reach == 'reach100': mkdir(f"{path}\\j3\\{dir_name}") w2t("数据目录合规性检查结束，未发现问题......", 0, 0, 'blue', tab_name) return config_file, reach33, reach66, reach100, prj_file, result_dirs else: w2t('初始路径下不允许有文件夹，且初始路径下只能存在如下五个文件，确认后重新运行！', 0, 0, 'red', tab_name) w2t(' 1. configs.xlsx\n 2. reach33/reach66/reach100_xxxx.xlsx\n 3. xxxx.zip', 0, 1, 'red', tab_name) def gen_result_file(path, curve_data, axis, _reach, _load, _speed, count): _d2d_vel = {'hw_joint_vel_feedback': []} _d2d_trq = {'device_servo_trq_feedback': []} _d2d_stop = {'device_safety_estop': []} for data in curve_data[-240:]: # 保留最后12s的数据 dict_results = data['data'] for item in dict_results: try: item['value'].reverse() except KeyError: continue if item.get('channel', None) == axis-1 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == axis-1 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 0 and item.get('name', None) == 'device_safety_estop': _d2d_stop['device_safety_estop'].extend(item['value']) df1 = DataFrame.from_dict(_d2d_vel) df2 = DataFrame.from_dict(_d2d_trq) df3 = DataFrame.from_dict(_d2d_stop) df = concat([df1, df2, df3], axis=1) _filename = f"{path}\\j{axis}\\reach{_reach}_load{_load}_speed{_speed}\\reach{_reach}_load{_load}_speed{_speed}_{count}.data" df.to_csv(_filename, sep='\t', index=False) def run_rl(path, loadsel, hr, md, config_file, result_dirs, w2t): _count = 0 _total = 63 display_pdo_params = [ {"name": "hw_joint_vel_feedback", "channel": 0}, {"name": "hw_joint_vel_feedback", "channel": 1}, {"name": "hw_joint_vel_feedback", "channel": 2}, {"name": "hw_joint_vel_feedback", "channel": 3}, {"name": "hw_joint_vel_feedback", "channel": 4}, {"name": "hw_joint_vel_feedback", "channel": 5}, {"name": "device_servo_trq_feedback", "channel": 0}, {"name": "device_servo_trq_feedback", "channel": 1}, {"name": "device_servo_trq_feedback", "channel": 2}, {"name": "device_servo_trq_feedback", "channel": 3}, {"name": "device_servo_trq_feedback", "channel": 4}, {"name": "device_servo_trq_feedback", "channel": 5}, {"name": "device_safety_estop", "channel": 0}, ] wb = load_workbook(config_file, read_only=True) ws = wb['Target'] write_diagnosis = float(ws.cell(row=3, column=10).value) get_init_speed = float(ws.cell(row=4, column=10).value) single_brake = str(ws.cell(row=5, column=10).value) logger.info(f"write_diagnosis = {write_diagnosis}, get_init_speed = {get_init_speed}, single_brake = {single_brake}") if ws.cell(row=1, column=1).value == 'positive': md.write_pon(1) elif ws.cell(row=1, column=1).value == 'negative': md.write_pon(0) else: w2t("configs.xlsx中Target页面A1单元格填写不正确，检查后重新运行...", 0, 111, 'red', tab_name) clibs.execution('diagnosis.open', hr, w2t, tab_name, open=True, display_open=True) clibs.execution('diagnosis.set_params', hr, w2t, tab_name, display_pdo_params=display_pdo_params) for condition in result_dirs: _reach = condition.split('_')[0].removeprefix('reach') _load = condition.split('_')[1].removeprefix('load') _speed = condition.split('_')[2].removeprefix('speed') # for single condition test _single_axis = 0 if single_brake != '0': _total = 3 _single_axis = int(single_brake.split('-')[0]) if _reach != single_brake.split('-')[1] or _load != single_brake.split('-')[2] or _speed != single_brake.split('-')[3]: continue for axis in range(1, 4): # for single condition test if _single_axis != 0 and _single_axis != axis: continue md.write_axis(axis) speed_max = 0 if axis == 3 and _reach != '100': continue w2t(f"-"*90, 0, 0, 'purple', tab_name) for count in range(1, 4): _count += 1 this_time = strftime("%Y-%m-%d %H:%M:%S", localtime(time())) prj_path = 'target/_build/target.prj' w2t(f"[{this_time} | {_count}/{_total}] 正在执行 {axis} 轴 {condition} 的第 {count} 次制动测试...", 0, 0, 'purple', tab_name) # 1. 关闭诊断曲线，触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电 md.trigger_estop() md.reset_estop() md.clear_alarm() md.write_act(0) sleep(write_diagnosis) # 软急停超差后，等待写诊断时间，可通过configs.xlsx配置 while count == 1: # 2. 修改未要执行的场景 ssh = SSHClient() ssh.set_missing_host_key_policy(AutoAddPolicy()) ssh.connect('192.168.0.160', 22, username='luoshi', password='luoshi2019') if ws.cell(row=1, column=1).value == 'positive': _rl_cmd = f"brake_E(j{axis}_{_reach}_p, j{axis}_{_reach}_n, p_speed, p_tool)" elif ws.cell(row=1, column=1).value == 'negative': _rl_cmd = f"brake_E(j{axis}_{_reach}_n, j{axis}_{_reach}_p, p_speed, p_tool)" else: w2t("configs.xlsx中Target页面A1单元格填写不正确，检查后重新运行...", 0, 111, 'red', tab_name) _rl_speed = f"VelSet {_speed}" _rl_tool = f"tool p_tool = tool{loadsel.removeprefix('tool')}" cmd = 'cd /home/luoshi/bin/controller/; ' cmd += 'sudo sed -i "/brake_E/d" projects/target/_build/brake/main.mod; ' cmd += f'sudo sed -i "/DONOTDELETE/i {_rl_cmd}" projects/target/_build/brake/main.mod; ' cmd += 'sudo sed -i "/VelSet/d" projects/target/_build/brake/main.mod; ' cmd += f'sudo sed -i "/MoveAbsJ/i {_rl_speed}" projects/target/_build/brake/main.mod; ' cmd += 'sudo sed -i "/tool p_tool/d" projects/target/_build/brake/main.mod; ' cmd += f'sudo sed -i "/VelSet/i {_rl_tool}" projects/target/_build/brake/main.mod; ' stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True) stdin.write('luoshi2019' + '\n') stdout.read().decode() # 需要read一下才能正常执行 stderr.read().decode() # 3. reload工程后，pp2main，并且自动模式和上电，最后运行程序 clibs.execution('overview.reload', hr, w2t, tab_name, prj_path=prj_path, tasks=['brake', 'stop0_related']) clibs.execution('rl_task.pp_to_main', hr, w2t, tab_name, tasks=['brake', 'stop0_related']) clibs.execution('state.switch_auto', hr, w2t, tab_name) clibs.execution('state.switch_motor_on', hr, w2t, tab_name) clibs.execution('rl_task.set_run_params', hr, w2t, tab_name, loop_mode=True, override=1.0) clibs.execution('rl_task.run', hr, w2t, tab_name, tasks=['brake', 'stop0_related']) _t_start = time() while True: if md.read_ready_to_go() == 1: md.write_act(True) break else: if (time() - _t_start) // 20 > 1: w2t("20s内未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', tab_name) else: sleep(1) # 4. 找出最大速度，传递给RL程序，最后清除相关记录 sleep(get_init_speed+5) # 冗余5s，指定时间后获取实际【正|负】方向的最大速度，可通过configs.xlsx配置 clibs.execution('rl_task.stop', hr, w2t, tab_name, tasks=['brake']) # 找出最大速度 _c_msg = hr.c_msg.copy() _number = 0 for _msg in _c_msg: if _number > get_init_speed*20: # 最开始回零点的时候，二轴速度可以达到最大值，实际摆动时，某一方向可能达不到 break if 'diagnosis.result' in _msg: _number += 1 dict_results = loads(_msg)['data'] for item in dict_results: if item.get('channel', None) == axis-1 and item.get('name', None) == 'hw_joint_vel_feedback': _ = clibs.RADIAN * sum(item['value']) / len(item['value']) if ws.cell(row=1, column=1).value == 'positive': speed_max = max(_, speed_max) elif ws.cell(row=1, column=1).value == 'negative': speed_max = min(_, speed_max) logger.info(f"speed max = {speed_max}") speed_max = abs(speed_max) speed_target = float(ws.cell(row=3, column=axis+1).value) * float(_speed) / 100 if speed_max < speed_target*0.95 or speed_max > speed_target*1.05: w2t(f"Axis: {axis}-{count} | Speed: {speed_max} | Shouldbe: {speed_target}", 0, 0, 'indigo', tab_name) md.write_speed_max(speed_max) hr.c_msg_xs.clear() if len(hr.c_msg) > 270: del hr.c_msg[270:] if speed_max < 10: md.clear_alarm() w2t("未获取到正确的速度，即将重新获取...", 0, 0, 'red', tab_name) continue else: break # 5. 清除软急停，重新运行程序，并打开曲线发送继续运动信号，当速度达到最大值时，通过DO触发急停 md.reset_estop() # 其实没必要 md.clear_alarm() clibs.execution('overview.reload', hr, w2t, tab_name, prj_path=prj_path, tasks=['brake', 'stop0_related']) clibs.execution('rl_task.pp_to_main', hr, w2t, tab_name, tasks=['brake', 'stop0_related']) clibs.execution('state.switch_auto', hr, w2t, tab_name) clibs.execution('state.switch_motor_on', hr, w2t, tab_name) clibs.execution('rl_task.run', hr, w2t, tab_name, tasks=['brake', 'stop0_related']) for i in range(3): if md.read_ready_to_go() == 1: md.write_act(1) break else: sleep(1) else: w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', tab_name) sleep(10) # 排除从其他位姿到零点位姿，再到轴极限位姿的时间 md.write_probe(1) _t_start = time() while True: if md.read_brake_done() == 1: sleep(1) # 保证速度归零 md.write_probe(0) break else: if (time() - _t_start) > 30: w2t(f"30s内未触发急停，该条数据无效，需要确认RL/Python程序编写正确并正常执行，或者判别是否是机器本体问题，比如正负方向速度是否一致...", 0, 0, 'red', tab_name) md.write_probe(0) break else: sleep(1) # 6. 保留数据并处理输出 sleep(1) # 为了让速度降为 0，1s 足够 curve_data = [] _c_msg = hr.c_msg.copy() for _msg in _c_msg: if 'diagnosis.result' in _msg: curve_data.insert(0, loads(_msg)) else: hr.c_msg_xs.clear() if len(hr.c_msg) > 270: del hr.c_msg[270:] gen_result_file(path, curve_data, axis, _reach, _load, _speed, count) else: w2t(f"\n{loadsel.removeprefix('tool')}%负载的制动性能测试执行完毕，如需采集其他负载，须切换负载类型，并更换其他负载，重新执行。", 0, 0, 'green', tab_name) def main(path, hr, md, loadsel, w2t): _s_time = time() data_dirs, data_files = clibs.traversal_files(path, w2t) config_file, reach33, reach66, reach100, prj_file, result_dirs = check_files(path, loadsel, data_dirs, data_files, w2t) clibs.prj_to_xcore(prj_file) run_rl(path, loadsel, hr, md, config_file, result_dirs, w2t) _e_time = time() time_total = _e_time - _s_time w2t(f"处理总时长：{time_total // 3600:02.0f} h {time_total % 3600 // 60:02.0f} m {time_total % 60:02.0f} s", 0, 0, 'green', tab_name) if __name__ == '__main__': main(*argv[1:])