def set_uwb_settings(pozyx, set_all=True, **kwargs):
new_uwb_settings = pypozyx.UWBSettings(**kwargs)
gain_db = kwargs.get('gain', 15.0)
devices_met = []
uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F]
rospy.loginfo("Start to configure with UWB settings: %s" % str(kwargs))
if set_all:
rospy.loginfo('Will scan for all remote devices and configure them')
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:
rospy.loginfo(
"Looking for devices on channel %d, bitrate 0x%x, "
"prf 0x%x, plen 0x%x", channel, bitrate, prf, plen)
uwb_settings = pypozyx.UWBSettings(
channel, bitrate, prf, plen, gain_db)
pozyx.clearDevices()
pozyx.setUWBSettings(uwb_settings)
for device in get_devices(pozyx):
if device not in devices_met:
pozyx.setUWBSettings(new_uwb_settings, device)
pozyx.saveRegisters(uwb_registers, device)
rospy.loginfo('Device with ID 0x%0.4x set',
device)
devices_met.append(device)
rospy.loginfo("Scanning end. Configured remote devices %s",
["0x%x" % d for d in devices_met])
pozyx.setUWBSettings(new_uwb_settings)
pozyx.saveRegisters(uwb_registers)
rospy.loginfo("Local device set. Configuration done.")
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 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...")
This should be run once every time you change settings etc but no
more than that, as it saves to flash. It's a ROS node that runs
until done then dies, so running this until it completes and then
starting the other nodes is valid.
PS: Don't forget the anchor configuration.
"""
import pypozyx
import rospy
# device_ids = [0x0001, 0x0002, 0x0003, 0x0004]
device_ids = [0x6902, 0x6E44, 0x6E7A, 0x6E6C] # our device IDs [0 1 2 3]
new_uwb_settings = pypozyx.UWBSettings(
channel=2, bitrate=2, prf=2, plen=0x04, gain_db=15.0)
gain_db = 15.0
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
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()
