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 has_updated_anchors(self, msg):
self.frame_id = msg.header.frame_id
anchors = [
px.DeviceCoordinates(
anchor.id, 1,
px.Coordinates(anchor.position.x, anchor.position.y,
anchor.position.z)) for anchor in msg.anchors
]
self.pozyx.lock.acquire(True)
try:
self.set_anchors(anchors)
self.check_config(anchors)
except PozyxException as e:
rospy.logerr(e.message)
finally:
self.pozyx.lock.release()
def has_updated_anchors(self, msg):
if not self.ready:
return
self.frame_id = msg.header.frame_id
anchors = [
px.DeviceCoordinates(anchor.id, 1, px.Coordinates(
anchor.position.x * 1000.0, anchor.position.y * 1000.0,
anchor.position.z * 1000.0))
for anchor in msg.anchors]
self.pozyx.lock.acquire(True)
try:
self.set_anchors(anchors)
self.check_config(anchors)
except PozyxException as e:
log_pozyx_exception(e)
self.reset_or_exit()
finally:
self.pozyx.lock.release()
def init():
global anchors, tags_id
rospy.init_node('pozyx_configure')
rospy.loginfo("Configuring device list.")
tags_param = rospy.get_param("tags/addresses", [])
tags_id = [None] + tags_param
anchors_param = rospy.get_param("anchors", [])
anchors = []
rospy.loginfo(str(anchors_param))
rospy.loginfo(str(tags_param))
for anchor in anchors_param:
rospy.loginfo(str(anchor))
rospy.loginfo(str(anchor["address"]))
anchors.append(
pypozyx.DeviceCoordinates(
anchor["address"], anchor["flag"],
pypozyx.Coordinates(anchor["position"][0],
anchor["position"][1],
anchor["position"][2])))
This is intended to be highly customisable, but will also be made with parameters and a launch
file in the future. Again, help/suggestions/feedback on these things are highly appreciated.
Run this after running the UWB configuration.
Automatic calibration at this point in time is highly discouraged.
"""
import pypozyx
import rospy
from serial.tools.list_ports import comports
# adding None will cause the local device to be configured for the anchors as well.
tag_ids = [None, 0x0001, 0x0002, 0x0003, 0x0004]
anchors = [
pypozyx.DeviceCoordinates(0x0001, 1, pypozyx.Coordinates(0, 0, 5000)),
pypozyx.DeviceCoordinates(0x0002, 1, pypozyx.Coordinates(5000, 0, 1000)),
pypozyx.DeviceCoordinates(0x0003, 1, pypozyx.Coordinates(0, 5000, 1000)),
pypozyx.DeviceCoordinates(0x0004, 1, pypozyx.Coordinates(5000, 5000, 1000))
]
def set_anchor_configuration():
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")
self.smoothData(pos)
self.tb.sendTransform(self.pos,
self.quat,
rospy.Time.now(),
"pozyx",
"map"
)
if __name__ == '__main__':
rospy.init_node('pozyx_pose_node')
port = '/dev/ttyACM1'
#port = pypozyx.get_serial_ports()[0].device
# necessary data for calibration, change the IDs and coordinates yourself
anchors = [pypozyx.DeviceCoordinates(0x6e49, 1, pypozyx.Coordinates(0, 0, 590)),
pypozyx.DeviceCoordinates(0x6e57, 1, pypozyx.Coordinates(2900, 400, 720)),
pypozyx.DeviceCoordinates(0x6e32, 1, pypozyx.Coordinates(100, 3000, 1210)),
pypozyx.DeviceCoordinates(0x6e0f, 1, pypozyx.Coordinates(5500, 3200, 910))]
algorithm = pypozyx.POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = pypozyx.POZYX_3D # positioning dimension
height = 100 # height of device, required in 2.5D positioning
pozyx = pypozyx.PozyxSerial(port)
remote_id = None #"0x6e3a"
r = ROSPozyx(pozyx, anchors, algorithm, dimension, height,remote_id)
try:
r.pozyx_pose_pub()
except rospy.ROSInterruptException:
pass
This is intended to be highly customisable, but will also be made with parameters and a launch
file in the future. Again, help/suggestions/feedback on these things are highly appreciated.
Run this after running the UWB configuration.
Automatic calibration at this point in time is highly discouraged.
"""
import pypozyx
import rospy
# adding None will cause the local device to be configured for the anchors as well.
#tag_ids = [None, 0x6955, 0x6940, 0x0003, 0x0004]
#tag_ids = [None,0x6955, 0x6940, 0x6937, 0x6935]
tag_ids = [None]
anchors = [
pypozyx.DeviceCoordinates(0x6955, 1, pypozyx.Coordinates(0, 0, 1000)),
pypozyx.DeviceCoordinates(0x6940, 1, pypozyx.Coordinates(5000, 0, 1000)),
pypozyx.DeviceCoordinates(0x6937, 1, pypozyx.Coordinates(0, 5000, 1000)),
pypozyx.DeviceCoordinates(0x6935, 1, pypozyx.Coordinates(5000, 5000, 1000))
]
def set_anchor_configuration():
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
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()
#!/usr/bin/env python
"""
Performs discovery for all tags in range and then configures the positioning
anchor list on all the discovered devices.
This requires all devices to be on the same UWB settings first, so I highly
recommend to run the uwb_configurator node first.
"""
import pypozyx
import rospy
# anchors IDs for 0,1,2,3 [0x6902, 0x6E44, 0x6E7A, 0x6E6C]
# x y z
anchors = [
pypozyx.DeviceCoordinates(0x6902, 1, pypozyx.Coordinates(0, 0, 0)), # o
pypozyx.DeviceCoordinates(0x6E44, 1, pypozyx.Coordinates(4340, -321,
0)), # x
pypozyx.DeviceCoordinates(0x6E7A, 1, pypozyx.Coordinates(0, 9440, 0)), # y
pypozyx.DeviceCoordinates(0x6E6C, 1, pypozyx.Coordinates(4618, 9334, 1028))
] # z
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:
# MOD @ 12-08-2020
for device_id in self.anchors:
try:
device_range = self.cluster.rangeTo(device_id, self.tag)
h = Header()
h.stamp = rospy.Time.from_sec(device_range[0] / 1000)
self.range_pub.publish(h, device_id, device_range[1],
device_range[2])
except Exception as e:
rospy.logerr(e)
if __name__ == "__main__":
anchors = [
pypozyx.DeviceCoordinates(0x971a, 0,
pypozyx.Coordinates(0.05, -0.035, 0.03)),
pypozyx.DeviceCoordinates(0x972d, 0,
pypozyx.Coordinates(0.05, -0.525, 0.03)),
pypozyx.DeviceCoordinates(0x9751, 0,
pypozyx.Coordinates(-0.02, -0.035, -0.05)),
pypozyx.DeviceCoordinates(0x9733, 0,
pypozyx.Coordinates(1.5, -0.035, 0.03))
]
tag = pypozyx.DeviceCoordinates(0x671d, 0, pypozyx.Coordinates(0, 0, 0))
stationaryAnchors = StationaryAnchors(anchors, tag)
try:
stationaryAnchors.start_node()
except rospy.ROSInterruptException:
