def __init__(self,
device_manager,
device_info=None,
node: RR.RobotRaconteurNode = None):
self._lock = threading.RLock()
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self._devices_states = self._node.GetStructureType(
'tech.pyri.devices_states.PyriDevicesStates')
self._device_not_found = self._node.GetExceptionType(
'tech.pyri.device_manager.DeviceNotFound')
self._device_manager = device_manager
self._refresh_counter = 0
self._devices = dict()
self._seqno = 0
self._date_time_util = DateTimeUtil(self._node)
self._date_time_utc_type = self._node.GetPodDType(
'com.robotraconteur.datetime.DateTimeUTC')
self._isoch_info = self._node.GetStructureType(
'com.robotraconteur.device.isoch.IsochInfo')
self._refresh_devices(1)
self._timer = self._node.CreateTimer(0.1, self._timer_cb)
self._timer.Start()
def __init__(self, device_id, width, height, fps, camera_info):
#if platform.system() == "Windows":
# self._capture = cv2.VideoCapture(device_id + cv2.CAP_DSHOW)
#else:
self._capture = cv2.VideoCapture(device_id)
assert self._capture.isOpened(), f"Could not open device: {device_id}"
self._seqno = 0
self._capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self._capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
self._capture.set(cv2.CAP_PROP_FPS, fps)
self._imaging_consts = RRN.GetConstants('com.robotraconteur.imaging')
self._image_consts = RRN.GetConstants('com.robotraconteur.image')
self._image_type = RRN.GetStructureType(
'com.robotraconteur.image.Image')
self._image_info_type = RRN.GetStructureType(
'com.robotraconteur.image.ImageInfo')
self._compressed_image_type = RRN.GetStructureType(
'com.robotraconteur.image.CompressedImage')
self._date_time_utc_type = RRN.GetPodDType(
'com.robotraconteur.datetime.DateTimeUTC')
self._isoch_info = RRN.GetStructureType(
'com.robotraconteur.device.isoch.IsochInfo')
self._capture_lock = threading.Lock()
self._streaming = False
self._fps = self._capture.get(cv2.CAP_PROP_FPS)
self._camera_info = camera_info
self._date_time_util = DateTimeUtil(RRN)
self._sensor_data_util = SensorDataUtil(RRN)
def test_now():
date_time_util = DateTimeUtil(RRN)
print(date_time_util.UtcNow())
uuid_dtype = RRN.GetNamedArrayDType("com.robotraconteur.uuid.UUID")
identifier_type = RRN.GetStructureType(
"com.robotraconteur.identifier.Identifier")
device_info_type = RRN.GetStructureType(
"com.robotraconteur.device.DeviceInfo")
uuid_info = RRN.ArrayToNamedArray(
np.array([
0xa8, 0xe3, 0xc0, 0x7b, 0x12, 0xd0, 0x45, 0xd9, 0x84, 0xb0, 0x01,
0xe9, 0x75, 0xf0, 0x95, 0x7f
], np.uint8), uuid_dtype)
device_info = device_info_type()
device_info.device = identifier_type()
device_info.device.name = "test_device"
device_info.device.uuid = uuid_info
print(date_time_util.UtcNow(device_info))
print(date_time_util.TimeSpec2Now(device_info))
print(date_time_util.TimeSpec3Now())
print(date_time_util.FillDeviceTime(device_info, 275837))
def __init__(self,
device_manager,
device_info=None,
node: RR.RobotRaconteurNode = None,
blockly_compiler_dir=None):
self._lock = threading.RLock()
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self._blockly_compiler_dir = blockly_compiler_dir
self._status_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureExecutionStatus')
self._action_const = self._node.GetConstants(
'com.robotraconteur.action')
self._sandbox_const = self._node.GetConstants('tech.pyri.sandbox')
self._output_codes = self._sandbox_const["ProcedureOutputTypeCode"]
self._blockly_compiler = BlocklyCompiler(
compiler_dir=self._blockly_compiler_dir)
self._device_manager = device_manager
self._executors = []
self._stopped = False
self._running = False
self._output = []
self._output_evt = threading.Condition()
self._datetime_util = DateTimeUtil(node=self._node)
self._run_number = 0
self._procedure_output_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureOutput')
self._procedure_output_list_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureOutputList')
class CameraImpl(object):
def __init__(self, device_id, width, height, fps, camera_info):
#if platform.system() == "Windows":
# self._capture = cv2.VideoCapture(device_id + cv2.CAP_DSHOW)
#else:
self._capture = cv2.VideoCapture(device_id)
assert self._capture.isOpened(), f"Could not open device: {device_id}"
self._seqno = 0
self._capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self._capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
self._capture.set(cv2.CAP_PROP_FPS, fps)
self._imaging_consts = RRN.GetConstants('com.robotraconteur.imaging')
self._image_consts = RRN.GetConstants('com.robotraconteur.image')
self._image_type = RRN.GetStructureType(
'com.robotraconteur.image.Image')
self._image_info_type = RRN.GetStructureType(
'com.robotraconteur.image.ImageInfo')
self._compressed_image_type = RRN.GetStructureType(
'com.robotraconteur.image.CompressedImage')
self._date_time_utc_type = RRN.GetPodDType(
'com.robotraconteur.datetime.DateTimeUTC')
self._isoch_info = RRN.GetStructureType(
'com.robotraconteur.device.isoch.IsochInfo')
self._capture_lock = threading.Lock()
self._streaming = False
self._fps = self._capture.get(cv2.CAP_PROP_FPS)
self._camera_info = camera_info
self._date_time_util = DateTimeUtil(RRN)
self._sensor_data_util = SensorDataUtil(RRN)
def RRServiceObjectInit(self, ctx, service_path):
self._downsampler = RR.BroadcastDownsampler(ctx)
self._downsampler.AddPipeBroadcaster(self.frame_stream)
self._downsampler.AddPipeBroadcaster(self.frame_stream_compressed)
self._downsampler.AddPipeBroadcaster(self.preview_stream)
self._downsampler.AddWireBroadcaster(self.device_clock_now)
self.frame_stream.MaxBacklog = 2
self.frame_stream_compressed.MaxBacklog = 2
self.preview_stream.MaxBacklog = 2
# TODO: Broadcaster peek handler in Python
self.device_clock_now.PeekInValueCallback = lambda ep: self._date_time_util.FillDeviceTime(
self._camera_info.device_info, self._seqno)
@property
def device_info(self):
return self._camera_info.device_info
@property
def camera_info(self):
return self._camera_info
def _cv_mat_to_image(self, mat):
is_mono = False
if (len(mat.shape) == 2 or mat.shape[2] == 1):
is_mono = True
image_info = self._image_info_type()
image_info.width = mat.shape[1]
image_info.height = mat.shape[0]
if is_mono:
image_info.step = mat.shape[1]
image_info.encoding = self._image_consts["ImageEncoding"]["mono8"]
else:
image_info.step = mat.shape[1] * 3
image_info.encoding = self._image_consts["ImageEncoding"]["bgr888"]
image_info.data_header = self._sensor_data_util.FillSensorDataHeader(
self._camera_info.device_info, self._seqno)
image = self._image_type()
image.image_info = image_info
image.data = mat.reshape(mat.size, order='C')
return image
def _cv_mat_to_compressed_image(self, mat, quality=100):
is_mono = False
if (len(mat.shape) == 2 or mat.shape[2] == 1):
is_mono = True
image_info = self._image_info_type()
image_info.width = mat.shape[1]
image_info.height = mat.shape[0]
image_info.step = 0
image_info.encoding = self._image_consts["ImageEncoding"]["compressed"]
image_info.data_header = self._sensor_data_util.FillSensorDataHeader(
self._camera_info.device_info, self._seqno)
image = self._compressed_image_type()
image.image_info = image_info
res, encimg = cv2.imencode(".jpg", mat,
[int(cv2.IMWRITE_JPEG_QUALITY), quality])
assert res, "Could not compress frame!"
image.data = encimg
return image
def capture_frame(self):
with self._capture_lock:
ret, mat = self._capture.read()
if not ret:
raise RR.OperationFailedException("Could not read from camera")
self._seqno += 1
return self._cv_mat_to_image(mat)
def capture_frame_compressed(self):
with self._capture_lock:
ret, mat = self._capture.read()
if not ret:
raise RRN.OperationFailedException(
"Could not read from camera")
self._seqno += 1
return self._cv_mat_to_compressed_image(mat)
def trigger(self):
raise RR.NotImplementedException("Not available on this device")
def frame_threadfunc(self):
while (self._streaming):
with self._capture_lock:
ret, mat = self._capture.read()
if not ret:
#TODO: notify user?
self._streaming = False
continue
self._seqno += 1
self.frame_stream.AsyncSendPacket(self._cv_mat_to_image(mat),
lambda: None)
self.frame_stream_compressed.AsyncSendPacket(
self._cv_mat_to_compressed_image(mat), lambda: None)
self.preview_stream.AsyncSendPacket(
self._cv_mat_to_compressed_image(mat, 70), lambda: None)
device_now = self._date_time_util.FillDeviceTime(
self._camera_info.device_info, self._seqno)
self.device_clock_now.OutValue = device_now
def start_streaming(self):
if (self._streaming):
raise RR.InvalidOperationException("Already streaming")
self._streaming = True
t = threading.Thread(target=self.frame_threadfunc)
t.start()
def stop_streaming(self):
if (not self._streaming):
raise RR.InvalidOperationException("Not streaming")
self._streaming = False
@property
def isoch_downsample(self):
return self._downsampler.GetClientDownsample(
RR.ServerEndpoint.GetCurrentEndpoint())
@isoch_downsample.setter
def isoch_downsample(self, value):
return self._downsampler.SetClientDownsample(
RR.ServerEndpoint.GetCurrentEndpoint(), value)
@property
def isoch_info(self):
ret = self._isoch_info()
ret.update_rate = self._fps
ret.max_downsample = 100
ret.isoch_epoch = np.zeros((1, ), dtype=self._date_time_utc_type)
@property
def capabilities(self):
return 0x1 | 0x2 | 0x4
def __init__(self):
self.tool_state_type = RRN.NewStructure(
"com.robotraconteur.robotics.tool.ToolState")
self._date_time_util = DateTimeUtil(RRN)
class PyriDevicesStatesService:
def __init__(self,
device_manager,
device_info=None,
node: RR.RobotRaconteurNode = None):
self._lock = threading.RLock()
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self._devices_states = self._node.GetStructureType(
'tech.pyri.devices_states.PyriDevicesStates')
self._device_not_found = self._node.GetExceptionType(
'tech.pyri.device_manager.DeviceNotFound')
self._device_manager = device_manager
self._refresh_counter = 0
self._devices = dict()
self._seqno = 0
self._date_time_util = DateTimeUtil(self._node)
self._date_time_utc_type = self._node.GetPodDType(
'com.robotraconteur.datetime.DateTimeUTC')
self._isoch_info = self._node.GetStructureType(
'com.robotraconteur.device.isoch.IsochInfo')
self._refresh_devices(1)
self._timer = self._node.CreateTimer(0.1, self._timer_cb)
self._timer.Start()
def RRServiceObjectInit(self, ctx, service_path):
self._downsampler = RR.BroadcastDownsampler(ctx)
self._downsampler.AddWireBroadcaster(self.devices_states)
def _refresh_devices(self, timeout):
self._device_manager.refresh_devices(timeout)
active_devices = self._device_manager.get_device_names()
for d in active_devices:
if d not in self._devices:
sub = self._device_manager.get_device_subscription(d)
device_ident = self._device_manager.get_device_info(d).device
self._devices[d] = PyriDevicesStatesActiveDevice(
d, device_ident, sub, self._node)
for d in list(self._devices.keys()):
if d not in active_devices:
del self._devices[d]
for dev in self._devices.values():
dev.refresh_types()
def getf_device_info(self, local_device_name):
return self._device_manager.get_device_info(local_device_name)
def getf_standard_device_info(self, local_device_name):
device_client = self._device_manager.get_device_client(
local_device_name, 0.2)
return device_client.device_info
def getf_extended_device_info(self, local_device_name):
with self._lock:
dev = self._devices.get(local_device_name, None)
if dev is None:
raise self._device_not_found(
f"Unknown local_device_name: {local_device_name}")
return dev.get_extended_device_info(1)
def _timer_cb(self, evt):
with self._lock:
self._seqno += 1
#print("Timer callback!")
if self._refresh_counter >= 50:
self._refresh_counter = 0
self._refresh_devices(0)
else:
self._refresh_counter += 1
devices_states = dict()
for d in self._devices.values():
devices_states[d.local_device_name] = d.get_device_state()
s = self._devices_states()
s.ts = self._date_time_util.TimeSpec3Now()
s.seqno = self._seqno
s.devices_states = devices_states
self.devices_states.OutValue = s
#print(self._seqno)
def close(self):
try:
self._timer.Stop()
except:
pass
@property
def isoch_downsample(self):
return self._downsampler.GetClientDownsample(
RR.ServerEndpoint.GetCurrentEndpoint())
@isoch_downsample.setter
def isoch_downsample(self, value):
return self._downsampler.SetClientDownsample(
RR.ServerEndpoint.GetCurrentEndpoint(), value)
@property
def isoch_info(self):
ret = self._isoch_info()
ret.update_rate = self._fps
ret.max_downsample = 100
ret.isoch_epoch = np.zeros((1, ), dtype=self._date_time_utc_type)
class PyriSandbox():
def __init__(self,
device_manager,
device_info=None,
node: RR.RobotRaconteurNode = None,
blockly_compiler_dir=None):
self._lock = threading.RLock()
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self._blockly_compiler_dir = blockly_compiler_dir
self._status_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureExecutionStatus')
self._action_const = self._node.GetConstants(
'com.robotraconteur.action')
self._sandbox_const = self._node.GetConstants('tech.pyri.sandbox')
self._output_codes = self._sandbox_const["ProcedureOutputTypeCode"]
self._blockly_compiler = BlocklyCompiler(
compiler_dir=self._blockly_compiler_dir)
self._device_manager = device_manager
self._executors = []
self._stopped = False
self._running = False
self._output = []
self._output_evt = threading.Condition()
self._datetime_util = DateTimeUtil(node=self._node)
self._run_number = 0
self._procedure_output_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureOutput')
self._procedure_output_list_type = self._node.GetStructureType(
'tech.pyri.sandbox.ProcedureOutputList')
def execute_procedure(self, procedure_name, params):
try:
if _valid_name_re.match(procedure_name) is None:
raise RR.InvalidArgumentException("Procedure name is invalid")
variable_manager = self._device_manager.get_device_client(
"variable_storage", 1)
procedure_src = variable_manager.getf_variable_value(
"procedure", procedure_name)
procedure_tags = variable_manager.getf_variable_tags(
"procedure", procedure_name)
assert isinstance(procedure_src.data,
str), "Procedure variable must be string"
if "pyri" in procedure_tags:
pyri_src = procedure_src.data
elif "blockly" in procedure_tags:
blockly_blocks = get_all_blockly_blocks()
pyri_src = self._blockly_compiler.compile(
procedure_name, procedure_src.data, blockly_blocks)
else:
assert False, "Invalid procedure type (must be pyri or blockly)"
plugin_sandbox_functions = get_all_plugin_sandbox_functions()
loc = {}
byte_code = compile_restricted(pyri_src,
'<pyri_sandbox>',
'exec',
policy=_policy)
sandbox_builtins = guards.get_pyri_builtins_with_name_guard(
plugin_sandbox_functions.keys())
sandbox_globals = {'__builtins__': sandbox_builtins}
sandbox_globals.update(plugin_sandbox_functions)
def send_local_print(text):
self._send_print(text, procedure_name,
self._output_codes["output"])
print_collector = PrintCollector()
print_collector.print_event += send_local_print
sandbox_globals["_print_"] = print_collector
self._run_number += 1
run_number = self._run_number
executor = ExecuteProcedureGenerator(self, procedure_name,
byte_code, sandbox_builtins,
sandbox_globals, loc, params,
print_collector, self._node,
self._device_manager,
self._status_type, run_number)
with self._lock:
if self._stopped:
raise RR.InvalidOperationException("Sandbox is stopping")
self._executors.append(executor)
self._send_print(f"Procedure \"{procedure_name}\" started",
procedure_name, self._output_codes["status"])
return executor
except Exception:
self._send_print(
f"Execute procedure \"{procedure_name}\" failed:\n\n{traceback.format_exc()}",
procedure_name, self._output_codes["error"])
raise
def stop_all(self):
with self._lock:
ex = self._executors
if len(ex) != 0:
self._stopped = True
for e in ex:
e._stopped()
def _close(self):
try:
self._blockly_compiler.close()
except:
pass
def _execution_complete(self, executor):
with self._lock:
if executor in self._executors:
self._executors.remove(executor)
self._stopped = False
self._running = False
def _send_print(self, text, procedure_name, status):
with self._output_evt:
o = self._procedure_output_type()
o.output_number = 1
if len(self._output) > 0:
o.output_number = self._output[-1].output_number + 1
o.output_type = status
o.time = self._datetime_util.UtcNow(self.device_info)
o.procedure_name = procedure_name
o.procedure_run_number = 0
o.output = text
self._output.append(o)
while len(self._output) > 10000:
self._output.pop(0)
self._output_evt.notify_all()
def getf_output(self):
return PyriSandboxOutputGenerator(self)