def init_pozyx(self):
if False and rospy.has_param('~gain_db'):
gain = rospy.get_param('~gain_db')
configure.set_gain(self.pozyx, gain, set_all=False)
anchors = []
if rospy.has_param('~anchors/positions'):
anchors_data = rospy.get_param('~anchors/positions')
anchors = [px.DeviceCoordinates(dev_id, 1, px.Coordinates(x, y, z))
for [dev_id, x, y, z] in anchors_data]
self.check_visible_devices()
rospy.sleep(0.1)
self.set_anchors(anchors)
rospy.sleep(0.1)
self.check_config(anchors)
rospy.sleep(0.1)
if self.continuous:
if self.enable_position:
ms = int(1000.0 / self.rate)
# self.pozyx.setUpdateInterval(200)
msr = px.SingleRegister(ms, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id)
self.pozyx.setPositionAlgorithm(self.algorithm, self.dimension)
else:
msr = px.SingleRegister(0, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id)
rospy.sleep(0.1)
def run(self):
position = px.Coordinates()
error = px.PositionError()
sensor_data = px.SensorData()
pozyx = self.driver.pozyx
interrupt = px.SingleRegister()
while not rospy.is_shutdown():
try:
if pozyx.checkForFlag(self.flag, self.DELAY, interrupt):
if self.driver.enable_position and interrupt.data[
0] & bm.POZYX_INT_STATUS_POS:
pozyx.getCoordinates(position)
pozyx.getPositionError(error)
if self.driver.accept_position(position, error):
x = np.array([position.x, position.y, position.z
]) / 1000.0
self.driver.publish_pose(x)
if (self.driver.enable_raw_sensors
and interrupt.data[0] & bm.POZYX_INT_STATUS_IMU):
pozyx.getAllSensorData(sensor_data)
self.driver.publish_sensor_data(sensor_data)
rospy.sleep(self.period)
except PozyxExceptionTimeout:
pass
except PozyxException as e:
rospy.logerr(e.message)
pozyx.ser.reset_output_buffer()
pozyx.ser.reset_input_buffer()
rospy.sleep(1)
def run(self):
position = px.Coordinates()
error = px.PositionError()
sensor_data = px.SensorData()
pozyx = self.driver.pozyx
interrupt = px.SingleRegister()
while not rospy.is_shutdown():
try:
if pozyx.checkForFlag(self.flag, self.period, interrupt):
if self.driver.enable_position and interrupt.data[0] & bm.POZYX_INT_STATUS_POS:
pozyx.getCoordinates(position)
pozyx.getPositionError(error)
if self.driver.accept_position(position, error):
x = np.array([position.x, position.y, position.z]) / 1000.0
self.driver.publish_pose(x, error=error)
if (self.driver.enable_raw_sensors and
interrupt.data[0] & bm.POZYX_INT_STATUS_IMU):
pozyx.getAllSensorData(sensor_data)
self.driver.publish_sensor_data(sensor_data)
# except PozyxExceptionTimeout as e:
# rospy.logwarn('%s: %s', e.__class__.__name__, e.message)
except PozyxException as e:
log_pozyx_exception(e)
# pozyx.ser.reset_output_buffer()
# pozyx.ser.reset_input_buffer()
# rospy.sleep(1)
self.driver.reset()
finally:
rospy.sleep(self.period * 0.5)
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub_0x6940')
r = rospy.Rate(10)
try:
#pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
pozyx = pypozyx.PozyxSerial('/dev/ttyACM0')
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
# temporary fix for the issue
pozyx.checkForFlag(pypozyx.definitions.bitmasks.POZYX_INT_MASK_RX_DATA,
pypozyx.POZYX_DELAY_INTERRUPT)
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS, str(destination_id))
rospy.loginfo(device_range)
r.sleep()
else:
error_code = pypozyx.SingleRegister()
pozyx.getErrorCode(error_code)
rospy.loginfo('ERROR: RANGING, error code %s' % error_code)
if str(error_code) in '0x1':
rospy.loginfo('ERROR: RANGING, killing publisher')
return
def expose_pozyx_config(self):
sr = px.SingleRegister()
# Changed: Not available in firware 1.1 (at least in the corresp. python lib)
# self.pozyx.setOperationMode(0, self.remote_id) # work as a tag
self.pozyx.getWhoAmI(sr, self.remote_id)
_id = '0x%.2x' % sr.data[0]
rospy.set_param('~id', _id)
self.pozyx.getFirmwareVersion(sr, self.remote_id)
rospy.set_param('~firmware', register2version(sr))
self.pozyx.getHardwareVersion(sr, self.remote_id)
rospy.set_param('~hardware', register2version(sr))
ni = px.NetworkID()
self.pozyx.getNetworkId(ni)
rospy.set_param('~uwb/network_id', str(ni))
s = px.UWBSettings()
self.pozyx.getUWBSettings(s, self.remote_id)
rospy.set_param('~uwb/channel', s.channel)
rospy.set_param('~uwb/bitrate', s.parse_bitrate())
rospy.set_param('~uwb/prf', s.parse_prf())
rospy.set_param('~uwb/plen', s.parse_plen())
rospy.set_param('~uwb/gain', s.gain_db)
self.pozyx.getOperationMode(sr, self.remote_id)
rospy.set_param('~uwb/mode', 'anchor' if (sr.data[0] & 1) == 1 else 'tag')
self.pozyx.getSensorMode(sr, self.remote_id)
rospy.set_param('~sensor_mode', sensor_mode(sr))
self.self_test()
def start(self):
try:
print(pypozyx.get_serial_ports()[1].device)
self.pozyx = pypozyx.PozyxSerial(
pypozyx.get_serial_ports()[1].device)
whoami = pypozyx.SingleRegister()
self.pozyx.getWhoAmI(whoami)
rospy.loginfo(
"Pozyx WhoAmI {0}".format(whoami)) # will return 0x43
# self.setNewId(0xA003, None)
# rospy.sleep(1)
networkid = pypozyx.NetworkID()
self.pozyx.getNetworkId(networkid)
rospy.loginfo("Network ID {0}".format(networkid))
self.pozyx.setPositionFilter(
pypozyx.PozyxConstants.FILTER_TYPE_NONE, 0)
# self.pozyx.setPositionFilter(pypozyx.PozyxConstants.FILTER_TYPE_MOVING_MEDIAN, 10)
# self.setAnchorsManual(save_to_flash=False)
self.setAnchorsManual(save_to_flash=True)
self.pozyx.printDeviceInfo()
self.printPublishConfigurationResultMore(None)
except rospy.ROSException as e:
rospy.loginfo("Pozyx not connected: {0}", e)
return
self.read_thread = Thread(target=self.read, args=())
self.read_thread.daemon = True
self.read_thread.start()
rospy.sleep(3) # wait to get stable pozyx position
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_self_test(self):
mask = {5: 'UWB',
4: 'PRESSURE',
3: 'IMU',
2: 'GYRO',
1: 'MAGNETOMETER',
0: 'ACCELEROMETER'}
sr = px.SingleRegister()
self.pozyx.getSelftest(sr, self.remote_id)
v = sr.data[0]
return {s: ((1 << i) & v) != 0 for i, s in mask.items()}
def check_sensors_calibration(self):
try:
sr = px.SingleRegister()
self.pozyx.getCalibrationStatus(sr)
except PozyxException as e:
log_pozyx_exception(e)
self.reset_or_exit()
return
v = sr.data[0]
# print('calib {0:b} {0}'.format(v))
mask = {3: 'SYS', 2: 'GYRO', 1: 'ACCELEROMETER', 0: 'MAGNETOMETER'}
return {s: (((3 << (2 * i)) & v) >> (2 * i)) == 3 for i, s in mask.items()}
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 _check_status(self, operation_name, status):
# checks the status of the operation
if status == pypozyx.POZYX_SUCCESS:
return
if status == pypozyx.POZYX_TIMEOUT:
raise Exception(operation_name + " timed out")
error_code = pypozyx.SingleRegister()
stat = self.local.getErrorCode(error_code)
if stat != pypozyx.POZYX_SUCCESS:
raise Exception("failed to get error code")
code = error_code[0]
raise Exception(getErrorMessage(operation_name, code))
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...")
def __init__(self, device):
rospy.init_node('pozyx_driver', anonymous=True)
updater = diagnostic_updater.Updater()
self.ps = deque(maxlen=20)
self.es = deque(maxlen=20)
self.remote_id = rospy.get_param('~remote_id', None)
self.base_frame_id = rospy.get_param('~base_frame_id', 'base_link')
debug = rospy.get_param('~debug', False)
self.enable_orientation = rospy.get_param('~enable_orientation', True)
self.enable_position = rospy.get_param('~enable_position', True)
self.enable_raw_sensors = rospy.get_param('~enable_sensors', True)
las = rospy.get_param("~linear_acceleration_stddev", 0.0)
avs = rospy.get_param("~angular_velocity_stddev", 0.0)
mfs = rospy.get_param("~magnetic_field_stddev", 0.0)
os = rospy.get_param("~orientation_stddev", 0.0)
ps = rospy.get_param('~position_stddev', 0.0)
self.pose_cov = cov([ps] * 3 + [os] * 3)
self.orientation_cov = cov([os] * 3)
self.angular_velocity_cov = cov([avs] * 3)
self.magnetic_field_cov = cov([mfs] * 3)
self.linear_acceleration_cov = cov([las] * 3)
_a = rospy.get_param('~algorithm', 'uwb_only')
_d = rospy.get_param('~dimension', '3d')
rate = rospy.get_param('~rate', 1.0) # Hz
self.algorithm = _algorithms.get(_a, px.POZYX_POS_ALG_UWB_ONLY)
self.dimension = _dimensions.get(_d, px.POZYX_3D)
baudrate = rospy.get_param('~baudrate', 115200)
# height of device, required in 2.5D positioning
self.height = rospy.get_param('~height', 0.0) # mm
p = None
while not p and not rospy.is_shutdown():
try:
p = px.PozyxSerial(device,
baudrate=baudrate,
print_output=debug)
except SystemExit:
rospy.sleep(1)
if not p:
return
self.pozyx = PozyxProxy(p, self.remote_id)
self.pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1)
self.pose_cov_pub = rospy.Publisher('pose_cov',
PoseWithCovarianceStamped,
queue_size=1)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.mag_pub = rospy.Publisher('mag', MagneticField, queue_size=1)
self.frame_id = rospy.get_param('~anchors/frame_id')
anchors = []
if rospy.has_param('~anchors/positions'):
anchors_data = rospy.get_param('~anchors/positions')
anchors = [
px.DeviceCoordinates(dev_id, 1, px.Coordinates(x, y, z))
for [dev_id, x, y, z] in anchors_data
]
self.check_visible_devices()
rospy.sleep(0.1)
self.set_anchors(anchors)
rospy.sleep(0.1)
self.check_config(anchors)
rospy.sleep(0.1)
self.tfBuffer = tf2_ros.Buffer()
self.tf = tf2_ros.TransformListener(self.tfBuffer)
if self.enable_orientation:
try:
t = self.tfBuffer.lookup_transform(self.frame_id, 'utm',
rospy.Time(),
rospy.Duration(5.0))
# r = t.transform.rotation
# self.rotation = [r.x, r.y, r.z, r.w]
self.rotation = quaternion_from_euler(0, 0, 0) # = r
# self.rotation = quaternion_multiply(r, self.rotation)
rospy.loginfo('rotation map-> pozyx %s', self.rotation)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr('Can not tranform from utm to map frame')
self.enable_orientation = False
sr = px.SingleRegister()
self.pozyx.setOperationMode(0, self.remote_id) # work as a tag
self.pozyx.getWhoAmI(sr, self.remote_id)
_id = '0x%.2x' % sr.data[0]
rospy.set_param('~id', _id)
updater.setHardwareID('Pozyx %s' % _id)
self.pozyx.getFirmwareVersion(sr, self.remote_id)
rospy.set_param('~firmware', register2version(sr))
self.pozyx.getHardwareVersion(sr, self.remote_id)
rospy.set_param('~hardware', register2version(sr))
ni = px.NetworkID()
self.pozyx.getNetworkId(ni)
rospy.set_param('~uwb/network_id', str(ni))
s = px.UWBSettings()
self.pozyx.getUWBSettings(s, self.remote_id)
rospy.set_param('~uwb/channel', s.channel)
rospy.set_param('~uwb/bitrate', s.parse_bitrate())
rospy.set_param('~uwb/prf', s.parse_prf())
rospy.set_param('~uwb/plen', s.parse_plen())
rospy.set_param('~uwb/gain', s.gain_db)
self.pozyx.getOperationMode(sr, self.remote_id)
rospy.set_param('~uwb/mode', 'anchor' if
(sr.data[0] & 1) == 1 else 'tag')
self.pozyx.getSensorMode(sr, self.remote_id)
rospy.set_param('~sensor_mode', sensor_mode(sr))
self_test = self.check_self_test()
if not all(self_test.values()):
rospy.logwarn('Failed Self Test %s', self_test)
else:
rospy.loginfo('Passed Self Test %s', self_test)
freq_bounds = {'min': rate * 0.8, 'max': rate * 1.2}
freq_param = diagnostic_updater.FrequencyStatusParam(
freq_bounds, 0.1, 10)
stamp_param = diagnostic_updater.TimeStampStatusParam()
self.pose_pub_stat = diagnostic_updater.DiagnosedPublisher(
self.pose_pub, updater, freq_param, stamp_param)
updater.add("Sensor calibration", self.update_sensor_diagnostic)
updater.add("Localization", self.update_localization_diagnostic)
rospy.on_shutdown(self.cleanup)
continuous = rospy.get_param('~continuous', False)
rospy.Timer(rospy.Duration(1), lambda evt: updater.update())
rospy.Subscriber('set_anchors', Anchors, self.has_updated_anchors)
if continuous:
if self.enable_position:
ms = int(1000.0 / rate)
# self.pozyx.setUpdateInterval(200)
msr = px.SingleRegister(ms, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr,
self.remote_id)
self.pozyx.setPositionAlgorithm(self.algorithm, self.dimension)
rospy.sleep(0.1)
else:
msr = px.SingleRegister(0, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr,
self.remote_id)
if self.enable_position or self.enable_raw_sensors:
Updater(self, ms * 0.001)
# PositionUpdater(self, ms / 1000.0)
# if self.enable_raw_sensors:
# IMUUpdater(self)
# # position = px.Coordinates()
# # while not rospy.is_shutdown():
# # try:
# # self.pozyx.checkForFlag(bm.POZYX_INT_STATUS_POS, 0.2)
# # self.pozyx.getCoordinates(position)
# # x = np.array([position.x, position.y, position.z])/1000.0
# # self.publish_pose(x)
# # except PozyxException as e:
# # rospy.logerr(e.message)
# # rospy.sleep(1)
rospy.spin()
else:
rospy.Timer(rospy.Duration(1 / rate), self.update)
rospy.spin()
