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

debug

Browse Source
This commit is contained in:
vmaosmini 2025-05-14 13:50:09 +08:00
parent 9e25a12683
commit de83fafc0d
1 changed files with 29 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
flandre/launcher.py
View File

@ -352,6 +352,12 @@ def robot_record(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():
@ -359,31 +365,35 @@ def robot_record(folder):
'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1 'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1
robot.rt.start() # rtsi 开始 robot.rt.start() # rtsi 开始
arr = [] arr = []
last_ns = time.time_ns()
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:
continue continue
if recv_out.recipe_id == output1.id: if recv_out.recipe_id == output1.id:
ns = time.time_ns()
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()
print(ns - last_ns,x)
last_ns = ns
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 # ns=ns
) )
arr.append(d) # arr.append(d)
print(arr.__len__()) # print(arr.__len__())
# if arr.__len__() == 100: # if arr.__len__() == 100:
# q.put(arr) # q.put(arr)
# arr = [] # arr = []
@ -393,11 +403,11 @@ def robot_record(folder):
print(q.get()[0]['ns']) print(q.get()[0]['ns'])
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__':