class Tag:
def __init__(self, anchors):
self.serial = PozyxSerial(self.getSerialport)
self.anchors = anchors
# position calculation algorithm and tracking dimension
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
def setup(self):
# sets up the Pozyx for positioning by calibrating its anchor list
print("")
print("POZYX POSITIONING Version {}".format(version))
print("-------------------------------------------------------")
print("")
print("- System will manually configure tag")
print("")
print("- System will auto start positioning")
print("")
print("-------------------------------------------------------")
print("")
self.setAnchors()
self.printConfig()
print("")
print("-------------------------------------------------------")
print("")
def setAnchors(self):
# adds the manually measured anchors to the Pozyx's device list one for one
status = self.serial.clearDevices(remote_id=None)
for anchor in self.anchors:
status &= self.serial.addDevice(anchor, remote_id=None)
if len(self.anchors) > 4:
status &= self.serial.setSelectionOfAnchors(PozyxConstants.ANCHOR_SELECT_AUTO,
len(self.anchors),
remote_id=None)
@property
def getSerialport(self):
# serialport connection test
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
return None
else:
return serial_port
def getPosition(self):
# performs positioning and exports the results
position = Coordinates()
try:
status = self.serial.doPositioning(position, self.dimension, self.algorithm, remote_id=None)
if status == POZYX_SUCCESS:
# print("POZYX data:", position)
return position
else:
self.printError("positioning")
except:
self.printError("positioning")
return None
def getOrientation(self):
# reads euler angles (yaw, roll, pitch) and exports the results
orientation = EulerAngles()
status = self.serial.getEulerAngles_deg(orientation)
if status == POZYX_SUCCESS:
# print("POZYX data:", orientation)
return orientation
else:
print("Sensor data not found")
return None
@classmethod
def mockedPosition(cls):
# return Coordinates(random.randint(0, 1000), random.randint(0, 1000), random.randint(0, 1000))
return Coordinates(random.randint(0, 2000), random.randint(0, 2000), random.randint(0, 2000))
@classmethod
def mockedOrientation(cls):
return EulerAngles(random.randint(0, 30), random.randint(0, 30), random.randint(0, 30))
def printConfig(self):
# prints the anchor configuration result
list_size = SingleRegister()
# prints the anchors list size
self.serial.getDeviceListSize(list_size, None)
if list_size[0] != len(self.anchors):
self.printError("configuration")
return
# prints the anchors list
device_list = DeviceList(list_size=list_size[0])
self.serial.getDeviceIds(device_list, None)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.serial.getDeviceCoordinates(device_list[i], anchor_coordinates, None)
print("ANCHOR: 0x%0.4x, %s" % (device_list[i], str(anchor_coordinates)))
sleep(0.025)
def printError(self, operation):
# Prints Pozyx's errors
error_code = SingleRegister()
if None is None:
self.serial.getErrorCode(error_code)
print("LOCAL ERROR %s, %s" % (operation, self.serial.getErrorMessage(error_code)))
return
status = self.serial.getErrorCode(error_code, None)
if status == POZYX_SUCCESS:
print("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % None, self.serial.getErrorMessage(error_code)))
else:
self.serial.getErrorCode(error_code)
print("ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s" %
(operation, self.serial.getErrorMessage(error_code)))
class Controller(object):
"""description of class"""
def CheckStatus(self, status):
if status == PozyxConstants.STATUS_FAILURE:
raise ConnectionError('Pozyx function failed')
if status == PozyxConstants.STATUS_TIMEOUT:
raise ConnectionError('Pozyx function timed out')
def __init__(self, anchors):
self.anchors = [DeviceCoordinates()]
self.anchors = anchors
self.port = get_first_pozyx_serial_port()
if self.port is None:
self.error = "No Pozyx connected"
return
self.pozyx = PozyxSerial(self.port)
networkId = NetworkID()
status = self.pozyx.getNetworkId(networkId)
self.id = networkId.id
self.CheckStatus(status)
self.ConfigureAnchor(self.id)
def ConfigureAnchor(self, anchorId):
for devCoords in self.anchors:
if devCoords.network_id == anchorId:
position = devCoords.pos
break
if anchorId == self.id:
anchorId = None
status = self.pozyx.setCoordinates(position, anchorId)
self.CheckStatus(status)
def ConfigureTag(self, tagId):
for anchor in self.anchors:
status = self.pozyx.addDevice(anchor, tagId)
self.CheckStatus(status)
status = self.pozyx.configureAnchors(self.anchors,
PozyxConstants.ANCHOR_SELECT_AUTO,
tagId)
self.CheckStatus(status)
mode = SingleRegister(PozyxConstants.GPIO_DIGITAL_INPUT)
pullup = SingleRegister(PozyxConstants.GPIO_PULL_UP)
status = self.pozyx.setConfigGPIO(1, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(2, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(3, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(4, mode, pullup, tagId)
self.CheckStatus(status)
def ConfigureUWB(self,
id,
txPower=33.0,
channel=1,
bitrate=0,
gain=67,
prf=2,
preamble=40):
if txPower < 0.0 or txPower > 33.0:
raise ValueError("Invalid txPower")
if id == self.id:
id = None
settings = UWBSettings()
settings.bitrate = bitrate
settings.channel = channel
settings.gain_db = gain
settings.plen = preamble
settings.prf = prf
status = self.pozyx.setUWBSettings(settings, id)
self.CheckStatus(status)
status = self.pozyx.setTxPower(txPower, id)
self.CheckStatus(status)
def GetError(self):
code = SingleRegister()
status = self.pozyx.getErrorCode(code)
self.CheckStatus(status)
return self.pozyx.getErrorMessage(code)
def GetCalibrationStatus(self, tagId):
result = SingleRegister()
status = self.pozyx.getCalibrationStatus(result, tagId)
self.CheckStatus(status)
value = dict()
value['magnetic'] = result.value & 3
value['accelerometer'] = (result.value & 12) >> 2
value['gyroscope'] = (result.value & 48) >> 4
value['system'] = (result.value & 192) >> 6
return value
def GetGPIO(self, tagId, pinId):
value = SingleRegister()
status = self.pozyx.getGPIO(pinId, value, tagId)
self.CheckStatus(status)
return value.value == 1
def GetPosition(self, tagId):
target = Coordinates()
status = self.pozyx.doPositioning(
target,
PozyxConstants.DIMENSION_3D,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
remote_id=tagId)
self.CheckStatus(status)
result = dict()
result['x'] = target.x
result['y'] = target.y
result['z'] = target.z
return target
def GetEulerAngles(self, tagId):
result = EulerAngles()
status = self.pozyx.getEulerAngles_deg(result, tagId)
self.CheckStatus(status)
res = dict()
res['heading'] = result.heading
res['roll'] = result.roll
res['pitch'] = result.pitch
return res
def GetPressure(self, tagId):
data = Pressure()
status = self.pozyx.getPressure_Pa(data, tagId)
self.CheckStatus(status)
return {'pressure': data.value}
def GetTemperature(self, tagId):
data = Temperature()
status = self.pozyx.getTemperature_c(data, tagId)
self.CheckStatus(status)
return {'temperature': data.value}
def DiscoverTags(self):
status = self.pozyx.doDiscovery(PozyxConstants.DISCOVERY_TAGS_ONLY)
self.CheckStatus(status)
count = SingleRegister()
status = self.pozyx.getDeviceListSize(count)
self.CheckStatus(status)
allIds = DeviceList(list_size=count.value)
status = self.pozyx.getDeviceIds(allIds)
self.CheckStatus(status)
tagIds = list()
for id in allIds:
any = False
for tagId in self.anchors:
if tagId.network_id == id:
any = True
if not any:
tagIds.append(id)
return tagIds
class RangeDebugger(Node):
def __init__(self):
super().__init__("range_debugger")
self.range_pub = self.create_publisher(String, "range", 10)
self.position_pub = self.create_publisher(Odometry, "odometry/pozyx",
1000)
self.markers_pub = self.create_publisher(MarkerArray,
"odometry/pozyx/markers", 10)
# serial port setting
serial_port = "/dev/ttyACM0"
seiral_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. CHeck your USB cable or your driver!")
quit()
self.pozyx = PozyxSerial(serial_port)
# remote and destination
# But sorry, just 1 tag is useable.
# "None" is setting for use USB-connected tag, "0xXX"(tag id) is to use remote tag.
self.tag_ids = [None] # TODO: To use multiple tags
self.ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
self.range_timer_ = self.create_timer(0.02, self.range_callback)
self.anchors = [
# DeviceCoordinates(0x605b, 1, Coordinates( 0, 0, 0)), # test
# DeviceCoordinates(0x603b, 1, Coordinates( 800, 0, 0)), # test
DeviceCoordinates(0x6023, 1, Coordinates(-13563, -8838,
475)), # ROOM
DeviceCoordinates(0x6e23, 1, Coordinates(-3327, -8849,
475)), # ROOM
DeviceCoordinates(0x6e49, 1, Coordinates(-3077, -2959,
475)), # ROOM
# DeviceCoordinates(0x6e58, 1, Coordinates( -7238, -3510, 475)), # ROOM
DeviceCoordinates(0x6050, 1, Coordinates(-9214, -9102,
475)), # ROOM
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_2D
self.height = 475
self.setup()
def setup(self):
self.setAnchorsManual()
for anchor in self.anchors:
self.get_logger().info("ANCHOR,0x%0.4x,%s" %
(anchor.network_id, str(anchor.pos)))
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
for tag_id in self.tag_ids:
status = self.pozyx.clearDevices(tag_id)
for anchor in self.anchors:
status &= self.pozyx.addDevice(anchor, tag_id)
if len(self.anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_MANUAL,
len(self.anchors),
remote_id=tag_id)
self.printPublishConfigurationResult(status, tag_id)
def printPublishConfigurationResult(self, status, tag_id):
"""Prints the configuration explicit result, prints and publishes error if one occurs"""
if tag_id is None:
tag_id = 0
if status == POZYX_SUCCESS:
self.get_logger().info("Configuration of tag %s: success" % tag_id)
else:
self.printPublishErrorCode("configuration", tag_id)
def printPublishErrorCode(self, operation, network_id):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if status == POZYX_SUCCESS:
self.get_logger().error("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
self.get_logger().error("Error % s, local error code %s" %
(operation, str(error_code)))
def range_callback(self):
"""Do ranging periodically, and publish visualizasion_msg MarkerArray"""
for tag_id in self.tag_ids:
for anchor in self.anchors:
device_range = DeviceRange()
status = self.pozyx.doRanging(anchor.network_id, device_range,
tag_id)
if status == POZYX_SUCCESS:
self.publishMarkerArray(device_range.distance, anchor)
# self.get_logger().info(f"{device_range.distance}")
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
self.get_logger().error(
"ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
self.get_logger().error(
"ERROR Ranging, couldn't retrieve local error")
self.doPositioning()
def doPositioning(self):
for tag_id in self.tag_ids:
position = Coordinates()
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
quat = Quaternion()
status &= self.pozyx.getNormalizedQuaternion(quat, tag_id)
if status == POZYX_SUCCESS:
self.printPublishPosition(position, tag_id, quat)
else:
self.printPublishErrorCode("positioning", tag_id)
def printPublishPosition(self, position, network_id, quat):
if network_id is None:
network_id = 0
odom = Odometry()
odom.header.stamp = self.get_clock().now().to_msg()
odom.header.frame_id = "pozyx"
odom.pose.pose.position.x = position.x * 0.001
odom.pose.pose.position.y = position.y * 0.001
if self.dimension == PozyxConstants.DIMENSION_3D:
odom.pose.pose.position.z = position.z * 0.001
else:
odom.pose.pose.position.z = float(self.height) / 1000
odom.pose.pose.orientation.x = quat.x
odom.pose.pose.orientation.y = quat.y
odom.pose.pose.orientation.z = quat.z
odom.pose.pose.orientation.w = quat.w
odom.pose.covariance = [
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.03,
]
self.position_pub.publish(odom)
def publishMarkerArray(self, distance, anchor):
"""
Visualization pozyx anchors for Rviz2
Parameters
----------
distance : float
"""
is_3D_estimation = (self.dimension == PozyxConstants.DIMENSION_3D)
pos_x = anchor.pos.x
pos_y = anchor.pos.y
pos_z = anchor.pos.z
markerArray = MarkerArray()
m_id = anchor.network_id
# marker of pozyx pos
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_pos" # namespace
marker_pos.id = m_id
m_id += 1
marker_pos.type = Marker.CUBE
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = 0.07
marker_pos.scale.y = 0.07
marker_pos.scale.z = 0.02
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 0.0
marker_pos.color.g = 1.0
marker_pos.color.b = 0.0
marker_pos.color.a = 1.0
markerArray.markers.append(marker_pos)
# marker of pozyx distance
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_distance" # namespace
marker_pos.id = m_id
m_id += 1
if is_3D_estimation:
marker_pos.type = Marker.SPHERE
marker_pos.scale.z = float(distance) * 2 / 1000
else:
marker_pos.type = Marker.CYLINDER
marker_pos.scale.z = 0.001
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = float(distance) * 2 / 1000
marker_pos.scale.y = float(distance) * 2 / 1000
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 0.0
marker_pos.color.g = 0.5
marker_pos.color.b = 0.5
marker_pos.color.a = 0.1
markerArray.markers.append(marker_pos)
# marker of pozyx label
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_distance_label" # namespace
marker_pos.id = m_id
m_id += 1
marker_pos.type = Marker.TEXT_VIEW_FACING
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = 1.0
marker_pos.scale.y = 1.0
marker_pos.scale.z = 0.2
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000 + 0.5
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 1.0
marker_pos.color.g = 1.0
marker_pos.color.b = 1.0
marker_pos.color.a = 1.0
marker_pos.text = f"{float(distance / 1000):.2f}\n{hex(anchor.network_id)}"
markerArray.markers.append(marker_pos)
# Publish markers!
self.markers_pub.publish(markerArray)
