def set_anchor_configuration():
tag_ids = [None]
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=POZYX_DISCOVERY_TAGS_ONLY)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
tag_ids += device_list.data
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO,
len(anchors),
remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rosply.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
def get_devices(pozyx):
pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
return device_list
else:
return []
def check_visible_devices(self):
try:
self.pozyx.doDiscovery()
list_size = px.SingleRegister()
self.pozyx.getDeviceListSize(list_size)
if list_size[0] == 0:
return
device_list = px.DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list)
for d in device_list.data:
rospy.loginfo("Found device 0x%0.4x" % d)
except Exception as e:
rospy.logerr(e)
def pozyx_range():
pub = rospy.Publisher('pozyx_range', DeviceRange, queue_size=100)
rospy.init_node('range_pub')
try:
serial_port = pypozyx.get_first_pozyx_serial_port()
if serial_port == None:
rospy.loginfo("Pozyx not connected")
return
pozyx = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Could not connect to pozyx")
return
# get network id of connected pozyx
self_id = pypozyx.NetworkID()
if pozyx.getNetworkId(self_id) != pypozyx.POZYX_SUCCESS:
rospy.loginfo("Could not get network id")
return
# discover all other pozyx
pozyx.doDiscoveryAll()
devices_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(devices_size)
devices = pypozyx.DeviceList(devices_size)
pozyx.getDeviceIds(devices)
for i in range(devices_size[0]):
rospy.loginfo("Found pozyx device with the id {}".format(
pypozyx.NetworkID(devices[i])))
# publish data
while not rospy.is_shutdown():
for i in range(devices_size[0]):
device_range = pypozyx.DeviceRange()
remote_id = pypozyx.NetworkID(devices[i])
if pozyx.doRanging(self_id[0], device_range, remote_id[0]):
h = Header()
h.stamp = rospy.Time.from_sec(device_range[0] / 1000)
pub.publish(h, remote_id[0], device_range[1], device_range[2])
else:
rospy.loginfo("Error while ranging")
def check_config(self, anchors):
list_size = px.SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
if list_size[0] != len(anchors):
rospy.logerr('anchors were not properly configured')
return
device_list = px.DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
for anchor in device_list:
anchor_coordinates = px.Coordinates()
self.pozyx.getDeviceCoordinates(
anchor, anchor_coordinates, self.remote_id)
rospy.loginfo("anchor 0x%0.4x set to %s",
anchor, anchor_coordinates)
def _get_device_list(self):
# gets the device list
device_list = []
device_list_size = pypozyx.SingleRegister()
status = self.local.getDeviceListSize(device_list_size)
self._check_status("get device list size", status)
devices = pypozyx.DeviceList(device_list_size.value)
status = self.local.getDeviceIds(devices)
self._check_status("get device ids", status)
for id in devices:
device_list.append(id)
return device_list
def set_same_uwb_settings():
rospy.init_node('uwb_configurator')
devices_met = []
uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F]
s = ''
for device in device_ids:
s += '0x%0.4x ' % device
rospy.loginfo("Setting devices with IDs:%s to UWB settings: %s" %
(s, str(new_uwb_settings)))
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
#BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
for channel in pypozyx.POZYX_ALL_CHANNELS:
for bitrate in pypozyx.POZYX_ALL_BITRATES:
for prf in pypozyx.POZYX_ALL_PRFS:
for plen in pypozyx.POZYX_ALL_PLENS:
uwb_settings = pypozyx.UWBSettings(
channel, bitrate, prf, plen, gain_db)
pozyx.clearDevices()
pozyx.setUWBSettings(uwb_settings)
pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(
list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
for device in device_list:
if device not in devices_met and device in device_ids:
pozyx.setUWBSettings(new_uwb_settings, device)
pozyx.saveRegisters(uwb_registers, device)
rospy.loginfo(
'Device with ID 0x%0.4x is set' % device)
devices_met.append(device)
pozyx.setUWBSettings(new_uwb_settings)
pozyx.saveRegisters(uwb_registers)
rospy.loginfo("Local device set! Shutting down configurator node now...")
