def get_position():
# start positioning
for _ in range(MAX_TRIES):
position = Coordinates()
if POZYX_SUCCESS == pozyx.doPositioning(position,
dimension,
height,
algorithm,
remote_id=remote_id):
return {'x': position.x, 'y': position.y, 'z': position.z}
# error handling
inactive_anchors = 0
if remote_id:
network_id = SingleRegister()
pozyx.getWhoAmI(network_id, remote_id=remote_id)
if network_id.data == [0]:
return send_error_msg(
'Could not establish connection to device with ID {}'.format(
remote_id.decode('utf-8')))
for a in anchors:
network_id = SingleRegister()
pozyx.getWhoAmI(network_id, remote_id=a.network_id)
if network_id.data == [0]:
inactive_anchors += 1
if inactive_anchors > 1:
return send_error_msg(
'Can\'t connect to at least {} anchors. Check the anchor\'s power connection '
'and the pozyx\'s USB connection'.format(inactive_anchors))
def get_device_list(pozyx):
""""""
device_list_size = SingleRegister()
pozyx.getDeviceListSize(device_list_size)
device_list = DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
return device_list
def printPublishConfigurationResult(self):
"""Prints and potentially publishes the anchor configuration result in a human-readable way."""
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/anchor", [
device_list[i],
int(anchor_coordinates.x),
int(anchor_coordinates.y),
int(anchor_coordinates.z)
])
sleep(0.025)
def loop(self):
"""Perform ranging and sets the LEDs accordingly."""
device_range = DeviceRange()
status = self.pozyx.doRanging(self.destination_id, device_range,
self.remote_id)
if status == POZYX_SUCCESS:
print(str(device_range))
d = str(device_range)
d = d.split(', ')
t = int(d[0].replace(' ms', ''))
dist = int(d[1].replace(' mm', ''))
dbm = int(d[2].replace(' dBm', ''))
vector = np.zeros((1, 3))
vector = np.array(([t, dist, dbm]))
# if self.ledControl(device_range.distance) == POZYX_FAILURE:
# print("ERROR: setting (remote) leds")
return vector
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
return ([None, None, None])
def printPublishErrorCode(self, operation):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
network_id = self.remote_id
if network_id is None:
self.pozyx.getErrorCode(error_code)
print("LOCAL ERROR %s, %s" %
(operation, self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/error",
[operation, 0, error_code[0]])
return
status = self.pozyx.getErrorCode(error_code, self.remote_id)
if status == POZYX_SUCCESS:
print("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/error", [operation, network_id, error_code[0]])
else:
self.pozyx.getErrorCode(error_code)
print(
"ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s"
% (operation, self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/error", [operation, 0, -1])
def printPublishErrorCode(self, operation, network_id):
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if not (status == POZYX_SUCCESS):
print("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
def loop(self):
"""Gets new IMU sensor data"""
sensor_data = SensorData()
calibration_status = SingleRegister()
if self.remote_id is not None or self.pozyx.checkForFlag(POZYX_INT_MASK_IMU, 0.01) == POZYX_SUCCESS:
status = self.pozyx.getAllSensorData(sensor_data, self.remote_id)
status &= self.pozyx.getCalibrationStatus(calibration_status, self.remote_id)
if status == POZYX_SUCCESS:
self.publishSensorData(sensor_data, calibration_status)
def ConfigureTag(self, tagId):
for anchor in self.anchors:
status = self.pozyx.addDevice(anchor, tagId)
self.CheckStatus(status)
status = self.pozyx.configureAnchors(self.anchors,
PozyxConstants.ANCHOR_SELECT_AUTO,
tagId)
self.CheckStatus(status)
mode = SingleRegister(PozyxConstants.GPIO_DIGITAL_INPUT)
pullup = SingleRegister(PozyxConstants.GPIO_PULL_UP)
status = self.pozyx.setConfigGPIO(1, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(2, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(3, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(4, mode, pullup, tagId)
self.CheckStatus(status)
def doSomething():
port = get_first_pozyx_serial_port()
print('Port:', port)
p = PozyxSerial(port)
whoami = SingleRegister()
p.regRead(POZYX_WHO_AM_I, whoami)
print('WhoAmI:', whoami)
return port, whoami.data
def GetCalibrationStatus(self, tagId):
result = SingleRegister()
status = self.pozyx.getCalibrationStatus(result, tagId)
self.CheckStatus(status)
value = dict()
value['magnetic'] = result.value & 3
value['accelerometer'] = (result.value & 12) >> 2
value['gyroscope'] = (result.value & 48) >> 4
value['system'] = (result.value & 192) >> 6
return value
def printPublishConfigurationResult(self):
list_size = SingleRegister()
status = self.pozyx.getDeviceListSize(list_size, self.remote_id)
rospy.loginfo("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
status = self.pozyx.getDeviceIds(device_list, self.remote_id)
rospy.loginfo("Calibration result: ")
rospy.loginfo("Anchors found: {0}".format(list_size[0]))
def printPublishErrorCode(self, operation, network_id):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if status == POZYX_SUCCESS:
print("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id, self.pozyx.getErrorMessage(error_code)))
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
print("Error % s, local error code %s" % (operation, str(error_code)))
def all_device_coordinates_in_device_list(pozyx, remote_id=None):
list_size = SingleRegister()
status = pozyx.getDeviceListSize(list_size, remote_id=remote_id)
if list_size.value == 0:
return
device_list = DeviceList(list_size=list_size.value)
status &= pozyx.getDeviceIds(device_list, remote_id=remote_id)
for device_id in device_list:
coordinates = Coordinates()
pozyx.getDeviceCoordinates(device_id, coordinates, remote_id=remote_id)
yield DeviceCoordinates(device_id, 0, pos=coordinates)
def loop(self):
"""Performs ranging and sets the LEDs accordingly"""
device_range = DeviceRange()
status = self.pozyx.doRanging(self.destination_id, device_range,
self.remote_id)
if status == POZYX_SUCCESS:
print(device_range)
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
def loop(self):
range = DeviceRange()
stat = self.pozyx.doRanging(self.dest, range, self.remote)
if (stat == POZYX_SUCCESS):
print(range)
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
def printError(self, operation):
# Prints Pozyx's errors
error_code = SingleRegister()
if None is None:
self.serial.getErrorCode(error_code)
print("LOCAL ERROR %s, %s" % (operation, self.serial.getErrorMessage(error_code)))
return
status = self.serial.getErrorCode(error_code, None)
if status == POZYX_SUCCESS:
print("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % None, self.serial.getErrorMessage(error_code)))
else:
self.serial.getErrorCode(error_code)
print("ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s" %
(operation, self.serial.getErrorMessage(error_code)))
def DiscoverTags(self):
status = self.pozyx.doDiscovery(PozyxConstants.DISCOVERY_TAGS_ONLY)
self.CheckStatus(status)
count = SingleRegister()
status = self.pozyx.getDeviceListSize(count)
self.CheckStatus(status)
allIds = DeviceList(list_size=count.value)
status = self.pozyx.getDeviceIds(allIds)
self.CheckStatus(status)
tagIds = list()
for id in allIds:
any = False
for tagId in self.anchors:
if tagId.network_id == id:
any = True
if not any:
tagIds.append(id)
return tagIds
def printPublishErrorCode(self, operation):
error_code = SingleRegister()
network_id = self.remote_id
if network_id is None:
self.pozyx.getErrorCode(error_code)
rospy.logerror("LOCAL ERROR %s, %s" %
(operation, self.pozyx.getErrorMessage(error_code)))
return
status = self.pozyx.getErrorCode(error_code, self.remote_id)
if status == POZYX_SUCCESS:
rospy.logerror("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
else:
self.pozyx.getErrorCode(error_code)
rospy.logerror(
"ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s"
% (operation, self.pozyx.getErrorMessage(error_code)))
def raise_error(self, fun):
pozyx = self.pozyx
remote_id = self.remote_id
error_code = SingleRegister()
if remote_id is None:
pozyx.getErrorCode(error_code)
self.lock.release()
c = error_code.data[0]
raise pozyx_exceptions.get(c, PozyxExceptionUnknown)(fun)
status = pozyx.getErrorCode(error_code, remote_id)
if status == POZYX_SUCCESS:
self.lock.release()
c = error_code.data[0]
raise pozyx_exceptions.get(c, PozyxExceptionUnknown)(fun)
else:
pozyx.getErrorCode(error_code)
self.lock.release()
raise PozyxExceptionNoConnection(fun)
def printPublishConfigurationResult(self):
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
pass
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def range_callback(self):
"""Do ranging periodically, and publish visualizasion_msg MarkerArray"""
for tag_id in self.tag_ids:
for anchor in self.anchors:
device_range = DeviceRange()
status = self.pozyx.doRanging(anchor.network_id, device_range,
tag_id)
if status == POZYX_SUCCESS:
self.publishMarkerArray(device_range.distance, anchor)
# self.get_logger().info(f"{device_range.distance}")
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
self.get_logger().error(
"ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
self.get_logger().error(
"ERROR Ranging, couldn't retrieve local error")
self.doPositioning()
def printPublishConfigurationResultMore(self, remote_id):
if not (remote_id is None):
print("Pozyx ID: 0x%0.4x" % remote_id)
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def loop(self):
"""Performs positioning and displays/exports the results."""
position = Coordinates()
sensor_data = SensorData()
self.publishData(position, sensor_data) #nur ohne anchors
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
self.publishData(position, sensor_data)
calibration_status = SingleRegister()
if self.remote_id is not None or self.pozyx.checkForFlag(
POZYX_INT_MASK_IMU, 0.01) == POZYX_SUCCESS:
status = self.pozyx.getAllSensorData(sensor_data, self.remote_id)
status &= self.pozyx.getCalibrationStatus(calibration_status,
self.remote_id)
def printConfig(self):
# prints the anchor configuration result
list_size = SingleRegister()
# prints the anchors list size
self.serial.getDeviceListSize(list_size, None)
if list_size[0] != len(self.anchors):
self.printError("configuration")
return
# prints the anchors list
device_list = DeviceList(list_size=list_size[0])
self.serial.getDeviceIds(device_list, None)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.serial.getDeviceCoordinates(device_list[i], anchor_coordinates, None)
print("ANCHOR: 0x%0.4x, %s" % (device_list[i], str(anchor_coordinates)))
sleep(0.025)
#import pypozyx
import sys, time
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings
#pozyx = PozyxLib() # PozyxSerial has PozyxLib's functions, just for generality
CURSOR_UP_ONE = '\x1b[1A'
ERASE_LINE = '\x1b[2K'
is_cursor_up = False
#print(pypozyx.get_first_pozyx_serial_port())
pozyx = PozyxSerial(get_first_pozyx_serial_port())
who_am_i = SingleRegister()
# get the data, passing along the container
status = pozyx.getWhoAmI(who_am_i)
acceleration = Acceleration()
euler_angles = EulerAngles()
uwb_settings = UWBSettings()
# check the status to see if the read was successful. Handling failure is covered later.
if status == POZYX_SUCCESS:
# print the container. Note how a SingleRegister will print as a hex string by default.
print('Who Am I: {}'.format(who_am_i)) # will print '0x43'
while True:
# initalize the Pozyx as above
# initialize the data container
# and repeat
# initialize the data container
# get the data, passing along the container
if is_cursor_up:
sys.stdout.write(CURSOR_UP_ONE)
def GetError(self):
code = SingleRegister()
status = self.pozyx.getErrorCode(code)
self.CheckStatus(status)
return self.pozyx.getErrorMessage(code)
POZYX_CONNECTED_TO_BASE = True
serial_port = get_pozyx_serial_port()
if serial_port is None:
POZYX_CONNECTED_TO_BASE = False
else:
try:
pozyx = PozyxSerial(serial_port)
status = pozyx.clearDevices()
for anchor in anchors:
status &= pozyx.addDevice(anchor)
remote_check = SingleRegister()
pozyx.getWhoAmI(remote_check, remote_id=remote_id)
if remote_check.data == [0]:
remote_id = None
MAX_TRIES = 1000
except PozyxConnectionError:
BUSY_SERIAL = True
def check_connection(func):
"""Check for errors before executing a pozyx function"""
@wraps(func)
def check(*args):
if BUSY_SERIAL:
return {
def GetGPIO(self, tagId, pinId):
value = SingleRegister()
status = self.pozyx.getGPIO(pinId, value, tagId)
self.CheckStatus(status)
return value.value == 1
