def indentify_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
_rtls_manager = RTLSManager(nodes, websocket_port=None)
_rtls_manager_subscriber = _rtls_manager.create_subscriber()
_rtls_manager.auto_params = False
_rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
_master_node, _passive_nodes, failed = _rtls_manager.wait_identified()
_all_nodes = [pn for pn in _passive_nodes] ## deep copy
_all_nodes.extend([_master_node])
_all_nodes.extend([f for f in failed])
_rtls_manager.stop()
while not _rtls_manager.stopped:
time.sleep(0.1)
_rtls_manager_subscriber = None
_rtls_manager = None
return _all_nodes
def set_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(
json.dumps(devices_setting)))
nodes = [
RTLSNode(node["com_port"], node["baud_rate"], node["name"])
for node in devices_setting
]
self._rtls_manager = RTLSManager(nodes,
websocket_port=self.websocket_port)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = False
self._start_message_receiver()
self.logger.info("Message receiver started")
self._rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified(
)
if self._master_node is None:
raise RtlsUtilMasterNotFoundException(
"No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException(
"{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.tof_calib_info = None
node.tof_calib_read_return = False
node.tof_initialized = False
node.seed_initialized = False
node.tof_calibration_configured = False
node.tof_calibrated = False
node.tof_started = False
node.ble_connected = False
node.device_resets = False
self.logger.info("Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
class RtlsUtil():
def __init__(self, logging_file, logging_level, websocket_port=None):
self.logger = logging.getLogger()
self.logger.setLevel(logging_level)
self.logger_fh = logging.FileHandler(logging_file)
self.logger_fh.setLevel(logging_level)
# formatter = logging.Formatter('[%(asctime)s] %(filename)-18sln %(lineno)3d %(threadName)-10s %(name)s - %(levelname)8s - %(message)s')
formatter = logging.Formatter('[%(asctime)s] %(name)9s - %(levelname)8s - %(message)s')
self.logger_fh.setFormatter(formatter)
# Messages can be filter by logger name
# blank means all all messages
# filter = logging.Filter()
# self.logger_fh.addFilter(filter)
self.logger.addHandler(self.logger_fh)
self._master_node = None
self._passive_nodes = []
self._all_nodes = []
self._rtls_manager = None
self._rtls_manager_subscriber = None
self._message_receiver_th = None
self._message_receiver_stop = False
self._scan_results = []
self._scan_stopped = threading.Event()
self._scan_stopped.clear()
self._ble_connected = False
self._connected_slave = []
self._master_disconnected = threading.Event()
self._master_disconnected.clear()
self._master_seed = None
self._timeout = 30
self._conn_handle = None
self._slave_attempt_to_connect = None
self._is_cci_started = False
self._is_aoa_started = False
self.aoa_results_queue = queue.Queue()
self.conn_info_queue = queue.Queue()
self.websocket_port = websocket_port
self.on_ble_disconnected_queue = queue.Queue()
def __del__(self):
self.done()
@property
def timeout(self):
return self._timeout
@timeout.setter
def timeout(self, value):
self._timeout = value
def _rtls_wait(self, true_cond_func, nodes, timeout_message):
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while not true_cond_func(nodes) and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(f"Timeout reached while waiting for : {timeout_message}")
def done(self):
if self._message_receiver_th is not None:
self._message_receiver_stop = True
self._message_receiver_th.join()
self._message_receiver_th = None
if self._rtls_manager:
self._rtls_manager.stop()
self._rtls_manager_subscriber = None
self._rtls_manager = None
self.logger_fh.close()
self.logger.removeHandler(self.logger_fh)
def _log_incoming_msg(self, item, identifier):
json_item = json.loads(item.as_json())
json_item["type"] = "Response" if json_item["type"] == "SyncRsp" else "Event"
# Filtering out "originator" and "subsystem" fields
new_dict = {k: v for (k, v) in json_item.items() if k != "originator" if k != "subsystem"}
# Get reference to RTLSNode based on identifier in message
sending_node = self._rtls_manager[identifier]
if sending_node in self._passive_nodes:
self.logger.info(f"PASSIVE : {identifier} --> {new_dict}")
else:
self.logger.info(f"MASTER : {identifier} --> {new_dict}")
def _message_receiver(self):
while not self._message_receiver_stop:
# Get messages from manager
try:
identifier, msg_pri, msg = self._rtls_manager_subscriber.pend(block=True, timeout=0.05).as_tuple()
self._log_incoming_msg(msg, identifier)
sending_node = self._rtls_manager[identifier]
if msg.command == "RTLS_EVT_DEBUG" and msg.type == "AsyncReq":
self.logger.debug(msg.payload)
if msg.command == "RTLS_CMD_SCAN" and msg.type == "AsyncReq":
self._add_scan_result({
'addr': msg.payload.addr,
'addrType': msg.payload.addrType,
'rssi': msg.payload.rssi
})
if msg.command == "RTLS_CMD_SCAN_STOP" and msg.type == "AsyncReq":
self._scan_stopped.set()
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_SUCCESS":
sending_node.connection_in_progress = False
sending_node.connected = True
if sending_node.identifier == self._master_node.identifier:
self._conn_handle = msg.payload.connHandle if 'connHandle' in msg.payload else -1
self._master_disconnected.clear()
if msg.command == "RTLS_CMD_CONN_PARAMS" and msg.type == "AsyncReq":
pass
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_LINK_TERMINATED":
sending_node.connection_in_progress = False
sending_node.connected = False
if sending_node.identifier == self._master_node.identifier:
self._master_disconnected.set()
if 'connHandle' in msg.payload:
for _slave in self._connected_slave[:]:
if _slave['conn_handle'] == msg.payload.connHandle:
self._connected_slave.remove(_slave)
break
# TODO:
# Make disocnnect wait for all required slave to disconnect
# if len(self._connected_slave) == 0:
# self._master_disconnected.set()
# else:
# self._master_disconnected.set()
elif 'connHandle' in msg.payload and sending_node in self._passive_nodes:
slave = self._get_slave_by_conn_handle(msg.payload.connHandle)
# In this point if slave is none it means connection fail while we in connection process
if slave is not None:
self.on_ble_disconnected_queue.put({
'node_identifier': sending_node.identifier,
'slave_addr': slave['addr'],
'isCciStarted': self._is_cci_started,
'isAoaStarted': self._is_aoa_started
})
else:
pass
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if msg.command in ["RTLS_CMD_AOA_RESULT_ANGLE",
"RTLS_CMD_AOA_RESULT_RAW",
"RTLS_CMD_AOA_RESULT_PAIR_ANGLES"] and msg.type == "AsyncReq":
self.aoa_results_queue.put({
"name": sending_node.name,
"type": "aoa",
"identifier": identifier,
"payload": msg.payload
})
if msg.command == 'RTLS_CMD_RESET_DEVICE' and msg.type == 'AsyncReq':
sending_node.device_resets = True
if msg.command == 'RTLS_CMD_CONN_INFO' and msg.type == 'SyncRsp':
sending_node.cci_started = True
if msg.command == 'RTLS_EVT_CONN_INFO' and msg.type == 'AsyncReq':
self.conn_info_queue.put({
"name": sending_node.name,
"type": "conn_info",
"identifier": identifier,
"payload": msg.payload
})
# print(msg.payload)
if msg.command == 'RTLS_CMD_SET_RTLS_PARAM' and msg.payload.rtlsParamType == "RTLS_PARAM_CONNECTION_INTERVAL" and msg.payload.status == "RTLS_SUCCESS":
sending_node.conn_interval_updated = True
if msg.command == 'RTLS_CMD_IDENTIFY' and msg.type == 'SyncRsp':
sending_node.identified = True
sending_node.identifier = msg.payload.identifier
sending_node.capabilities = msg.payload.capabilities
sending_node.devId = msg.payload.devId
sending_node.revNum = msg.payload.revNum
except queue.Empty:
pass
def _start_message_receiver(self):
self._message_receiver_stop = False
self._message_receiver_th = threading.Thread(target=self._message_receiver)
self._message_receiver_th.setDaemon(True)
self._message_receiver_th.start()
def _empty_queue(self, q):
while True:
try:
q.get(timeout=0.5)
except queue.Empty:
break
def _is_passive_in_nodes(self, nodes):
for node in nodes:
if not node.capabilities.get('RTLS_MASTER', False):
return True
return False
## User Function
def add_user_log(self, msg):
print(msg)
self.logger.info(msg)
## Devices API
def indentify_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
_rtls_manager = RTLSManager(nodes, websocket_port=None)
_rtls_manager_subscriber = _rtls_manager.create_subscriber()
_rtls_manager.auto_params = False
_rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
_master_node, _passive_nodes, failed = _rtls_manager.wait_identified()
_all_nodes = [pn for pn in _passive_nodes] ## deep copy
_all_nodes.extend([_master_node])
_all_nodes.extend([f for f in failed])
_rtls_manager.stop()
while not _rtls_manager.stopped:
time.sleep(0.1)
_rtls_manager_subscriber = None
_rtls_manager = None
return _all_nodes
def set_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
self._rtls_manager = RTLSManager(nodes, websocket_port=self.websocket_port)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = True
self._start_message_receiver()
self.logger.info("Message receiver started")
self._rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified()
if self._master_node is None:
raise RtlsUtilMasterNotFoundException("No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException("{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.ble_connected = False
node.device_resets = False
self.logger.info("Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
def get_devices_capability(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.identified = False
node.rtls.identify()
true_cond_func = lambda nodes: all([n.identified for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All device to identified")
ret = []
for node in nodes_to_set:
dev_info = {
"node_mac_address": node.identifier,
"capabilities": node.capabilities
}
ret.append(dev_info)
return ret
######
## Common BLE API
def _get_slave_by_addr(self, addr):
for _scan_result in self._scan_results:
if _scan_result['addr'].lower() == addr.lower():
return _scan_result
return None
def _get_slave_by_conn_handle(self, conn_handle):
for _slave in self._connected_slave:
if _slave['conn_handle'] == conn_handle:
return _slave
return None
def _add_scan_result(self, scan_result):
if self._get_slave_by_addr(scan_result['addr']) is None:
self._scan_results.append(scan_result)
def scan(self, scan_time_sec, expected_slave_bd_addr=None):
self._scan_results = []
timeout = time.time() + scan_time_sec
timeout_reached = time.time() > timeout
while not timeout_reached:
self._scan_stopped.clear()
self._master_node.rtls.scan()
scan_start_time = time.time()
scan_timeout_reached = time.time() > (scan_start_time + 10)
while not self._scan_stopped.isSet() and not scan_timeout_reached:
time.sleep(0.1)
scan_timeout_reached = time.time() > (scan_start_time + 10)
if scan_timeout_reached:
raise RtlsUtilEmbeddedFailToStopScanException("Embedded side didn't finished due to timeout")
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(expected_slave_bd_addr) is not None:
break
timeout_reached = time.time() > timeout
else:
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(expected_slave_bd_addr) is not None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
raise RtlsUtilScanNoResultsException("No device with slave capability found")
return self._scan_results
@property
def ble_connected(self):
return len(self._connected_slave) > 0
def ble_connected_to(self, slave):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException("Input slave not a string and not contains required keys")
for s in self._connected_slave:
if s['addr'] == slave['addr'] and s['conn_handle'] == slave['conn_handle']:
return True
return False
def ble_connect(self, slave, connect_interval_mSec):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException("Input slave not a string and not contains required keys")
interval = int(connect_interval_mSec / 1.25)
self._conn_handle = None
self._slave_attempt_to_connect = slave
self._master_node.connection_in_progress = True
self._master_node.connected = False
self._master_node.rtls.connect(slave['addrType'], slave['addr'], interval)
true_cond_func = lambda master_node: master_node.connection_in_progress == False
self._rtls_wait(true_cond_func, self._master_node, "All node to connect")
if self._master_node.connected:
slave['conn_handle'] = self._conn_handle
self._connected_slave.append(slave)
self._slave_attempt_to_connect = None
self._ble_connected = True
self.logger.info("Connection process done")
return self._conn_handle
return None
def ble_disconnect(self, conn_handle=None, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.terminate_link()
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.terminate_link(slave['conn_handle'])
else:
node.rtls.terminate_link(conn_handle)
true_cond_func = lambda event: event.isSet()
self._rtls_wait(true_cond_func, self._master_disconnected, "Master disconnect")
self._ble_connected = False
self.logger.info("Disconnect process done")
def set_connection_interval(self, connect_interval_mSec, conn_handle=None):
conn_interval = int(connect_interval_mSec / 1.25)
data_len = 2
data_bytes = conn_interval.to_bytes(data_len, byteorder='little')
self._master_node.conn_interval_updated = False
if str(self._master_node.devId) == "DeviceFamily_ID_CC26X0R2":
self._master_node.rtls.set_rtls_param('RTLS_PARAM_CONNECTION_INTERVAL', data_len, data_bytes)
else:
if conn_handle is None:
for s in self._connected_slave:
self._master_node.rtls.set_rtls_param(s['conn_handle'],
'RTLS_PARAM_CONNECTION_INTERVAL',
data_len,
data_bytes)
else:
self._master_node.rtls.set_rtls_param(conn_handle,
'RTLS_PARAM_CONNECTION_INTERVAL',
data_len,
data_bytes)
true_cond_func = lambda nodes: all([n.conn_interval_updated for n in nodes])
self._rtls_wait(true_cond_func, [self._master_node], "Master node set connection interval")
self.logger.info("Connection Interval Updated")
def reset_devices(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.device_resets = False
node.rtls.reset_device()
true_cond_func = lambda nodes: all([n.device_resets for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to reset")
def is_multi_connection_supported(self, nodes):
for node in nodes:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
return False
return True
######
## CCI - Continuous Connection Info
def cci_start(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.cci_started = False
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(True)
else:
if conn_handle is None:
for s in self._connected_slave:
node.rtls.get_conn_info(s['conn_handle'], True)
else:
node.rtls.get_conn_info(conn_handle, True)
true_cond_func = lambda nodes: all([n.cci_started for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node start continues connect info (CCI)")
self._is_cci_started = True
def cci_stop(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(False)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.get_conn_info(slave['conn_handle'], False)
else:
node.rtls.get_conn_info(conn_handle, False)
self._is_cci_started = False
######
## AOA - Angle of Arrival
def is_aoa_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].AOA_TX == True or device_capab['capabilities'].AOA_RX == True):
return False
return True
def _aoa_set_params(self, node, aoa_params, conn_handle):
try:
node.aoa_initialized = False
node_role = 'AOA_MASTER' if node.capabilities.get('RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_set_params(
node_role,
aoa_params['aoa_run_mode'],
aoa_params['aoa_cc2640r2']['aoa_cte_scan_ovs'],
aoa_params['aoa_cc2640r2']['aoa_cte_offset'],
aoa_params['aoa_cc2640r2']['aoa_cte_length'],
aoa_params['aoa_cc2640r2']['aoa_sampling_control']
)
else:
node.rtls.aoa_set_params(
node_role,
aoa_params['aoa_run_mode'],
conn_handle,
aoa_params['aoa_cc26x2']['aoa_slot_durations'],
aoa_params['aoa_cc26x2']['aoa_sample_rate'],
aoa_params['aoa_cc26x2']['aoa_sample_size'],
aoa_params['aoa_cc26x2']['aoa_sampling_control'],
aoa_params['aoa_cc26x2']['aoa_sampling_enable'],
aoa_params['aoa_cc26x2']['aoa_pattern_len'],
aoa_params['aoa_cc26x2']['aoa_ant_pattern']
)
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
def aoa_set_params(self, aoa_params, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.aoa_initialized = False
if conn_handle is None:
for slave in self._connected_slave:
self._aoa_set_params(node, aoa_params, slave['conn_handle'])
else:
self._aoa_set_params(node, aoa_params, conn_handle)
true_cond_func = lambda nodes: all([n.aoa_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to set AOA params")
def _aoa_set_state(self, start, cte_interval=1, cte_length=20, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node_role = 'AOA_MASTER' if node.capabilities.get('RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_start(start)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.aoa_start(slave['conn_handle'], start, cte_interval, cte_length)
else:
node.rtls.aoa_start(conn_handle, start, cte_interval, cte_length)
self._is_aoa_started = start
def aoa_start(self, cte_length, cte_interval, nodes=None, conn_handle=None):
self._aoa_set_state(start=True, cte_length=cte_length, cte_interval=cte_interval, nodes=nodes,
conn_handle=conn_handle)
self.logger.info("AOA Started")
def aoa_stop(self, nodes=None, conn_handle=None):
self._aoa_set_state(start=False, nodes=nodes, conn_handle=conn_handle)
self.logger.info("AOA Stopped")
# Auto detect AoA or ToF support related
tof_supported = False
aoa_supported = False
# If slave addr is None, the script will connect to the first RTLS slave
# that it found. If you wish to connect to a specific device
# (in the case of multiple RTLS slaves) then you may specify the address
# explicitly as given in the comment to the right
slave_addr = '54:6C:0E:A0:4A:CF'
# Initialize manager reference, because on Exception we need to stop the manager to stop all the threads.
manager = None
try:
# Start an RTLSManager instance without WebSocket server enabled
manager = RTLSManager(my_nodes, websocket_port=None)
# Create a subscriber object for RTLSManager messages
subscriber = manager.create_subscriber()
# Tell the manager to automatically distribute connection parameters
manager.auto_params = True
# Start RTLS Node threads, Serial threads, and manager thread
manager.start()
# Wait until nodes have responded to automatic identify command and get reference
# to single master RTLSNode and list of passive RTLSNode instances
master_node, passive_nodes, failed = manager.wait_identified()
if len(failed):
print(
f"ERROR: {len(failed)} nodes could not be identified. Are they programmed?"
)
class RtlsUtil():
def __init__(self, logging_file, logging_level, websocket_port=None):
self.logger = logging.getLogger()
self.logger.setLevel(logging_level)
self.logger_fh = logging.FileHandler(logging_file)
self.logger_fh.setLevel(logging_level)
# formatter = logging.Formatter('[%(asctime)s] %(filename)-18sln %(lineno)3d %(threadName)-10s %(name)s - %(levelname)8s - %(message)s')
formatter = logging.Formatter(
'[%(asctime)s] %(name)s - %(levelname)8s - %(message)s')
self.logger_fh.setFormatter(formatter)
# Messages can be filter by logger name
# blank means all all messages
# filter = logging.Filter()
# self.logger_fh.addFilter(filter)
self.logger.addHandler(self.logger_fh)
self._master_node = None
self._passive_nodes = []
self._all_nodes = []
self._rtls_manager = None
self._rtls_manager_subscriber = None
self._message_receiver_th = None
self._message_receiver_stop = False
self._scan_results = []
self._scan_stopped = threading.Event()
self._scan_stopped.clear()
self._ble_connected = False
self._connected_slave = []
self._master_disconnected = threading.Event()
self._master_disconnected.clear()
self._master_seed = None
self._timeout = 30
self._conn_handle = None
self._slave_attempt_to_connect = None
self._is_cci_started = False
self._is_tof_started = False
self._is_aoa_started = False
self.aoa_results_queue = queue.Queue()
self.tof_results_queue = queue.Queue()
self.conn_info_queue = queue.Queue()
self.websocket_port = websocket_port
self.on_ble_disconnected_queue = queue.Queue()
self._connections_in_process = []
def __del__(self):
self.done()
@property
def timeout(self):
return self._timeout
@timeout.setter
def timeout(self, value):
self._timeout = value
def _rtls_wait(self, true_cond_func, nodes, timeout_message):
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while not true_cond_func(nodes) and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(
f"Timeout reached while waiting for : {timeout_message}")
def done(self):
if self._message_receiver_th is not None:
self._message_receiver_stop = True
self._message_receiver_th.join()
self._message_receiver_th = None
if self._rtls_manager:
self._rtls_manager.stop()
self._rtls_manager_subscriber = None
self._rtls_manager = None
self.logger_fh.close()
self.logger.removeHandler(self.logger_fh)
def _message_receiver(self):
while not self._message_receiver_stop:
# Get messages from manager
try:
identifier, msg_pri, msg = self._rtls_manager_subscriber.pend(
block=True, timeout=0.05).as_tuple()
# Get reference to RTLSNode based on identifier in message
sending_node = self._rtls_manager[identifier]
if sending_node in self._passive_nodes:
self.logger.info(
f"PASSIVE: {identifier} --> {msg.as_json()}")
else:
self.logger.info(
f"MASTER: {identifier} --> {msg.as_json()}")
if msg.command == "RTLS_EVT_DEBUG" and msg.type == "AsyncReq":
self.logger.debug(msg.payload)
if msg.command == "RTLS_CMD_SCAN" and msg.type == "AsyncReq":
self._add_scan_result({
'addr': msg.payload.addr,
'addrType': msg.payload.addrType,
'rssi': msg.payload.rssi
})
if msg.command == "RTLS_CMD_SCAN_STOP" and msg.type == "AsyncReq":
self._scan_stopped.set()
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_SUCCESS":
sending_node.connection_in_progress = False
sending_node.connected = True
if sending_node.identifier == self._master_node.identifier:
self._conn_handle = msg.payload.connHandle if 'connHandle' in msg.payload else -1
for _connection_in_process in self._connections_in_process:
if _connection_in_process[
'slave'] == self._slave_attempt_to_connect:
_connection_in_process[
'conn_handle'] = self._conn_handle
self._master_disconnected.clear()
if msg.command == "RTLS_CMD_CONN_PARAMS" and msg.type == "AsyncReq":
if sending_node.identifier == self._master_node.identifier and \
len(self._connections_in_process) > 0:
for _connection_in_process in self._connections_in_process[:]:
if 'peerAddr' in msg.payload:
if _connection_in_process['slave'][
'addr'] == msg.payload.peerAddr:
for passive in _connection_in_process[
'passives']:
# print(f"Passive {passive} set conn params for handle {msg.payload.connHandle}")
passive.rtls.set_ble_conn_info(
**msg.payload)
else:
for passive in _connection_in_process[
'passives']:
passive.rtls.set_ble_conn_info(
**msg.payload)
## next command can be doen beacuse [:] in for , [:] means copy of array
self._connections_in_process.remove(
_connection_in_process)
break
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_LINK_TERMINATED":
sending_node.connection_in_progress = False
sending_node.connected = False
if sending_node.identifier == self._master_node.identifier:
self._master_disconnected.set()
if 'connHandle' in msg.payload:
for _slave in self._connected_slave[:]:
if _slave[
'conn_handle'] == msg.payload.connHandle:
self._connected_slave.remove(_slave)
break
# TODO:
# Make disocnnect wait for all required slave to disconnect
# if len(self._connected_slave) == 0:
# self._master_disconnected.set()
# else:
# self._master_disconnected.set()
elif 'connHandle' in msg.payload and sending_node in self._passive_nodes:
slave = self._get_slave_by_conn_handle(
msg.payload.connHandle)
# In this point if slave is none it means connection fail while we in connection process
if slave is not None:
self.on_ble_disconnected_queue.put({
'node_identifier':
sending_node.identifier,
'slave_addr':
slave['addr'],
'isCciStarted':
self._is_cci_started,
'isTofStarted':
self._is_tof_started,
'isAoaStarted':
self._is_aoa_started
})
else:
pass
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if msg.command in [
"RTLS_CMD_AOA_RESULT_ANGLE", "RTLS_CMD_AOA_RESULT_RAW",
"RTLS_CMD_AOA_RESULT_PAIR_ANGLES"
] and msg.type == "AsyncReq":
self.aoa_results_queue.put({
"name": sending_node.name,
"type": "aoa",
"identifier": identifier,
"payload": msg.payload
})
if msg.command == 'RTLS_CMD_TOF_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.tof_initialized = True
if msg.command == 'RTLS_CMD_TOF_GET_SEC_SEED' and msg.payload.seed is not None:
self._master_seed = msg.payload.seed
if msg.command == 'RTLS_CMD_TOF_SET_SEC_SEED' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.seed_initialized = True
if msg.command == 'RTLS_CMD_TOF_ENABLE' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.tof_started = True
if msg.command in [
"RTLS_CMD_TOF_RESULT_DIST", "RTLS_CMD_TOF_RESULT_STAT",
"RTLS_CMD_TOF_RESULT_RAW"
] and msg.type == "AsyncReq":
self.tof_results_queue.put({
"name": sending_node.name,
"type": "tof",
"identifier": identifier,
"payload": msg.payload
})
if msg.command == 'RTLS_CMD_TOF_CALIBRATE' and msg.type == 'SyncRsp':
sending_node.tof_calibration_configured = True
if msg.command == 'RTLS_CMD_TOF_CALIBRATE' and msg.type == 'AsyncReq':
sending_node.tof_calibrated = True
if msg.command == 'RTLS_CMD_RESET_DEVICE' and msg.type == 'AsyncReq':
sending_node.device_resets = True
if msg.command == 'RTLS_CMD_TOF_CALIB_NV_READ' and msg.type == 'AsyncReq':
sending_node.tof_calib_info = msg.payload
sending_node.tof_calib_read_return = True
if msg.command == 'RTLS_CMD_CONN_INFO' and msg.type == 'SyncRsp':
sending_node.cci_started = True
if msg.command == 'RTLS_EVT_CONN_INFO' and msg.type == 'AsyncReq':
self.conn_info_queue.put({
"name": sending_node.name,
"type": "conn_info",
"identifier": identifier,
"payload": msg.payload
})
if msg.command == 'RTLS_CMD_SET_RTLS_PARAM' and msg.payload.rtlsParamType == "RTLS_PARAM_CONNECTION_INTERVAL" and msg.payload.status == "RTLS_SUCCESS":
sending_node.conn_interval_updated = True
if msg.command == 'RTLS_CMD_TOF_SWITCH_ROLE' and msg.payload.status == "RTLS_SUCCESS":
sending_node.role_switched = True
if msg.command == 'RTLS_CMD_IDENTIFY' and msg.type == 'SyncRsp':
sending_node.identified = True
sending_node.identifier = msg.payload.identifier
sending_node.capabilities = msg.payload.capabilities
sending_node.devId = msg.payload.devId
sending_node.revNum = msg.payload.revNum
except queue.Empty:
pass
def _start_message_receiver(self):
self._message_receiver_stop = False
self._message_receiver_th = threading.Thread(
target=self._message_receiver)
self._message_receiver_th.setDaemon(True)
self._message_receiver_th.start()
def _empty_queue(self, q):
while True:
try:
q.get(timeout=0.5)
except queue.Empty:
break
def _is_passive_in_nodes(self, nodes):
for node in nodes:
if not node.capabilities.get('RTLS_MASTER', False):
return True
return False
## User Function
def add_user_log(self, msg):
print(msg)
self.logger.info(msg)
## Devices API
def indentify_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(
json.dumps(devices_setting)))
nodes = [
RTLSNode(node["com_port"], node["baud_rate"], node["name"])
for node in devices_setting
]
_rtls_manager = RTLSManager(nodes, websocket_port=None)
_rtls_manager_subscriber = _rtls_manager.create_subscriber()
_rtls_manager.auto_params = False
_rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
_master_node, _passive_nodes, failed = _rtls_manager.wait_identified()
_all_nodes = [pn for pn in _passive_nodes] ## deep copy
_all_nodes.extend([_master_node])
_all_nodes.extend([f for f in failed])
_rtls_manager.stop()
while not _rtls_manager.stopped:
time.sleep(0.1)
_rtls_manager_subscriber = None
_rtls_manager = None
return _all_nodes
def set_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(
json.dumps(devices_setting)))
nodes = [
RTLSNode(node["com_port"], node["baud_rate"], node["name"])
for node in devices_setting
]
self._rtls_manager = RTLSManager(nodes,
websocket_port=self.websocket_port)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = False
self._start_message_receiver()
self.logger.info("Message receiver started")
self._rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified(
)
if self._master_node is None:
raise RtlsUtilMasterNotFoundException(
"No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException(
"{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.tof_calib_info = None
node.tof_calib_read_return = False
node.tof_initialized = False
node.seed_initialized = False
node.tof_calibration_configured = False
node.tof_calibrated = False
node.tof_started = False
node.ble_connected = False
node.device_resets = False
self.logger.info("Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
def get_devices_capability(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.identified = False
node.rtls.identify()
true_cond_func = lambda nodes: all([n.identified for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All device to identified")
ret = []
for node in nodes_to_set:
dev_info = {
"node_mac_address": node.identifier,
"capabilities": node.capabilities
}
ret.append(dev_info)
return ret
######
## Common BLE API
def _get_slave_by_addr(self, addr):
for _scan_result in self._scan_results:
if _scan_result['addr'].lower() == addr.lower():
return _scan_result
return None
def _get_slave_by_conn_handle(self, conn_handle):
for _slave in self._connected_slave:
if _slave['conn_handle'] == conn_handle:
return _slave
return None
def _add_scan_result(self, scan_result):
if self._get_slave_by_addr(scan_result['addr']) is None:
self._scan_results.append(scan_result)
def scan(self, scan_time_sec, expected_slave_bd_addr=None):
self._scan_results = []
timeout = time.time() + scan_time_sec
timeout_reached = time.time() > timeout
while not timeout_reached:
self._scan_stopped.clear()
self._master_node.rtls.scan()
while not self._scan_stopped.isSet():
time.sleep(0.1)
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(
expected_slave_bd_addr) is not None:
break
timeout_reached = time.time() > timeout
else:
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(
expected_slave_bd_addr) is not None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
raise RtlsUtilScanNoResultsException(
"No device with slave capability found")
return self._scan_results
@property
def ble_connected(self):
return len(self._connected_slave) > 0
def ble_connected_to(self, slave):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException(
"Input slave not a string and not contains required keys")
for s in self._connected_slave:
if s['addr'] == slave['addr'] and s['conn_handle'] == slave[
'conn_handle']:
return True
return False
def ble_connect(self, slave, connect_interval_mSec):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException(
"Input slave not a string and not contains required keys")
interval = int(connect_interval_mSec / 1.25)
self._conn_handle = None
self._slave_attempt_to_connect = slave
self._master_node.connection_in_progress = True
self._master_node.connected = False
self._connections_in_process.append({
'slave': slave,
'conn_handle': None,
'passives': self._passive_nodes
})
self._master_node.rtls.connect(slave['addrType'], slave['addr'],
interval)
true_cond_func = lambda master_node: master_node.connection_in_progress == False
self._rtls_wait(true_cond_func, self._master_node,
"All node to connect")
if self._master_node.connected:
slave['conn_handle'] = self._conn_handle
self._connected_slave.append(slave)
self._slave_attempt_to_connect = None
self._ble_connected = True
self.logger.info("Connection process done")
return self._conn_handle
return None
def ble_disconnect(self, conn_handle=None, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.terminate_link()
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.terminate_link(slave['conn_handle'])
else:
node.rtls.terminate_link(conn_handle)
true_cond_func = lambda event: event.isSet()
self._rtls_wait(true_cond_func, self._master_disconnected,
"Master disconnect")
self._ble_connected = False
self.logger.info("Disconnect process done")
def passive_reconnect(self, passive_node, slave):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException(
"Input slave not a string and not contains required keys")
# SKIP if device is CC26X0R2
# TODO : Handle passive recoonect for CC26X0R2
if str(passive_node.devId) == "DeviceFamily_ID_CC26X0R2":
return
self._connections_in_process.append({
'slave': slave,
'conn_handle': slave['conn_handle'],
'passives': [passive_node]
})
passive_node.connection_in_progress = True
passive_node.connected = False
self._master_node.rtls.get_active_conn_info(slave['conn_handle'])
true_cond_func = lambda node: node.connection_in_progress == False
self._rtls_wait(true_cond_func, passive_node,
"Passive node fail to reconnect")
return slave['conn_handle'] if passive_node.connected else None
def set_connection_interval(self, connect_interval_mSec, conn_handle=None):
conn_interval = int(connect_interval_mSec / 1.25)
data_len = 2
data_bytes = conn_interval.to_bytes(data_len, byteorder='little')
self._master_node.conn_interval_updated = False
if str(self._master_node.devId) == "DeviceFamily_ID_CC26X0R2":
self._connections_in_process.append({
'slave': None,
'conn_handle': None,
'passives': self._passive_nodes
})
self._master_node.rtls.set_rtls_param(
'RTLS_PARAM_CONNECTION_INTERVAL', data_len, data_bytes)
else:
if conn_handle is None:
for s in self._connected_slave:
self._connections_in_process.append({
'slave':
s,
'conn_handle':
s['conn_handle'],
'passives':
self._passive_nodes
})
self._master_node.rtls.set_rtls_param(
s['conn_handle'], 'RTLS_PARAM_CONNECTION_INTERVAL',
data_len, data_bytes)
else:
self._connections_in_process.append({
'slave':
self._get_slave_by_conn_handle(conn_handle),
'conn_handle':
conn_handle,
'passives':
self._passive_nodes
})
self._master_node.rtls.set_rtls_param(
conn_handle, 'RTLS_PARAM_CONNECTION_INTERVAL', data_len,
data_bytes)
true_cond_func = lambda nodes: all(
[n.conn_interval_updated for n in nodes])
self._rtls_wait(true_cond_func, [self._master_node],
"Master node set connection interval")
self.logger.info("Connection Interval Updated")
def reset_devices(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.device_resets = False
node.rtls.reset_device()
true_cond_func = lambda nodes: all([n.device_resets for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to reset")
######
## CCI - Continuous Connection Info
def cci_start(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.cci_started = False
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(True)
else:
if conn_handle is None:
for s in self._connected_slave:
node.rtls.get_conn_info(s['conn_handle'], True)
else:
node.rtls.get_conn_info(conn_handle, True)
true_cond_func = lambda nodes: all([n.cci_started for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All node start continues connect info (CCI)")
self._is_cci_started = True
def cci_stop(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(False)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.get_conn_info(slave['conn_handle'], False)
else:
node.node.rtls.get_conn_info(conn_handle, False)
self._is_cci_started = False
######
## AOA - Angle of Arrival
def is_aoa_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].AOA_TX == True
or device_capab['capabilities'].AOA_RX == True):
return False
return True
def _aoa_set_params(self, node, aoa_params, conn_handle):
try:
node.aoa_initialized = False
node_role = 'AOA_MASTER' if node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_set_params(
node_role, aoa_params['aoa_run_mode'],
aoa_params['aoa_cc2640r2']['aoa_cte_scan_ovs'],
aoa_params['aoa_cc2640r2']['aoa_cte_offset'],
aoa_params['aoa_cc2640r2']['aoa_cte_length'],
aoa_params['aoa_cc2640r2']['aoa_sampling_control'])
else:
node.rtls.aoa_set_params(
node_role, aoa_params['aoa_run_mode'], conn_handle,
aoa_params['aoa_cc26x2']['aoa_slot_durations'],
aoa_params['aoa_cc26x2']['aoa_sample_rate'],
aoa_params['aoa_cc26x2']['aoa_sample_size'],
aoa_params['aoa_cc26x2']['aoa_sampling_control'],
aoa_params['aoa_cc26x2']['aoa_sampling_enable'],
aoa_params['aoa_cc26x2']['aoa_pattern_len'],
aoa_params['aoa_cc26x2']['aoa_ant_pattern'])
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
def aoa_set_params(self, aoa_params, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.aoa_initialized = False
if conn_handle is None:
for slave in self._connected_slave:
self._aoa_set_params(node, aoa_params,
slave['conn_handle'])
else:
self._aoa_set_params(node, aoa_params, conn_handle)
true_cond_func = lambda nodes: all([n.aoa_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All node to set AOA params")
def _aoa_set_state(self,
start,
cte_interval=1,
cte_length=20,
nodes=None,
conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node_role = 'AOA_MASTER' if node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_start(start)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.aoa_start(slave['conn_handle'], start,
cte_interval, cte_length)
else:
node.rtls.aoa_start(conn_handle, start, cte_interval,
cte_length)
self._is_aoa_started = start
def aoa_start(self,
cte_length,
cte_interval,
nodes=None,
conn_handle=None):
self._aoa_set_state(start=True,
cte_length=cte_length,
cte_interval=cte_interval,
nodes=nodes,
conn_handle=conn_handle)
self.logger.info("AOA Started")
def aoa_stop(self, nodes=None, conn_handle=None):
self._aoa_set_state(start=False, nodes=nodes, conn_handle=conn_handle)
self.logger.info("AOA Stopped")
######
## TOF - Time of Flight
def is_tof_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].TOF_MASTER == True
or device_capab['capabilities'].TOF_PASSIVE == True):
return False
return True
def _tof_get_master_seed(self):
self._master_seed = None
self._master_node.rtls.tof_get_sec_seed()
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while self._master_seed is None and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(
f"Timeout reached while waiting for : Master SEED")
def tof_set_params(self, tof_params, seed_exchange=True, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
try:
for node in nodes_to_set:
node.tof_initialized = False
node_role = 'TOF_MASTER' if node.capabilities.get(
'TOF_MASTER', False) else 'TOF_PASSIVE'
node.rtls.tof_set_params(
node_role, tof_params['tof_samples_per_burst'],
len(tof_params['tof_freq_list']),
tof_params['tof_slave_lqi_filter'],
tof_params['tof_post_process_lqi_thresh'],
tof_params['tof_post_process_magn_ratio'],
tof_params['tof_auto_rssi'], tof_params['tof_sample_mode'],
tof_params['tof_run_mode'], tof_params['tof_freq_list'])
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
true_cond_func = lambda nodes: all([n.tof_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All nodes set TOF params")
if seed_exchange:
self._tof_get_master_seed()
for node in nodes_to_set:
## Set seed only to passive nodes
if not node.capabilities.get('TOF_MASTER', False):
node.seed_initialized = False
node.rtls.tof_set_sec_seed(self._master_seed)
true_cond_func = lambda nodes: all([
n.seed_initialized for n in nodes
if not node.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Passive nodes set SEED")
def _tof_calib_set_passive_node(self, nodes, samples_per_freq, distance,
use_nv_calib):
for node in nodes:
if not node.capabilities.get('TOF_MASTER', False):
node.tof_calibration_configured = False
node.tof_calibrated = False
node.rtls.tof_calib(True, samples_per_freq, distance,
use_nv_calib)
true_cond_func = lambda nodes: all([
n.tof_calibration_configured for n in nodes
if not n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes,
"Passive nodes to set TOF calibration params")
def _tof_calib_set_master_node(self, nodes, samples_per_freq, distance,
use_nv_calib):
for node in nodes:
if node.capabilities.get('TOF_MASTER', False):
node.tof_calibration_configured = False
node.tof_calibrated = False
node.rtls.tof_calib(True, samples_per_freq, distance,
use_nv_calib)
true_cond_func = lambda nodes: all([
n.tof_calibration_configured for n in nodes
if n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes,
"Master node to set TOF calibration params")
def _tof_start_in_passive_node(self, nodes):
for node in nodes:
if not node.capabilities.get('TOF_MASTER', False):
node.tof_started = False
node.rtls.tof_start(True)
true_cond_func = lambda nodes: all([
n.tof_started for n in nodes
if not node.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes, "Passive nodes to start TOF")
def _tof_start_in_master_node(self, nodes):
for node in nodes:
if node.capabilities.get('TOF_MASTER', False):
node.tof_started = False
node.rtls.tof_start(True)
true_cond_func = lambda nodes: all([n.tof_started for n in nodes])
self._rtls_wait(true_cond_func, nodes, "All nodes to start TOF")
def tof_start(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
self._tof_start_in_passive_node(nodes_to_set)
self._tof_start_in_master_node(nodes_to_set)
self._is_tof_started = True
self.logger.info("TOF Started")
def tof_stop(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.tof_started = False
node.rtls.tof_start(False)
self._is_tof_started = False
self.logger.info("TOF Stopped")
def tof_calibrate(self,
samples_per_freq,
distance,
use_nv_calib=False,
nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
if self._is_passive_in_nodes(nodes_to_set):
self._tof_calib_set_passive_node(nodes_to_set, samples_per_freq,
distance, use_nv_calib)
self.logger.info('Calibration started in all passive nodes')
self._tof_start_in_passive_node(nodes_to_set)
self.logger.info('ToF started in all passive nodes')
self._tof_calib_set_master_node(nodes_to_set, 0, distance,
use_nv_calib)
self.logger.info('Calibration started in master node')
self._tof_start_in_master_node(nodes_to_set)
self.logger.info('ToF started in master node')
true_cond_func = lambda nodes: all([
n.tof_calibrated for n in nodes
if not n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Passive nodes to end TOF calibration")
for node in nodes_to_set:
if node.capabilities.get('TOF_MASTER', False):
node.rtls.tof_calib(False, 0, distance, False)
else:
self._tof_calib_set_master_node(nodes_to_set, samples_per_freq,
distance, use_nv_calib)
self._tof_start_in_master_node(nodes_to_set)
true_cond_func = lambda nodes: all([
n.tof_calibrated for n in nodes
if n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Master nodes to end TOF calibration")
for node in nodes_to_set:
node.tof_started = False
node.rtls.tof_start(False)
self._empty_queue(self.tof_results_queue)
self.logger.info("Calibration Done")
def tof_get_calib_info(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.tof_calib_info = None
node.tof_calib_read_return = False
node.rtls.read_calib_from_NV()
true_cond_func = lambda nodes: all(
[n.tof_calib_read_return for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All nodes return calibration info from NV")
calib_info = []
for node in nodes_to_set:
info = {
"node_mac_address": node.identifier,
"calibration_info": node.tof_calib_info
}
calib_info.append(info)
return calib_info
def tof_role_switch(self, tof_master_node, tof_passive_node):
if not tof_master_node.capabilities.get('TOF_MASTER', False):
raise RtlsUtilException(
"Given TOF Master Node not active as TOF Master Node")
if not tof_passive_node.capabilities.get('TOF_PASSIVE', False):
raise RtlsUtilException(
"Given TOF Passive Node not active as TOF Passive Node")
tof_master_node.role_switched = False
tof_master_node.rtls.tof_switch_role("TOF_PASSIVE")
tof_passive_node.role_switched = False
tof_passive_node.rtls.tof_switch_role("TOF_MASTER")
true_cond_func = lambda nodes: all([n.role_switched for n in nodes])
self._rtls_wait(true_cond_func, [tof_master_node, tof_passive_node],
"Role Switch")
class RtlsUtil():
def __init__(self, logging_file, logging_level, websocket_port=None):
self.logger = logging.getLogger()
self.logger.setLevel(logging_level)
self.logger_fh = logging.FileHandler(logging_file)
self.logger_fh.setLevel(logging_level)
# formatter = logging.Formatter('[%(asctime)s] %(filename)-18sln %(lineno)3d %(threadName)-10s %(name)s - %(levelname)8s - %(message)s')
formatter = logging.Formatter('[%(asctime)s] %(name)9s - %(levelname)8s - %(message)s')
self.logger_fh.setFormatter(formatter)
# Messages can be filter by logger name
# blank means all all messages
# filter = logging.Filter()
# self.logger_fh.addFilter(filter)
self.logger.addHandler(self.logger_fh)
self._master_node = None
self._passive_nodes = []
self._all_nodes = []
self._rtls_manager = None
self._rtls_manager_subscriber = None
self._message_receiver_th = None
self._message_receiver_stop = False
self._scan_results = []
self._scan_stopped = threading.Event()
self._scan_stopped.clear()
self._ble_connected = False
self._connected_slave = []
self._master_disconnected = threading.Event()
self._master_disconnected.clear()
self._master_seed = None
self._timeout = 30
self._conn_handle = None
self._slave_attempt_to_connect = None
self._is_cci_started = False
self._is_aoa_started = False
self.aoa_results_queue = queue.Queue()
self.conn_info_queue = queue.Queue()
self.websocket_port = websocket_port
self.on_ble_disconnected_queue = queue.Queue()
def __del__(self):
self.done()
@property
def timeout(self):
return self._timeout
@timeout.setter
def timeout(self, value):
self._timeout = value
def _rtls_wait(self, true_cond_func, nodes, timeout_message):
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while not true_cond_func(nodes) and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(f"Timeout reached while waiting for : {timeout_message}")
def done(self):
if self._message_receiver_th is not None:
self._message_receiver_stop = True
self._message_receiver_th.join()
self._message_receiver_th = None
if self._rtls_manager:
self._rtls_manager.stop()
self._rtls_manager_subscriber = None
self._rtls_manager = None
self.logger_fh.close()
self.logger.removeHandler(self.logger_fh)
def _log_incoming_msg(self, item, identifier):
json_item = json.loads(item.as_json())
json_item["type"] = "Response" if json_item["type"] == "SyncRsp" else "Event"
# Filtering out "originator" and "subsystem" fields
new_dict = {k: v for (k, v) in json_item.items() if k != "originator" if k != "subsystem"}
# Get reference to RTLSNode based on identifier in message
sending_node = self._rtls_manager[identifier]
if sending_node in self._passive_nodes:
self.logger.info(f"PASSIVE : {identifier} --> {new_dict}")
else:
self.logger.info(f"MASTER : {identifier} --> {new_dict}")
def _message_receiver(self):
while not self._message_receiver_stop:
# Get messages from manager
try:
identifier, msg_pri, msg = self._rtls_manager_subscriber.pend(block=True, timeout=0.05).as_tuple()
self._log_incoming_msg(msg, identifier)
sending_node = self._rtls_manager[identifier]
if msg.command == "RTLS_EVT_DEBUG" and msg.type == "AsyncReq":
self.logger.debug(msg.payload)
if msg.command == "RTLS_CMD_SCAN" and msg.type == "AsyncReq":
self._add_scan_result({
'addr': msg.payload.addr,
'addrType': msg.payload.addrType,
'rssi': msg.payload.rssi
})
if msg.command == "RTLS_CMD_SCAN_STOP" and msg.type == "AsyncReq":
self._scan_stopped.set()
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_SUCCESS":
sending_node.connection_in_progress = False
sending_node.connected = True
if sending_node.identifier == self._master_node.identifier:
self._conn_handle = msg.payload.connHandle if 'connHandle' in msg.payload else -1
self._master_disconnected.clear()
if msg.command == "RTLS_CMD_CONN_PARAMS" and msg.type == "AsyncReq":
pass
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_LINK_TERMINATED":
sending_node.connection_in_progress = False
sending_node.connected = False
if sending_node.identifier == self._master_node.identifier:
self._master_disconnected.set()
if 'connHandle' in msg.payload:
for _slave in self._connected_slave[:]:
if _slave['conn_handle'] == msg.payload.connHandle:
self._connected_slave.remove(_slave)
break
# TODO:
# Make disocnnect wait for all required slave to disconnect
# if len(self._connected_slave) == 0:
# self._master_disconnected.set()
# else:
# self._master_disconnected.set()
elif 'connHandle' in msg.payload and sending_node in self._passive_nodes:
slave = self._get_slave_by_conn_handle(msg.payload.connHandle)
# In this point if slave is none it means connection fail while we in connection process
if slave is not None:
self.on_ble_disconnected_queue.put({
'node_identifier': sending_node.identifier,
'slave_addr': slave['addr'],
'isCciStarted': self._is_cci_started,
'isAoaStarted': self._is_aoa_started
})
else:
pass
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if msg.command in ["RTLS_CMD_AOA_RESULT_ANGLE",
"RTLS_CMD_AOA_RESULT_RAW",
"RTLS_CMD_AOA_RESULT_PAIR_ANGLES"] and msg.type == "AsyncReq":
self.aoa_results_queue.put({
"name": sending_node.name,
"type": "aoa",
"identifier": identifier,
"payload": msg.payload
})
print(msg.payload)
savedangle[msg.payload.connHandle].append(msg.payload.angle)
savedangle[msg.payload.connHandle+1].append(msg.payload.angle+80)
savedangle[msg.payload.connHandle+2].append(msg.payload.angle+140)
if(len(savedangle[0])>0 and len(savedangle[1])>0 and len(savedangle[2])>0):
def dis(a, b):
""" generated source for method dis """
return sqrt(dis_s(a, b))
def dis_s(a, b):
""" generated source for method dis_s """
return ((a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]))
def lineCross(a, b, c, d):
""" generated source for method lineCross """
if not (min(a[0], b[0]) <= max(c[0], d[0]) and min(c[1], d[1]) <= max(a[1], b[1]) and min(c[0], d[0]) <= max(a[0], b[0]) and min(a[1], b[1]) <= max(c[1], d[1])):
return False
u = int()
v = int()
w = int()
z = int()
u = (c[0] - a[0]) * (b[1] - a[1]) - (b[0] - a[0]) * (c[1] - a[1])
v = (d[0] - a[0]) * (b[1] - a[1]) - (b[0] - a[0]) * (d[1] - a[1])
w = (a[0] - c[0]) * (d[1] - c[1]) - (d[0] - c[0]) * (a[1] - c[1])
z = (b[0] - c[0]) * (d[1] - c[1]) - (d[0] - c[0]) * (b[1] - c[1])
return (u * v <= 0.00000001 and w * z <= 0.00000001)
NaN = 11111
def isnan(a):
return a==NaN
""" generated source for method main """
corrdinatelist = []
location7 = [3.0, 0.0]
corrdinatelist.append(location7)
location5 = [0.0, 2.0]
corrdinatelist.append(location5)
location4 = [-3.0, 0.0]
corrdinatelist.append(location4)
direction = []
direction.append(-1)
direction.append(-1)
direction.append(-1)
anglelist = []
anglelist.append(sum(savedangle[1])/len(savedangle[1])-sum(savedangle[0])/len(savedangle[0]))
anglelist.append(sum(savedangle[2])/len(savedangle[2])-sum(savedangle[1])/len(savedangle[1]))
#anglelist.append(90.54159)
#anglelist.append(71.08507749999999)
truth = [0.0, 0.0]
firstangle = 0.0
for point in corrdinatelist:
diffx = point[0] - truth[0] + 1e-10
diffy = point[1] - truth[1]
angle = degrees(atan(diffy / diffx))
if diffx < 0:
angle += 180
if diffy < 0 and diffx > 0:
angle += 360
if firstangle != 0.0:
diffangle = -angle + firstangle
if diffangle < 0.0:
diffangle += 360.0
firstangle = angle
smin = 0.0
smax = 0.0
for i in corrdinatelist:
for j in i:
smin = min(smin, j)
smax = max(smax, j)
scale = smax - smin
totangle = 0.0
for angle in anglelist:
totangle += angle
anglelist.append(360 - totangle)
numOfPois = len(anglelist)
i = 0
while i < numOfPois:
if anglelist[i] > 180.0:
anglelist[i]=(360 - anglelist[i])
direction[i]=(-1 * (direction[i]))
i += 1
circles = []
results = []
poi1 = 0
while poi1 < numOfPois:
nextPoi = (poi1 + 1) % numOfPois
midPointX = (corrdinatelist[poi1][0] + corrdinatelist[nextPoi][0]) / 2
midPointY = (corrdinatelist[poi1][1] + corrdinatelist[nextPoi][1]) / 2
tangent = abs(tan(radians(anglelist[poi1])))
toCircleX = -(corrdinatelist[nextPoi][1] - corrdinatelist[poi1][1]) / 2 / tangent
toCircleY = (corrdinatelist[nextPoi][0] - corrdinatelist[poi1][0]) / 2 / tangent
circle = [[0,0],[0,0],[0,0]]
circle[0][0] = midPointX + toCircleX
circle[0][1] = midPointY + toCircleY
circle[1][0] = midPointX - toCircleX
circle[1][1] = midPointY - toCircleY
midPoint = [midPointX, midPointY]
check = 0
while check < 2:
x = circle[check][0]
y = circle[check][1]
x1 = corrdinatelist[poi1][0]
y1 = corrdinatelist[poi1][1]
x2 = corrdinatelist[nextPoi][0]
y2 = corrdinatelist[nextPoi][1]
ans = (y - y1) * (x2 - x1) - (x - x1) * (y2 - y1)
if (ans * direction[poi1] > 0.0) ^ (anglelist[poi1] < 90.0):
if check == 0:
circle[0][0] = circle[1][0]
circle[0][1] = circle[1][1]
check += 1
circle[2][0] = dis(midPoint, corrdinatelist[poi1]) / sin(radians(anglelist[poi1]))
circles.append(circle)
poi1 += 1
poi2 = 0
while poi2 < numOfPois:
nextPoi = (poi2 + 1) % numOfPois
result = [[[[0, 0], [0, 0]], [[0, 0], [0, 0]]], [[[0, 0], [0, 0]], [[0, 0], [0, 0]]]]
poinum = 0
while poinum < 1:
nextnum = 0
while nextnum < 1:
a1 = circles[poi2][poinum][0]
b1 = circles[poi2][poinum][1]
r1 = circles[poi2][2][0]
a2 = circles[nextPoi][nextnum][0]
b2 = circles[nextPoi][nextnum][1]
r2 = circles[nextPoi][2][0]
#print(a1,b1,r1,a2,b2,r2)
if isnan(a1) or isnan(a2):
nextnum += 1
continue
A = -2 * (a1 - a2)
B = -2 * (b1 - b2)
C = a1 * a1 - a2 * a2 + b1 * b1 - b2 * b2 - r1 * r1 + r2 * r2
AA = 1 + A * A / B / B
BB = -2 * a1 + 2 * A / B * (C / B + b1)
CC = a1 * a1 + pow(C / B + b1, 2) - r1 * r1
delta = BB * BB - 4 * AA * CC
if delta < 0:
x = (-A * C - b1 * A * B + a1 * B * B) / (A * A + B * B)
y = -(A * x + C) / B
result[poinum][nextnum][0][0] = x
result[poinum][nextnum][0][1] = y
result[poinum][nextnum][1][0] = NaN
results.append(result)
else:
sqrtdelta = sqrt(delta)
x = (-BB + sqrtdelta) / 2 / AA
y = -(A * x + C) / B
result[poinum][nextnum][0][0] = x
result[poinum][nextnum][0][1] = y
x = (-BB - sqrtdelta) / 2 / AA
y = -(A * x + C) / B
result[poinum][nextnum][1][0] = x
result[poinum][nextnum][1][1] = y
check = 0
while check < 2:
if dis_s(result[poinum][nextnum][check], corrdinatelist[nextPoi]) < 1E-10:
#print("delete for wrongdir:0 ",result[poinum][nextnum][check])
result[poinum][nextnum][check][0] = NaN
check += 1
continue
circle = circles[poi2][poinum]
line1 = corrdinatelist[poi2]
line2 = corrdinatelist[nextPoi]
angle = anglelist[poi2]
ans = lineCross(circle, result[poinum][nextnum][check], line1, line2)
if ans == True and angle < 70 or ans == False and angle > 110:
#print("delete for wrongdir: ")# + poi2 + poinum + nextnum + check + " " + Arrays.toString(result[poinum][nextnum][check]))
result[poinum][nextnum][check][0] = NaN
check += 1
continue
morePoi = (nextPoi + 1) % numOfPois
circle = circles[nextPoi][nextnum]
line1 = corrdinatelist[nextPoi]
line2 = corrdinatelist[morePoi]
angle = anglelist[nextPoi]
ans = lineCross(circle, result[poinum][nextnum][check], line1, line2)
if ans == True and angle < 70 or ans == False and angle > 110:
#print("delete for wrongdir: ")# + poi2 + poinum + nextnum + check + " " + Arrays.toString(result[poinum][nextnum][check]))
result[poinum][nextnum][check][0] = NaN
check += 1
continue
check += 1
nextnum += 1
poinum += 1
results.append(result)
poi2 += 1
#print(results)
crossnum = [None] * numOfPois
poi3 = 0
while poi3 < numOfPois:
crossnum[poi3] = 0
poinum = 0
while poinum < 1:
nextnum = 0
while nextnum < 1:
check = 0
while check < 2:
if isnan(results[poi3][poinum][nextnum][check][0]) == False:
# print("point: " , poi3 , poinum , nextnum,results[poi3][poinum][nextnum])
crossnum[poi3] += 1
check += 1
nextnum += 1
poinum += 1
poi3 += 1
#print('crossnum',crossnum)
minn = 10000000
bestmidPointX = 0
bestmidPointY = 0
maxx = 0
midPointX = 0
midPointY = 0
pointer = 0
while pointer < 1:
maxx = 0
midPointX = 0
midPointY = 0
poi4 = 0
while poi4 < numOfPois:
nextPoi = (poi4 + 1) % numOfPois
if crossnum[poi4] == 0:
poi4 += 1
continue
if crossnum[nextPoi] == 0:
poi4 += 1
continue
points = results[poi4][(pointer >> (poi4 * 2)) & 1][(pointer >> (poi4 * 2 + 1)) & 1]
crosspoint = []
if isnan(points[0][0]):
if isnan(points[1][0]):
maxx = 0
break
else:
crosspoint = points[1]
else:
crosspoint = points[0]
points = results[nextPoi][(pointer >> (2 * nextPoi)) & 1][(pointer >> (2 * nextPoi + 1)) & 1]
nextpoint = []
if isnan(points[0][0]):
if isnan(points[1][0]):
maxx = 0
break
else:
nextpoint = points[1]
else:
nextpoint = points[0]
maxx = max(maxx, dis_s(crosspoint, nextpoint))
midPointX += crosspoint[0]
midPointY += crosspoint[1]
poi4 += 1
if maxx != 0 and maxx < minn:
minn = maxx
bestmidPointX = midPointX
bestmidPointY = midPointY
#print("res: " , minn , midPointX , midPointY)
if maxx != 0 and maxx == minn:
midPoint = [midPointX / numOfPois, midPointY / numOfPois]
#print("possible: dist:" , maxx , " pos: " ,midPoint)
poi = 0
while poi < numOfPois:
nextPoi = (poi + 1) % numOfPois
if crossnum[poi] == 0:
poi += 1
continue
if crossnum[nextPoi] == 0:
poi += 1
continue
points = results[poi][(pointer >> (poi * 2)) & 1][(pointer >> (poi * 2 + 1)) & 1]
crosspoint = []
if isnan(points[0][0]):
if isnan(points[1][0]):
maxx = 0
break
else:
crosspoint = points[1]
else:
crosspoint = points[0]
# print("poi: " , poi , ((pointer >> (poi * 2)) & 1) , ((pointer >> (poi * 2 + 1)) & 1) ,crosspoint)
poi += 1
if maxx != 0 and (maxx < scale * scale or maxx == n):
midPoint = [midPointX / numOfPois, midPointY / numOfPois]
pointer += 1
midPoint = [bestmidPointX / numOfPois, bestmidPointY / numOfPois]
answer = midPoint
print(answer)
if msg.command == 'RTLS_CMD_RESET_DEVICE' and msg.type == 'AsyncReq':
sending_node.device_resets = True
if msg.command == 'RTLS_CMD_CONN_INFO' and msg.type == 'SyncRsp':
sending_node.cci_started = True
if msg.command == 'RTLS_EVT_CONN_INFO' and msg.type == 'AsyncReq':
self.conn_info_queue.put({
"name": sending_node.name,
"type": "conn_info",
"identifier": identifier,
"payload": msg.payload
})
if msg.command == 'RTLS_CMD_SET_RTLS_PARAM' and msg.payload.rtlsParamType == "RTLS_PARAM_CONNECTION_INTERVAL" and msg.payload.status == "RTLS_SUCCESS":
sending_node.conn_interval_updated = True
if msg.command == 'RTLS_CMD_IDENTIFY' and msg.type == 'SyncRsp':
sending_node.identified = True
sending_node.identifier = msg.payload.identifier
sending_node.capabilities = msg.payload.capabilities
sending_node.devId = msg.payload.devId
sending_node.revNum = msg.payload.revNum
except queue.Empty:
pass
def _start_message_receiver(self):
self._message_receiver_stop = False
self._message_receiver_th = threading.Thread(target=self._message_receiver)
self._message_receiver_th.setDaemon(True)
self._message_receiver_th.start()
def _empty_queue(self, q):
while True:
try:
q.get(timeout=0.5)
except queue.Empty:
break
def _is_passive_in_nodes(self, nodes):
for node in nodes:
if not node.capabilities.get('RTLS_MASTER', False):
return True
return False
## User Function
def add_user_log(self, msg):
print(msg)
self.logger.info(msg)
## Devices API
def indentify_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
_rtls_manager = RTLSManager(nodes, websocket_port=None)
_rtls_manager_subscriber = _rtls_manager.create_subscriber()
_rtls_manager.auto_params = False
_rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
_master_node, _passive_nodes, failed = _rtls_manager.wait_identified()
_all_nodes = [pn for pn in _passive_nodes] ## deep copy
_all_nodes.extend([_master_node])
_all_nodes.extend([f for f in failed])
_rtls_manager.stop()
while not _rtls_manager.stopped:
time.sleep(0.1)
_rtls_manager_subscriber = None
_rtls_manager = None
return _all_nodes
def set_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
self._rtls_manager = RTLSManager(nodes, websocket_port=self.websocket_port)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = True
self._start_message_receiver()
self.logger.info("Message receiver started")
self._rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified()
if self._master_node is None:
raise RtlsUtilMasterNotFoundException("No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException("{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.ble_connected = False
node.device_resets = False
self.logger.info("Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
def get_devices_capability(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.identified = False
node.rtls.identify()
true_cond_func = lambda nodes: all([n.identified for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All device to identified")
ret = []
for node in nodes_to_set:
dev_info = {
"node_mac_address": node.identifier,
"capabilities": node.capabilities
}
ret.append(dev_info)
return ret
######
## Common BLE API
def _get_slave_by_addr(self, addr):
for _scan_result in self._scan_results:
if _scan_result['addr'].lower() == addr.lower():
return _scan_result
return None
def _get_slave_by_conn_handle(self, conn_handle):
for _slave in self._connected_slave:
if _slave['conn_handle'] == conn_handle:
return _slave
return None
def _add_scan_result(self, scan_result):
if self._get_slave_by_addr(scan_result['addr']) is None:
self._scan_results.append(scan_result)
def scan(self, scan_time_sec, expected_slave_bd_addr=None):
self._scan_results = []
timeout = time.time() + scan_time_sec
timeout_reached = time.time() > timeout
while not timeout_reached:
self._scan_stopped.clear()
self._master_node.rtls.scan()
scan_start_time = time.time()
scan_timeout_reached = time.time() > (scan_start_time + 10)
while not self._scan_stopped.isSet() and not scan_timeout_reached:
time.sleep(0.1)
scan_timeout_reached = time.time() > (scan_start_time + 10)
if scan_timeout_reached:
raise RtlsUtilEmbeddedFailToStopScanException("Embedded side didn't finished due to timeout")
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(expected_slave_bd_addr) is not None:
break
timeout_reached = time.time() > timeout
else:
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(expected_slave_bd_addr) is not None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
raise RtlsUtilScanNoResultsException("No device with slave capability found")
return self._scan_results
@property
def ble_connected(self):
return len(self._connected_slave) > 0
def ble_connected_to(self, slave):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException("Input slave not a string and not contains required keys")
for s in self._connected_slave:
if s['addr'] == slave['addr'] and s['conn_handle'] == slave['conn_handle']:
return True
return False
def ble_connect(self, slave, connect_interval_mSec):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException("Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException("Input slave not a string and not contains required keys")
interval = int(connect_interval_mSec / 1.25)
self._conn_handle = None
self._slave_attempt_to_connect = slave
self._master_node.connection_in_progress = True
self._master_node.connected = False
self._master_node.rtls.connect(slave['addrType'], slave['addr'], interval)
true_cond_func = lambda master_node: master_node.connection_in_progress == False
self._rtls_wait(true_cond_func, self._master_node, "All node to connect")
if self._master_node.connected:
slave['conn_handle'] = self._conn_handle
self._connected_slave.append(slave)
self._slave_attempt_to_connect = None
self._ble_connected = True
self.logger.info("Connection process done")
return self._conn_handle
return None
def ble_disconnect(self, conn_handle=None, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.terminate_link()
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.terminate_link(slave['conn_handle'])
else:
node.rtls.terminate_link(conn_handle)
true_cond_func = lambda event: event.isSet()
self._rtls_wait(true_cond_func, self._master_disconnected, "Master disconnect")
self._ble_connected = False
self.logger.info("Disconnect process done")
def set_connection_interval(self, connect_interval_mSec, conn_handle=None):
conn_interval = int(connect_interval_mSec / 1.25)
data_len = 2
data_bytes = conn_interval.to_bytes(data_len, byteorder='little')
self._master_node.conn_interval_updated = False
if str(self._master_node.devId) == "DeviceFamily_ID_CC26X0R2":
self._master_node.rtls.set_rtls_param('RTLS_PARAM_CONNECTION_INTERVAL', data_len, data_bytes)
else:
if conn_handle is None:
for s in self._connected_slave:
self._master_node.rtls.set_rtls_param(s['conn_handle'],
'RTLS_PARAM_CONNECTION_INTERVAL',
data_len,
data_bytes)
else:
self._master_node.rtls.set_rtls_param(conn_handle,
'RTLS_PARAM_CONNECTION_INTERVAL',
data_len,
data_bytes)
true_cond_func = lambda nodes: all([n.conn_interval_updated for n in nodes])
self._rtls_wait(true_cond_func, [self._master_node], "Master node set connection interval")
self.logger.info("Connection Interval Updated")
def reset_devices(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.device_resets = False
node.rtls.reset_device()
true_cond_func = lambda nodes: all([n.device_resets for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to reset")
def is_multi_connection_supported(self, nodes):
for node in nodes:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
return False
return True
######
## CCI - Continuous Connection Info
def cci_start(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.cci_started = False
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(True)
else:
if conn_handle is None:
for s in self._connected_slave:
node.rtls.get_conn_info(s['conn_handle'], True)
else:
node.rtls.get_conn_info(conn_handle, True)
true_cond_func = lambda nodes: all([n.cci_started for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node start continues connect info (CCI)")
self._is_cci_started = True
def cci_stop(self, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.get_conn_info(False)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.get_conn_info(slave['conn_handle'], False)
else:
node.rtls.get_conn_info(conn_handle, False)
self._is_cci_started = False
######
## AOA - Angle of Arrival
def is_aoa_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].AOA_TX == True or device_capab['capabilities'].AOA_RX == True):
return False
return True
def _aoa_set_params(self, node, aoa_params, conn_handle):
try:
node.aoa_initialized = False
node_role = 'AOA_MASTER' if node.capabilities.get('RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_set_params(
node_role,
aoa_params['aoa_run_mode'],
aoa_params['aoa_cc2640r2']['aoa_cte_scan_ovs'],
aoa_params['aoa_cc2640r2']['aoa_cte_offset'],
aoa_params['aoa_cc2640r2']['aoa_cte_length'],
aoa_params['aoa_cc2640r2']['aoa_sampling_control']
)
else:
node.rtls.aoa_set_params(
node_role,
aoa_params['aoa_run_mode'],
conn_handle,
aoa_params['aoa_cc26x2']['aoa_slot_durations'],
aoa_params['aoa_cc26x2']['aoa_sample_rate'],
aoa_params['aoa_cc26x2']['aoa_sample_size'],
aoa_params['aoa_cc26x2']['aoa_sampling_control'],
aoa_params['aoa_cc26x2']['aoa_sampling_enable'],
aoa_params['aoa_cc26x2']['aoa_pattern_len'],
aoa_params['aoa_cc26x2']['aoa_ant_pattern']
)
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
def aoa_set_params(self, aoa_params, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.aoa_initialized = False
if conn_handle is None:
for slave in self._connected_slave:
self._aoa_set_params(node, aoa_params, slave['conn_handle'])
else:
self._aoa_set_params(node, aoa_params, conn_handle)
true_cond_func = lambda nodes: all([n.aoa_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to set AOA params")
def _aoa_set_state(self, start, cte_interval=1, cte_length=20, nodes=None, conn_handle=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node_role = 'AOA_MASTER' if node.capabilities.get('RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_start(start)
else:
if conn_handle is None:
for slave in self._connected_slave:
node.rtls.aoa_start(slave['conn_handle'], start, cte_interval, cte_length)
else:
node.rtls.aoa_start(conn_handle, start, cte_interval, cte_length)
self._is_aoa_started = start
def aoa_start(self, cte_length, cte_interval, nodes=None, conn_handle=None):
self._aoa_set_state(start=True, cte_length=cte_length, cte_interval=cte_interval, nodes=nodes,
conn_handle=conn_handle)
self.logger.info("AOA Started")
def aoa_stop(self, nodes=None, conn_handle=None):
self._aoa_set_state(start=False, nodes=nodes, conn_handle=conn_handle)
self.logger.info("AOA Stopped")
class RtlsUtil():
def __init__(self, logging_file, logging_level):
logging.basicConfig(
filename=logging_file,
filemode="a",
level=logging_level,
format=
'[%(asctime)s] %(filename)-18sln %(lineno)3d %(threadName)-10s %(levelname)8s - %(message)s'
)
self._master_node = None
self._passive_nodes = []
self._all_nodes = []
self._rtls_manager = None
self._rtls_manager_subscriber = None
self._message_receiver_th = None
self._message_receiver_stop = False
self._scan_results = []
self._scan_stopped = threading.Event()
self._scan_stopped.clear()
self._ble_connected = False
self._master_disconnected = threading.Event()
self._master_disconnected.clear()
self._master_seed = None
self._timeout = 30
self.aoa_results_queue = queue.Queue()
self.tof_results_queue = queue.Queue()
self.conn_info_queue = queue.Queue()
self.acc_results_queue = queue.Queue()
def __del__(self):
self.done()
@property
def timeout(self):
return self._timeout
@timeout.setter
def timeout(self, value):
self._timeout = value
def _rtls_wait(self, true_cond_func, nodes, timeout_message):
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while not true_cond_func(nodes) and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(
f"Timeout reached while waiting for : {timeout_message}")
def done(self):
if self._message_receiver_th is not None:
self._message_receiver_stop = True
self._message_receiver_th.join()
self._message_receiver_th = None
if self._rtls_manager:
self._rtls_manager.stop()
while not self._rtls_manager.stopped:
time.sleep(0.1)
self._rtls_manager_subscriber = None
self._rtls_manager = None
def _message_receiver(self):
while not self._message_receiver_stop:
# Get messages from manager
try:
identifier, msg_pri, msg = self._rtls_manager_subscriber.pend(
block=True, timeout=0.05).as_tuple()
# Get reference to RTLSNode based on identifier in message
sending_node = self._rtls_manager[identifier]
if sending_node in self._passive_nodes:
logging.log(RtlsUtilLoggingLevel.UTIL_RAW_TRAFFIC,
f"PASSIVE: {identifier} --> {msg.as_json()}")
else:
logging.log(RtlsUtilLoggingLevel.UTIL_RAW_TRAFFIC,
f"MASTER: {identifier} --> {msg.as_json()}")
if msg.command == "RTLS_CMD_SCAN" and msg.type == "AsyncReq":
self._add_scan_result({
'addr': msg.payload.addr,
'addrType': msg.payload.addrType
})
if msg.command == "RTLS_CMD_SCAN_STOP" and msg.type == "AsyncReq":
self._scan_stopped.set()
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_SUCCESS":
sending_node.ble_connected = True
if sending_node.identifier == self._master_node.identifier:
self._master_disconnected.clear()
if msg.command == "RTLS_CMD_CONNECT" and msg.type == "AsyncReq" and msg.payload.status == "RTLS_LINK_TERMINATED":
sending_node.ble_connected = False
if sending_node.identifier == self._master_node.identifier:
self._master_disconnected.set()
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if msg.command in [
"RTLS_CMD_AOA_RESULT_ANGLE", "RTLS_CMD_AOA_RESULT_RAW",
"RTLS_CMD_AOA_RESULT_PAIR_ANGLES"
] and msg.type == "AsyncReq":
self.aoa_results_queue.put({
"name": sending_node.name,
"type": "aoa",
"identifier": identifier,
"payload": msg.payload
})
logging.log(RtlsUtilLoggingLevel.UTIL_AOA_RESULTS,
msg=json.dumps(msg.payload))
if msg.command == 'RTLS_CMD_ACC_ENABLE': #and msg.payload.status == 'RTLS_SUCCESS':
#print(msg.payload.status)
print("Acc enabled")
if msg.command == 'RTLS_CMD_ACC_RESULT_RAW' and msg.type == "AsyncReq":
self.acc_results_queue.put({ #"Payload" : msg.payload })
"X0": msg.payload.X0,
"X1": msg.payload.X1,
"Y0": msg.payload.Y0,
"Y1": msg.payload.Y1,
"Z0": msg.payload.Z0,
"Z1": msg.payload.Z1
})
logging.log(RtlsUtilLoggingLevel.UTIL_ALL,
msg=json.dumps(msg.payload))
if msg.command == 'RTLS_CMD_ACC_RESULT' and msg.type == "AsyncReq":
self.acc_results_queue.put({#"Payload" : msg.payload })
"X" : msg.payload.X,
"Y" : msg.payload.Y,
"Z" : msg.payload.Z,
})
logging.log(RtlsUtilLoggingLevel.UTIL_ALL,
msg=json.dumps(msg.payload))
if msg.command == 'RTLS_CMD_TOF_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.tof_initialized = True
if msg.command == 'RTLS_CMD_TOF_GET_SEC_SEED' and msg.payload.seed is not None:
self._master_seed = msg.payload.seed
if msg.command == 'RTLS_CMD_TOF_SET_SEC_SEED' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.seed_initialized = True
if msg.command == 'RTLS_CMD_TOF_ENABLE' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.tof_started = True
if msg.command in [
"RTLS_CMD_TOF_RESULT_DIST", "RTLS_CMD_TOF_RESULT_STAT",
"RTLS_CMD_TOF_RESULT_RAW"
] and msg.type == "AsyncReq":
self.tof_results_queue.put({
"name": sending_node.name,
"type": "tof",
"identifier": identifier,
"payload": msg.payload
})
logging.log(RtlsUtilLoggingLevel.UTIL_TOF_RESULTS,
msg=json.dumps(msg.payload))
if msg.command == 'RTLS_CMD_TOF_CALIBRATE' and msg.type == 'SyncRsp':
sending_node.tof_calibration_configured = True
if msg.command == 'RTLS_CMD_TOF_CALIBRATE' and msg.type == 'AsyncReq':
sending_node.tof_calibrated = True
if msg.command == 'RTLS_CMD_RESET_DEVICE' and msg.type == 'AsyncReq':
sending_node.device_resets = True
if msg.command == 'RTLS_CMD_TOF_CALIB_NV_READ' and msg.type == 'AsyncReq':
sending_node.tof_calib_info = msg.payload
sending_node.tof_calib_read_return = True
if msg.command == 'RTLS_CMD_CONN_INFO' and msg.type == 'SyncRsp':
sending_node.cci_started = True
if msg.command == 'RTLS_EVT_CONN_INFO' and msg.type == 'AsyncReq':
self.conn_info_queue.put({
"name": sending_node.name,
"type": "conn_info",
"identifier": identifier,
"payload": msg.payload
})
logging.log(RtlsUtilLoggingLevel.UTIL_CCI_RESULTS,
msg=json.dumps(msg.payload))
if msg.command == 'RTLS_CMD_SET_RTLS_PARAM' and msg.payload.rtlsParamType == "RTLS_PARAM_CONNECTION_INTERVAL" and msg.payload.status == "RTLS_SUCCESS":
sending_node.conn_interval_updated = True
if msg.command == 'RTLS_CMD_TOF_SWITCH_ROLE' and msg.payload.status == "RTLS_SUCCESS":
sending_node.role_switched = True
if msg.command == 'RTLS_CMD_IDENTIFY' and msg.type == 'SyncRsp':
sending_node.identified = True
sending_node.identifier = msg.payload.identifier
sending_node.capabilities = msg.payload.capabilities
sending_node.devId = msg.payload.devId
sending_node.revNum = msg.payload.revNum
except queue.Empty:
pass
def _start_message_receiver(self):
self._message_receiver_stop = False
self._message_receiver_th = threading.Thread(
target=self._message_receiver)
self._message_receiver_th.setDaemon(True)
self._message_receiver_th.start()
def _empty_queue(self, q):
while True:
try:
q.get(timeout=0.5)
except queue.Empty:
break
def _is_passive_in_nodes(self, nodes):
for node in nodes:
if not node.capabilities.get('RTLS_MASTER', False):
return True
return False
## Devices API
def set_devices(self, devices_setting):
logging.log(RtlsUtilLoggingLevel.UTIL_ALL,
"Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [
RTLSNode(node["com_port"], node["baud_rate"], node["name"])
for node in devices_setting
]
self._rtls_manager = RTLSManager(nodes, websocket_port=None)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = True
self._start_message_receiver()
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "Message receiver started")
self._rtls_manager.start()
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified(
)
if self._master_node is None:
raise RtlsUtilMasterNotFoundException(
"No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException(
"{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.tof_calib_info = None
node.tof_calib_read_return = False
node.tof_initialized = False
node.seed_initialized = False
node.tof_calibration_configured = False
node.tof_calibrated = False
node.tof_started = False
node.ble_connected = False
node.device_resets = False
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
def get_devices_capability(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.identified = False
node.rtls.identify()
true_cond_func = lambda nodes: all([n.identified for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All device to identified")
ret = []
for node in nodes_to_set:
dev_info = {
"node_mac_address": node.identifier,
"capabilities": node.capabilities
}
ret.append(dev_info)
return ret
######
## Common BLE API
def _get_slave_by_addr(self, addr):
for _scan_result in self._scan_results:
if _scan_result['addr'].lower() == addr.lower():
return _scan_result
return None
def _add_scan_result(self, scan_result):
if self._get_slave_by_addr(scan_result['addr']) is None:
self._scan_results.append(scan_result)
def scan(self, scan_time_sec, expected_slave_bd_addr=None):
self._scan_results = []
timeout = time.time() + scan_time_sec
timeout_reached = time.time() > timeout
while not timeout_reached:
self._scan_stopped.clear()
self._master_node.rtls.scan()
while not self._scan_stopped.isSet():
time.sleep(0.1)
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(
expected_slave_bd_addr) is not None:
break
timeout_reached = time.time() > timeout
else:
if len(self._scan_results) > 0:
if expected_slave_bd_addr is not None and self._get_slave_by_addr(
expected_slave_bd_addr) is not None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
raise RtlsUtilScanNoResultsException(
"No device with slave capability found")
return self._scan_results
@property
def ble_connected(self):
return self._ble_connected
def ble_connect(self, slave, connect_interval_mSec):
if isinstance(slave, str):
slave = self._get_slave_by_addr(slave)
if slave is None:
raise RtlsUtilScanSlaveNotFoundException(
"Expected slave not found in scan list")
else:
if 'addr' not in slave.keys() or 'addrType' not in slave.keys():
raise RtlsUtilException(
"Input slave not a string and not contains required keys")
interval = int(connect_interval_mSec / 1.25)
self._master_node.rtls.connect(slave['addrType'], slave['addr'],
interval)
true_cond_func = lambda nodes: all([n.ble_connected for n in nodes])
self._rtls_wait(true_cond_func, self._all_nodes, "All node to connect")
self._ble_connected = True
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "Connection process done")
def ble_disconnect(self):
self._master_node.rtls.terminate_link()
true_cond_func = lambda event: event.isSet()
self._rtls_wait(true_cond_func, self._master_disconnected,
"Master disconnect")
self._ble_connected = False
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "Disconnect process done")
def set_connection_interval(self, connect_interval_mSec):
conn_interval = int(connect_interval_mSec / 1.25)
data_len = 2
data_bytes = conn_interval.to_bytes(data_len, byteorder='little')
self._master_node.conn_interval_updated = False
self._master_node.rtls.set_rtls_param('RTLS_PARAM_CONNECTION_INTERVAL',
data_len, data_bytes)
true_cond_func = lambda nodes: all(
[n.conn_interval_updated for n in nodes])
self._rtls_wait(true_cond_func, [self._master_node],
"Master node set connection interval")
logging.log(RtlsUtilLoggingLevel.UTIL_ALL,
"Connection Interval Updated")
def reset_devices(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.device_resets = False
node.rtls.reset_device()
true_cond_func = lambda nodes: all([n.device_resets for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set, "All node to reset")
######
## CCI - Continuous Connection Info
def cci_start(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
self._empty_queue(self.conn_info_queue)
for node in nodes_to_set:
node.cci_started = False
node.rtls.get_conn_info(True)
true_cond_func = lambda nodes: all([n.cci_started for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All node start continues connect info (CCI)")
def cci_stop(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.rtls.get_conn_info(False)
######
## ACC
def acc_start(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
self._empty_queue(self.acc_results_queue)
for node in nodes_to_set:
node.acc_started = True #HÄR
node.rtls.acc_start() #HÄR
true_cond_func = lambda nodes: all([n.acc_started
for n in nodes]) #HÄR
self._rtls_wait(true_cond_func, nodes_to_set, "All node start ACC")
def acc_stop(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.rtls.get_conn_info(False) #HÄR
######
## AOA - Angle of Arrival
def is_aoa_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].AOA_TX == True
or device_capab['capabilities'].AOA_RX == True):
return False
return True
def aoa_set_params(self, aoa_params, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
try:
for node in nodes_to_set:
node.aoa_initialized = False
node_role = 'AOA_MASTER' if node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_set_params(
node_role, aoa_params['aoa_run_mode'],
aoa_params['aoa_cc2640r2']['aoa_cte_scan_ovs'],
aoa_params['aoa_cc2640r2']['aoa_cte_offset'],
aoa_params['aoa_cc2640r2']['aoa_cte_length'])
else:
node.rtls.aoa_set_params(
node_role, aoa_params['aoa_run_mode'],
aoa_params['aoa_cc26x2']['aoa_slot_durations'],
aoa_params['aoa_cc26x2']['aoa_sample_rate'],
aoa_params['aoa_cc26x2']['aoa_sample_size'],
aoa_params['aoa_cc26x2']['aoa_sampling_control'],
aoa_params['aoa_cc26x2']['aoa_sampling_enable'],
aoa_params['aoa_cc26x2']['aoa_num_of_ant'],
aoa_params['aoa_cc26x2']['aoa_ant_array_switch'],
aoa_params['aoa_cc26x2']['aoa_ant_array'])
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
true_cond_func = lambda nodes: all([n.aoa_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All node to set AOA params")
def _aoa_set_state(self, start, cte_interval=1, cte_length=20, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node_role = 'AOA_MASTER' if node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
if str(node.devId) == "DeviceFamily_ID_CC26X0R2":
node.rtls.aoa_start(start)
else:
node.rtls.aoa_start(start, cte_interval, cte_length)
def aoa_start(self, cte_length, cte_interval, nodes=None):
self._aoa_set_state(start=True,
cte_length=cte_length,
cte_interval=cte_interval,
nodes=nodes)
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "AOA Started")
def aoa_stop(self, nodes=None):
self._aoa_set_state(start=False, nodes=nodes)
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "AOA Stopped")
######
## TOF - Time of Flight
def is_tof_supported(self, nodes):
devices_capab = self.get_devices_capability(nodes)
for device_capab in devices_capab:
if not (device_capab['capabilities'].TOF_MASTER == True
or device_capab['capabilities'].TOF_PASSIVE == True):
return False
return True
def _tof_get_master_seed(self):
self._master_seed = None
self._master_node.rtls.tof_get_sec_seed()
timeout = time.time() + self._timeout
timeout_reached = time.time() > timeout
while self._master_seed is None and not timeout_reached:
time.sleep(0.1)
timeout_reached = time.time() > timeout
if timeout_reached:
raise RtlsUtilTimeoutException(
f"Timeout reached while waiting for : Master SEED")
def tof_set_params(self, tof_params, seed_exchange=True, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
try:
for node in nodes_to_set:
node.tof_initialized = False
node_role = 'TOF_MASTER' if node.capabilities.get(
'TOF_MASTER', False) else 'TOF_PASSIVE'
node.rtls.tof_set_params(
node_role, tof_params['tof_samples_per_burst'],
len(tof_params['tof_freq_list']),
tof_params['tof_slave_lqi_filter'],
tof_params['tof_post_process_lqi_thresh'],
tof_params['tof_post_process_magn_ratio'],
tof_params['tof_auto_rssi'], tof_params['tof_sample_mode'],
tof_params['tof_run_mode'], tof_params['tof_freq_list'])
except KeyError as ke:
raise RtlsUtilException("Invalid key : {}".format(str(ke)))
true_cond_func = lambda nodes: all([n.tof_initialized for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All nodes set TOF params")
if seed_exchange:
self._tof_get_master_seed()
for node in nodes_to_set:
## Set seed only to passive nodes
if not node.capabilities.get('TOF_MASTER', False):
node.seed_initialized = False
node.rtls.tof_set_sec_seed(self._master_seed)
true_cond_func = lambda nodes: all([
n.seed_initialized for n in nodes
if not node.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Passive nodes set SEED")
def _tof_calib_set_passive_node(self, nodes, samples_per_freq, distance,
use_nv_calib):
for node in nodes:
if not node.capabilities.get('TOF_MASTER', False):
node.tof_calibration_configured = False
node.tof_calibrated = False
node.rtls.tof_calib(True, samples_per_freq, distance,
use_nv_calib)
true_cond_func = lambda nodes: all([
n.tof_calibration_configured for n in nodes
if not n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes,
"Passive nodes to set TOF calibration params")
def _tof_calib_set_master_node(self, nodes, samples_per_freq, distance,
use_nv_calib):
for node in nodes:
if node.capabilities.get('TOF_MASTER', False):
node.tof_calibration_configured = False
node.tof_calibrated = False
node.rtls.tof_calib(True, samples_per_freq, distance,
use_nv_calib)
true_cond_func = lambda nodes: all([
n.tof_calibration_configured for n in nodes
if n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes,
"Master node to set TOF calibration params")
def _tof_start_in_passive_node(self, nodes):
for node in nodes:
if not node.capabilities.get('TOF_MASTER', False):
node.tof_started = False
node.rtls.tof_start(True)
true_cond_func = lambda nodes: all([
n.tof_started for n in nodes
if not node.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes, "Passive nodes to start TOF")
def _tof_start_in_master_node(self, nodes):
for node in nodes:
if node.capabilities.get('TOF_MASTER', False):
node.tof_started = False
node.rtls.tof_start(True)
true_cond_func = lambda nodes: all([n.tof_started for n in nodes])
self._rtls_wait(true_cond_func, nodes, "All nodes to start TOF")
def tof_start(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
self._tof_start_in_passive_node(nodes_to_set)
self._tof_start_in_master_node(nodes_to_set)
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "TOF Started")
def tof_stop(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("nodes input must be from list type")
for node in nodes_to_set:
node.tof_started = False
node.rtls.tof_start(False)
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "TOF Stopped")
def tof_calibrate(self,
samples_per_freq,
distance,
use_nv_calib=False,
nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
if self._is_passive_in_nodes(nodes_to_set):
self._tof_calib_set_passive_node(nodes_to_set, samples_per_freq,
distance, use_nv_calib)
self._tof_start_in_passive_node(nodes_to_set)
self._tof_calib_set_master_node(nodes_to_set, 0, distance,
use_nv_calib)
self._tof_start_in_master_node(nodes_to_set)
true_cond_func = lambda nodes: all([
n.tof_calibrated for n in nodes
if not n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Passive nodes to end TOF calibration")
for node in nodes_to_set:
if node.capabilities.get('TOF_MASTER', False):
node.rtls.tof_calib(False, 0, distance, False)
else:
self._tof_calib_set_master_node(nodes_to_set, samples_per_freq,
distance, use_nv_calib)
self._tof_start_in_master_node(nodes_to_set)
true_cond_func = lambda nodes: all([
n.tof_calibrated for n in nodes
if n.capabilities.get('TOF_MASTER', False)
])
self._rtls_wait(true_cond_func, nodes_to_set,
"Master nodes to end TOF calibration")
for node in nodes_to_set:
node.tof_started = False
node.rtls.tof_start(False)
self._empty_queue(self.tof_results_queue)
logging.log(RtlsUtilLoggingLevel.UTIL_ALL, "Calibration Done")
def tof_get_calib_info(self, nodes=None):
nodes_to_set = self._all_nodes
if nodes is not None:
if isinstance(nodes, list):
nodes_to_set = nodes
else:
raise RtlsUtilException("Nodes input must be from list type")
for node in nodes_to_set:
node.tof_calib_info = None
node.tof_calib_read_return = False
node.rtls.read_calib_from_NV()
true_cond_func = lambda nodes: all(
[n.tof_calib_read_return for n in nodes])
self._rtls_wait(true_cond_func, nodes_to_set,
"All nodes return calibration info from NV")
calib_info = []
for node in nodes_to_set:
info = {
"node_mac_address": node.identifier,
"calibration_info": node.tof_calib_info
}
calib_info.append(info)
return calib_info
def tof_role_switch(self, tof_master_node, tof_passive_node):
if not tof_master_node.capabilities.get('TOF_MASTER', False):
raise RtlsUtilException(
"Given TOF Master Node not active as TOF Master Node")
if not tof_passive_node.capabilities.get('TOF_PASSIVE', False):
raise RtlsUtilException(
"Given TOF Passive Node not active as TOF Passive Node")
tof_master_node.role_switched = False
tof_master_node.rtls.tof_switch_role("TOF_PASSIVE")
tof_passive_node.role_switched = False
tof_passive_node.rtls.tof_switch_role("TOF_MASTER")
true_cond_func = lambda nodes: all([n.role_switched for n in nodes])
self._rtls_wait(true_cond_func, [tof_master_node, tof_passive_node],
"Role Switch")
def run_forever():
# Initialize, but don't start RTLS Nodes to give to the RTLSManager
my_nodes = [
RTLSNode('/dev/ttyACM0', 115200),
RTLSNode('/dev/ttyACM2', 115200)
]
# Initialize references to the connected devices
master_node = None
passive_nodes = []
# Initialize references to the connected devices
address = None
address_type = None
# AoA related settings
aoa_run_mode = 'AOA_MODE_ANGLE'
aoa_cte_scan_ovs = 4
aoa_cte_offset = 4
aoa_cte_time = 20
# Auto detect AoA or ToF support related
aoa_supported = False
# If slave addr is None, the script will connect to the first RTLS slave
# that it found. If you wish to connect to a specific device
# (in the case of multiple RTLS slaves) then you may specify the address
# explicitly as given in the comment to the right
slave_addr = None # '54:6C:0E:83:3F:3D'
# Scan list
scanResultList = list()
# connection interval in units of 1.25msec configured in connection request, default is 80 (100ms)
conn_interval = 80
# Initialize manager reference, because on Exception we need to stop the manager to stop all the threads.
manager = None
try:
# Start an RTLSManager instance without WebSocket server enabled
manager = RTLSManager(my_nodes, websocket_port=None)
# Create a subscriber object for RTLSManager messages
subscriber = manager.create_subscriber()
# Tell the manager to automatically distribute connection parameters
manager.auto_params = True
# Start RTLS Node threads, Serial threads, and manager thread
manager.start()
# Wait until nodes have responded to automatic identify command and get reference
# to single master RTLSNode and list of passive RTLSNode instances
master_node, passive_nodes, failed = manager.wait_identified()
if len(failed):
print(
f"ERROR: {len(failed)} nodes could not be identified. Are they programmed?"
)
# Exit if no master node exists
if not master_node:
raise RuntimeError("No RTLS Master node connected")
# Combined list for lookup
all_nodes = passive_nodes + [master_node]
# Initialize application variables on nodes
for node in all_nodes:
node.seed_initialized = False
node.aoa_initialized = False
#
# At this point the connected devices are initialized and ready
#
# Display list of connected devices and their capabilities
print(
f"{master_node.identifier} {', '.join([cap for cap, available in master_node.capabilities.items() if available])}"
)
# Iterate over Passives and detect their capabilities
for pn in passive_nodes:
print(
f"{pn.identifier} {', '.join([cap for cap, available in pn.capabilities.items() if available])}"
)
# Check over aggregated capabilities to see if they make sense
capabilities_per_node = [[
cap for cap, avail in node.capabilities.items() if avail
] for node in all_nodes]
# Assume AoA if all nodes are not ToF
aoa_supported = all(
not ('TOF_PASSIVE' in node_caps or 'TOF_MASTER' in node_caps)
for node_caps in capabilities_per_node)
# Check that Nodes all must be AoA
if not (aoa_supported):
raise RuntimeError("All nodes must be AoA")
# Need at least 1 passive for AoA
if aoa_supported and len(passive_nodes) == 0:
raise RuntimeError('Need at least 1 passive for AoA')
# Send an example command to each of them, from commands listed at the bottom of rtls/ss_rtls.py
for n in all_nodes:
n.rtls.identify()
while True:
# Get messages from manager
try:
identifier, msg_pri, msg = subscriber.pend(
block=True, timeout=0.05).as_tuple()
# Get reference to RTLSNode based on identifier in message
sending_node = manager[identifier]
if sending_node in passive_nodes:
print(f"PASSIVE: {identifier} --> {msg.as_json()}")
else:
print(f"MASTER: {identifier} --> {msg.as_json()}")
# If we received an error, print it.
if msg.command == 'RTLS_EVT_ERROR':
print(
f"Received RTLS_EVT_ERROR with status: {msg.payload.status}"
)
# If we received an assert, print it.
if msg.command == 'RTLS_EVT_ASSERT' and msg.type == 'AsyncReq':
raise RuntimeError(
f"Received HCI H/W Assert with code: {msg.payload.cause}"
)
# After identify is received, we start scanning
if msg.command == 'RTLS_CMD_IDENTIFY':
master_node.rtls.scan()
# Once we start scaning, we will save the address of the
# last scan response
if msg.command == 'RTLS_CMD_SCAN' and msg.type == 'AsyncReq':
# Slave address none means that we connect to any slave
if slave_addr is None:
address = msg.payload.addr
address_type = msg.payload.addrType
else:
scanResultList.append(msg.payload.addr)
scanResultList.append(msg.payload.addrType)
# Once the scan has stopped and we have a valid address, then
# connect
if msg.command == 'RTLS_CMD_SCAN_STOP':
if slave_addr is None:
if address is not None and address_type is not None:
master_node.rtls.connect(address_type, address,
conn_interval)
elif slave_addr in scanResultList:
i = scanResultList.index(slave_addr)
master_node.rtls.connect(scanResultList[i + 1],
scanResultList[i],
conn_interval)
scanResultList.clear()
else:
# If we didn't find the device, keep scanning.
master_node.rtls.scan()
# Once we are connected, then we can do stuff
if msg.command == 'RTLS_CMD_CONNECT' and msg.type == 'AsyncReq':
if msg.payload.status == 'RTLS_SUCCESS':
# Find the role based on capabilities of sending node
role = 'AOA_MASTER' if sending_node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
# Send AoA params
sending_node.rtls.aoa_set_params(
role, aoa_run_mode, aoa_cte_scan_ovs,
aoa_cte_offset, aoa_cte_time)
else:
# If the connection failed, keep scanning
master_node.rtls.scan()
# Count the number of nodes that have ToF initialized
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if all([n.aoa_initialized for n in all_nodes]):
# Start AoA on the master and passive nodes
for node in all_nodes:
node.rtls.aoa_start(True)
# Wait for security seed
if msg.command == 'RTLS_CMD_TOF_GET_SEC_SEED' and msg.payload.seed is not 0:
seed = msg.payload.seed
for node in passive_nodes:
node.rtls.tof_set_sec_seed(seed)
# Wait until passives have security seed set
if msg.command == 'RTLS_CMD_TOF_SET_SEC_SEED' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.seed_initialized = True
try:
with open("Output/test.json", "a") as fichier:
message = json.loads("[" + msg.as_json() + "]")
message[0]["payload"]["time"] = timestamp
message[0]["payload"]["distance"] = args.distance
message[0]["payload"]["position"] = args.position
json.dump(message[0]["payload"], fichier)
print(message[0]["payload"])
fichier.write('\n')
fichier.close
except:
print('\nProblem writing on JSON file')
break
except queue.Empty:
pass
finally:
if manager:
manager.stop()