def set_anchor_configuration():
global anchors, tags_id
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
serial_port = rospy.get_param("port",
pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Pozyx not connected")
return
for tag in tags_id:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.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:
rospy.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
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(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=pypozyx.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(pypozyx.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 pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
pubStamped = rospy.Publisher('pozyx_pose_stamped',
PoseStamped,
queue_size=40)
rospy.init_node('pozyx_pose_node')
h = std_msgs.msg.Header()
h.frame_id = "pozyx"
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
h.stamp = rospy.Time.now()
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
pubStamped.publish(
h,
Pose(
Point(
float(coords.x) / 1000,
float(coords.y) / 1000,
float(coords.z) / 1000),
Quaternion(quat.x, quat.y, quat.z, quat.w)))
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(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=pypozyx.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(pypozyx.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 pozyx_positioning_pub():
pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100)
rospy.init_node('positioning_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
rospy.loginfo(coords)
pub.publish(Point32(coords.x, coords.y, coords.z))
def pozyx_euler_pub():
pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100)
rospy.init_node('pozyx_euler_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id)
rospy.loginfo(euler_angles)
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
def pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
rospy.init_node('pozyx_pose_node')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(device_range)
else:
rospy.loginfo('ERROR: RANGING')
def range_publisher():
global msg, device_range, range_pub
try:
range_pub = rospy.Publisher('pozyx/range', Range, queue_size=1)
serial_port = rospy.get_param("port",
pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
remote = rospy.get_param("remote", None) # From where, None is local
destination = rospy.get_param("destination", 0x6042) # To where
except:
rospy.loginfo("Serial device Pozyx not found")
return
while not rospy.is_shutdown():
# doRanging : Destination, Device_range, Remote
local_device.doRanging(destination, device_range, remote)
msg.range = device_range.distance
range_pub.publish(msg)
def pozyx_position_euler_pub():
position_pub = rospy.Publisher(
'pozyx_positioning', Point32, queue_size=100)
euler_pub = rospy.Publisher(
'pozyx_euler_angles', EulerAngles, queue_size=100)
rospy.init_node('position_euler_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
pub.publish(coords.x, coords.y, coords.z)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
pub.publish(euler_angles.heading,
euler_angles.roll, euler_angles.pitch)
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
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)
rospy.loginfo(device_range)
else:
error_code = SingleRegister()
pozyx.getErrorCode(error_code)
rospy.loginfo('ERROR: RANGING, error code %s' % error_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:
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 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:
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 pozyx_position_euler_pub():
position_pub = rospy.Publisher('pozyx_positioning',
Point32,
queue_size=100)
euler_pub = rospy.Publisger('pozyx_euler_angles',
EulerAngles,
queue_size=100)
rospy.init_node('position_euler_pub')
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
pub.publish(coords.x, coords.y, coords.z)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
def init():
global pozyx_coords, pozyx_coords_err, pozyx_sensors
global ros_pose, ros_mag, ros_imu, local_device, lock
global height, algorithm, remote, serial_port, posehz, imuhz
rospy.init_node('pozyx_pose_node')
rospy.Subscriber("pozyx/height", Int16, height_cb)
serial_port = rospy.get_param("port", pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
# 0 is raw, 1 is fused but not yet implemented, 2 is filtered
algorithm = rospy.get_param("algorithm", 2)
remote = rospy.get_param("remote", None)
height = rospy.get_param("height", 100)
posehz = rospy.Rate(rospy.get_param("posehz", 2))
imuhz = rospy.Rate(rospy.get_param("imuhz", 100))
ros_pose = PoseWithCovarianceStamped()
ros_mag = MagneticField()
ros_imu = Imu()
ros_pose.header.frame_id = "pozyx"
ros_mag.header.frame_id = "pozyx"
ros_imu.header.frame_id = "pozyx"
pozyx_coords = pypozyx.Coordinates()
pozyx_coords_err = pypozyx.PositionError()
pozyx_sensors = pypozyx.SensorData()
imu_thread = Thread(target=imu_publisher)
coord_thread = Thread(target=coordinate_publisher)
lock = Lock()
imu_thread.start()
coord_thread.start()
rospy.spin()
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")
return
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.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:
rospy.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
return ans2
# if np.abs(r4 - dist1) < np.abs(r4 - dist2):
# return ans1
# else:
# return ans2
def getCov(self):
return
#
if __name__ == "__main__":
rospy.init_node('pozyx_node_trilateration')
frequency = float(rospy.get_param('~frequency'))
dt = 1 / frequency
serial_port = get_serial_ports()[2].device
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote = False
if not remote:
remote_id = None
anchor0_id = int(rospy.get_param('~anchor0_id'), 16)
anchor1_id = int(rospy.get_param('~anchor1_id'), 16)
anchor2_id = int(rospy.get_param('~anchor2_id'), 16)
anchor3_id = int(rospy.get_param('~anchor3_id'), 16)
anchor0_coordinates = np.array(
def get_device():
devices = [d for d in px.get_serial_ports() if is_pozyx(d)]
if devices:
return devices[0].device
return None
def get_serial_port_names():
return [port.device for port in get_serial_ports()]
def get_pozyx_serial_port():
for port in get_serial_ports():
if inspect_port(port):
return port.device