add queue record rtsi
This commit is contained in:
vmaosmini
parent
081c1bc2f3
commit
9e25a12683
@ -4,8 +4,10 @@ import json
|
||||
import logging
|
||||
import multiprocessing
|
||||
import os
|
||||
import queue
|
||||
import shutil
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
import tomllib
|
||||
from enum import Enum, auto
|
||||
@ -50,7 +52,8 @@ class LaunchComponent(Enum):
|
||||
def launch(arg: dict[LaunchComponent, dict]):
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
multiprocessing.set_start_method('spawn')
|
||||
bp = multiprocessing.Process(target=Broker(broker=True), kwargs=dict(software_config=C))
|
||||
bp = multiprocessing.Process(target=Broker(
|
||||
broker=True), kwargs=dict(software_config=C))
|
||||
bp.start()
|
||||
ps = []
|
||||
for k, v in arg.items():
|
||||
@ -63,7 +66,8 @@ def launch(arg: dict[LaunchComponent, dict]):
|
||||
|
||||
pps = []
|
||||
for p in ps:
|
||||
pps.append(multiprocessing.Process(target=p, kwargs=dict(software_config=C)))
|
||||
pps.append(multiprocessing.Process(
|
||||
target=p, kwargs=dict(software_config=C)))
|
||||
for p in pps:
|
||||
p.start()
|
||||
|
||||
@ -119,7 +123,8 @@ def entrypoint(dev):
|
||||
@click.option('--data_folder', default=None)
|
||||
@click.option('--generate_pyqt', default=True)
|
||||
@click.option('-p', '--path', type=str,
|
||||
default=platformdirs.user_config_path('Flandre', 'Scarlet') / 'launch.toml',
|
||||
default=platformdirs.user_config_path(
|
||||
'Flandre', 'Scarlet') / 'launch.toml',
|
||||
help='Path to launch.toml'
|
||||
)
|
||||
def gui(data_folder, generate_pyqt, path):
|
||||
@ -128,7 +133,8 @@ def gui(data_folder, generate_pyqt, path):
|
||||
return
|
||||
if generate_pyqt:
|
||||
subprocess.run([pyuic6, '-o', P.PYQT / 'Main.py', P.PYQT / 'Main.ui'])
|
||||
subprocess.run([pyuic6, '-o', P.PYQT / 'Image.py', P.PYQT / 'Image.ui'])
|
||||
subprocess.run(
|
||||
[pyuic6, '-o', P.PYQT / 'Image.py', P.PYQT / 'Image.ui'])
|
||||
if data_folder is not None:
|
||||
C.record_folder = Path(data_folder)
|
||||
|
||||
@ -289,7 +295,8 @@ def device_recvzero():
|
||||
b = pull.recv()
|
||||
ts, sequence_id, encoder, buffer = b2t(b)
|
||||
if ts == last_ts:
|
||||
print('tszero', sequence_id, encoder, (ts - last_ts) / 10 ** 6, flush=True)
|
||||
print('tszero', sequence_id, encoder,
|
||||
(ts - last_ts) / 10 ** 6, flush=True)
|
||||
print(b2b(b), b2b(last_b))
|
||||
if sequence_id == last_sequence_id:
|
||||
print('szero', sequence_id, encoder, flush=True)
|
||||
@ -325,7 +332,8 @@ def robot():
|
||||
@robot.command('monitor')
|
||||
def robot_monitor():
|
||||
robot.setup()
|
||||
output1 = robot.rt.output_subscribe('actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1
|
||||
output1 = robot.rt.output_subscribe(
|
||||
'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1
|
||||
robot.rt.start() # rtsi 开始
|
||||
while True:
|
||||
recv_out: DataObject = robot.rt.get_output_data()
|
||||
@ -343,30 +351,53 @@ def robot_record(folder):
|
||||
p = Path(folder)
|
||||
p.mkdir(parents=True, exist_ok=True)
|
||||
robot.setup()
|
||||
output1 = robot.rt.output_subscribe('actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1
|
||||
robot.rt.start() # rtsi 开始
|
||||
while True:
|
||||
recv_out: DataObject = robot.rt.get_output_data()
|
||||
if recv_out is None:
|
||||
continue
|
||||
if recv_out.recipe_id == output1.id:
|
||||
x, y, z, rx, ry, rz = recv_out.actual_TCP_pose
|
||||
fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force
|
||||
d = dict(
|
||||
x=x,
|
||||
y=y,
|
||||
z=z,
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
fz=fz,
|
||||
rx=rx,
|
||||
ry=ry,
|
||||
rz=rz,
|
||||
frx=frx,
|
||||
fry=fry,
|
||||
frz=frz,
|
||||
)
|
||||
(p / f'{time.time_ns()}.rtsi.json').write_text(json.dumps(d))
|
||||
|
||||
q = queue.Queue()
|
||||
|
||||
def rtsi_thread():
|
||||
output1 = robot.rt.output_subscribe(
|
||||
'actual_TCP_pose,actual_TCP_force', 250) # 输出订阅,配方1
|
||||
robot.rt.start() # rtsi 开始
|
||||
arr = []
|
||||
while True:
|
||||
recv_out: DataObject = robot.rt.get_output_data()
|
||||
if recv_out is None:
|
||||
continue
|
||||
if recv_out.recipe_id == output1.id:
|
||||
ns = time.time_ns()
|
||||
x, y, z, rx, ry, rz = recv_out.actual_TCP_pose
|
||||
fx, fy, fz, frx, fry, frz = recv_out.actual_TCP_force
|
||||
d = dict(
|
||||
x=x,
|
||||
y=y,
|
||||
z=z,
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
fz=fz,
|
||||
rx=rx,
|
||||
ry=ry,
|
||||
rz=rz,
|
||||
frx=frx,
|
||||
fry=fry,
|
||||
frz=frz,
|
||||
ns=ns
|
||||
)
|
||||
arr.append(d)
|
||||
print(arr.__len__())
|
||||
# if arr.__len__() == 100:
|
||||
# q.put(arr)
|
||||
# arr = []
|
||||
|
||||
def write_thread():
|
||||
while True:
|
||||
print(q.get()[0]['ns'])
|
||||
|
||||
tr = threading.Thread(target=rtsi_thread)
|
||||
tw = threading.Thread(target=write_thread)
|
||||
tr.start()
|
||||
tw.start()
|
||||
tr.join()
|
||||
tw.join()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
25
test/device_gui.py
Normal file
25
test/device_gui.py
Normal file
@ -0,0 +1,25 @@
|
||||
import struct
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import zmq
|
||||
|
||||
from flandre.config import C
|
||||
from flandre.nodes.Device import Device, DeviceCmd
|
||||
from flandre.nodes.Mi import Mi
|
||||
|
||||
if __name__ == '__main__':
|
||||
context = zmq.Context()
|
||||
req_driver_socket = context.socket(zmq.REQ)
|
||||
req_driver_socket.connect(C.live_rep_socket)
|
||||
magic = 7355608
|
||||
cmd = DeviceCmd.GetData
|
||||
cv2.namedWindow("test", cv2.WINDOW_AUTOSIZE)
|
||||
while True:
|
||||
req_driver_socket.send(struct.pack('i', magic) + struct.pack('i', cmd.value))
|
||||
b = req_driver_socket.recv()
|
||||
arr = np.frombuffer(b, dtype=np.int16, offset=4 + 8 + 4).reshape((256, 5002))
|
||||
cv2.imshow('test', arr)
|
||||
cv2.waitKey(10)
|
||||
# print(b.__len__())
|
||||
Loading…
Reference in New Issue
Block a user