yukari/flandre
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add robot record

Browse Source
This commit is contained in:
remilia 2025-05-26 23:12:52 +08:00
parent 2cd7c4ee35
commit 84ee2c1f84
1 changed files with 25 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
flandre/launcher.py
View File

@ -412,13 +412,6 @@ def robot_record(folder):
p = Path(folder) p = Path(folder)
p.mkdir(parents=True, exist_ok=True) p.mkdir(parents=True, exist_ok=True)
robot.setup() robot.setup()
# last_ns = time.perf_counter_ns()
# while True:
# ns = time.perf_counter_ns()
# print(ns - last_ns)
# last_ns = ns
q = queue.Queue() q = queue.Queue()
def rtsi_thread(): def rtsi_thread():
@ -426,8 +419,6 @@ def robot_record(folder):
'actual_TCP_pose,actual_TCP_force,timestamp', 250) # 输出订阅,配方1 'actual_TCP_pose,actual_TCP_force,timestamp', 250) # 输出订阅,配方1
robot.rt.start() # rtsi 开始 robot.rt.start() # rtsi 开始
arr = [] arr = []
last_ns = time.time_ns()
last_device_ts = 0
while True: while True:
recv_out: DataObject = robot.rt.get_output_data() recv_out: DataObject = robot.rt.get_output_data()
if recv_out is None: if recv_out is None:
@ -435,42 +426,41 @@ def robot_record(folder):
if recv_out.recipe_id == output1.id: if recv_out.recipe_id == output1.id:
x, y, z, rx, ry, rz = recv_out.actual_TCP_pose x, y, z, rx, ry, rz = recv_out.actual_TCP_pose
fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force
ns = time.time_ns() local_ns = time.time_ns()
print(ns - last_ns, recv_out.timestamp - last_device_ts, x)
last_ns = ns
last_device_ts = recv_out.timestamp
d = dict( d = dict(
# x=x, x=x,
# y=y, y=y,
# z=z, z=z,
# fx=fx, fx=fx,
# fy=fy, fy=fy,
# fz=fz, fz=fz,
# rx=rx, rx=rx,
# ry=ry, ry=ry,
# rz=rz, rz=rz,
# frx=frx, frx=frx,
# fry=fry, fry=fry,
# frz=frz, frz=frz,
# ns=ns local_ns=local_ns,
device_ts=recv_out.timestamp,
) )
# arr.append(d) arr.append(d)
# print(arr.__len__()) if arr.__len__() == 100:
# if arr.__len__() == 100: q.put(arr)
# q.put(arr) arr = []
# arr = []
def write_thread(): def write_thread():
while True: while True:
print(q.get()[0]['ns']) ns = time.time_ns()
# print(q.get()[0]['ns'])
(p / f'{ns}.json').write_text(json.dumps(q.get()))
tr = threading.Thread(target=rtsi_thread) tr = threading.Thread(target=rtsi_thread)
# tw = threading.Thread(target=write_thread) tw = threading.Thread(target=write_thread)
tr.start() tr.start()
# tw.start() tw.start()
tr.join() tr.join()
# tw.join() tw.join()
if __name__ == '__main__': if __name__ == '__main__':