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)