def pozyx_pose_pub(port1, port2): global distance pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=40) try: pozyx1 = PozyxSerial(port1) except: rospy.loginfo("Pozyx 1 not connected") return try: pozyx2 = PozyxSerial(port2) except: rospy.loginfo("Pozyx 2 not connected") return pozyx1.setPositionFilter(filter, 3) pozyx2.setPositionFilter(filter, 3) pos1 = Coordinates() pos2 = Coordinates() pose = PoseStamped() pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() yaw = 0. pos1_old = copy.copy(pos1) pos2_old = copy.copy(pos2) yaw_old = 0. while not rospy.is_shutdown(): status1 = pozyx1.doPositioning(pos1, dimension=dimension, algorithm=algorithm) time_delta1 = (rospy.Time.now() - pose.header.stamp).to_sec() if (status1 == POZYX_SUCCESS and distance_2d(pos1, pos1_old) < time_delta1 * max_speed): status2 = pozyx2.doPositioning(pos2, dimension=dimension, algorithm=algorithm) time_delta2 = (rospy.Time.now() - pose.header.stamp).to_sec() yaw = atan2(pos2.y - pos1.y, pos2.x - pos1.x) + radians(tag_rot) if (status2 == POZYX_SUCCESS and distance_2d(pos2, pos2_old) < time_delta2 * max_speed and distance_2d(pos1, pos2) < tag_distance + 2 * pozyx_error and abs(degrees(yaw - yaw_old)) < time_delta2 * max_rot_speed): # simple out-of-range value filter pose.pose.position = Point((pos1.x + pos2.x) / 1000., (pos1.y + pos2.y) / 1000., distance) pose.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, yaw)) pose.header.stamp = rospy.Time.now() pub.publish(pose) #print distance_2d(pos1, pos1_old), time_delta1*max_speed, distance_2d(pos2, pos2_old), time_delta2*max_speed, distance_2d(pos1, pos2), tag_distance + 2*pozyx_error, yaw_old, yaw pos1_old = copy.copy(pos1) pos2_old = copy.copy(pos2) yaw_old = yaw if enable_logging: rospy.loginfo( "POS: %s, QUAT: %s" % (str(pose.pose.position), str(pose.pose.orientation)))
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
class DataSet(): def __init__(self): self.pozyx = PozyxSerial(get_first_pozyx_serial_port()) self.direct = EulerAngles() self.position = Coordinates() def get_angle(self, req): if req.sigin == True: self.pozyx.getEulerAngles_deg(self.direct, remote_id=None) return int(self.direct.heading)
def doSomething(): port = get_first_pozyx_serial_port() print('Port:', port) p = PozyxSerial(port) whoami = SingleRegister() p.regRead(POZYX_WHO_AM_I, whoami) print('WhoAmI:', whoami) return port, whoami.data
def __init__(self, anchors): self.serial_port = get_first_pozyx_serial_port() print(self.serial_port) if self.serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") self.pozyx = PozyxSerial(self.serial_port) #print(self.serial_port) if (anchors.get('count') == 4): self.anchors = [ DeviceCoordinates( 0x6110, 1, Coordinates( anchors.get('0x6110')[0], anchors.get('0x6110')[1], anchors.get('0x6110')[2])), DeviceCoordinates( 0x6115, 1, Coordinates( anchors.get('0x6115')[0], anchors.get('0x6115')[1], anchors.get('0x6115')[2])), DeviceCoordinates( 0x6117, 1, Coordinates( anchors.get('0x6117')[0], anchors.get('0x6117')[1], anchors.get('0x6117')[2])), DeviceCoordinates( 0x611e, 1, Coordinates( anchors.get('0x611e')[0], anchors.get('0x611e')[1], anchors.get('0x611e')[2])) ] self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY self.dimension = PozyxConstants.DIMENSION_3D self.height = 1000 #self.pub = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10) self.pub = rospy.Publisher('/pose', PoseStamped, queue_size=10) self.pose = PoseStamped() self.subZ = rospy.Subscriber('range', Int16, self.rangeCallback)
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 main(): serial_port = get_first_pozyx_serial_port() if serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") quit() remote_id = 0x1234 # remote device network ID remote = False # whether to use a remote device if not remote: remote_id = None use_processing = True # enable to send position data through OSC ip = "127.0.0.1" # IP for the OSC UDP network_port = 8888 # network port for the OSC UDP osc_udp_client = None if use_processing: osc_udp_client = SimpleUDPClient(ip, network_port) anchors = [DeviceCoordinates(0x6E2A, 1, Coordinates(0, 0, 3175)), DeviceCoordinates(0x6E0E, 1, Coordinates(0, 4114, 3175)), DeviceCoordinates(0x697F, 1, Coordinates(3429, 0, 3175)), DeviceCoordinates(0x6E6F, 1, Coordinates(3429, 4114, 3175))] algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use dimension = POZYX_3D # positioning dimension height = 1000 # height of device, required in 2.5D positioning pozyx = PozyxSerial(serial_port) r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id) r.setup() while 1: r.loop() if not GPIO.input(buttonPin): in_use() maintenance = format_time() data = {"ID": Id, "X": x, "Y": y, "InUse": inUse, "Maintenance": maintenance} firebase.post('/Ventilator', data) time.sleep(5)
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 run(self): pozyx = PozyxSerial(get_first_pozyx_serial_port()) global RID r = PozyxControl(pozyx, tag_ids, anchors, RID, algorithm, dimension, height) r.setup() while isRecording: r.loop()
def setup_poszyx(config_data): # TODO: clean out # shortcut to not have to find out the port yourself serial_port = get_serial_ports()[1].device remote_id = hex_string_to_hex(config_data.get( "remote_device")) if config_data.get("remote_device") else None # TODO: still not sure what it does # use_processing = True # enable to send position data through OSC # ip = "127.0.0.1" # IP for the OSC UDP # network_port = 8888 # network port for the OSC UDP osc_udp_client = None # if use_processing: # osc_udp_client = SimpleUDPClient(ip, network_port) anchors = [] for anchor in config_data["anchors"]: # necessary data for calibration, change the IDs and coordinates yourself device_coordinates = DeviceCoordinates( hex_string_to_hex(anchor["label"]), anchor["type"], Coordinates(anchor["coordinates"]["x-value"], anchor["coordinates"]["y-value"], anchor["coordinates"]["z-value"])) anchors.append(device_coordinates) # TODO check which algorithm is BEST algorithm = POZYX_POS_ALG_TRACKING # positioning algorithm to use dimension = POZYX_3D # positioning dimension #algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use #dimension = POZYX_2_5D # TODO check what is needed height = config_data[ "moving_device_height"] # height of device, required in 2.5D positioning # setup pozyx = PozyxSerial(serial_port) localizer_instance = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id) localizer_instance.setup() sensor_data = None if config_data.get("sensor_data"): sensor_data = Orientation3D(pozyx, osc_udp_client, remote_id) sensor_data.setup() return {"localizer": localizer_instance, "sensor_data": sensor_data}
fileName = eParser.get('default', 'file_name') exportMQTT = eParser.get('default', 'export_mqtt') host = eParser.get('default', 'mqtt_host') topic = eParser.get('default', 'mqtt_topic') clientID = eParser.get('default', 'mqtt_clientid') exportAPI = eParser.get('default', 'export_api') apiUrl = eParser.get('default', 'api_url') apiTag = eParser.get('default', 'api_tag') #Setup variables positionTag = Coordinates() #Check if connected serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) print("Connection success!") else: print("No Pozyx port was found") exit() try: if exportFile is "y": f = open("position.txt", "a+") if exportMQTT is "y": client = mqtt.Client(client_id=clientID) client.connect("broker.mqttdashboard.com") client.loop_start() while True: pozyx.doPositioning(positionTag, remote_id=r_id) publishString = hex(r_id) + ": " + str(positionTag) print(publishString)
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings, Coordinates, DeviceCoordinates # initalize the Pozyx as above serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) uwb_settings = UWBSettings() pozyx.getUWBSettings(uwb_settings) print(f'UWB Settings: {uwb_settings}') else: print("No Pozyx port was found") # assume an anchor 0x6140 that we want to add to the device list and immediately save the device list after. anchor = DeviceCoordinates(0x6140, 0, Coordinates(000, 0000, 0)) print(anchor) pozyx.addDevice(anchor) pozyx.saveNetwork() # after, we can start positioning. Positioning takes its parameters from the configuration in the tag's # registers, and so we only need the coordinates. position = Coordinates() print(f'Position before positioning: {position}') pozyx.doPositioning(position) print(f'Position after positioning: {position}') # initialize the data container who_am_i = SingleRegister() # get the data, passing along the container status = pozyx.getWhoAmI(who_am_i) # check the status to see if the read was successful. Handling failure is covered later.
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, Coordinates serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) print("Connection success!") else: print("No Pozyx port was found") position = Coordinates() pozyx.doPositioning(position) print(position) with open('/home/pi/pozyx/position.txt', 'a') as f: f.write(str(position) + "\n")
anchor1Y = int(cParser.get('default', 'anchor1_Y')) anchor1Z = int(cParser.get('default', 'anchor1_Z')) anchor2X = int(cParser.get('default', 'anchor2_X')) anchor2Y = int(cParser.get('default', 'anchor2_Y')) anchor2Z = int(cParser.get('default', 'anchor2_Z')) anchor3X = int(cParser.get('default', 'anchor3_X')) anchor3Y = int(cParser.get('default', 'anchor3_Y')) anchor3Z = int(cParser.get('default', 'anchor3_Z')) anchor4X = int(cParser.get('default', 'anchor4_X')) anchor4Y = int(cParser.get('default', 'anchor4_Y')) anchor4Z = int(cParser.get('default', 'anchor4_Z')) #Check if connected serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) print("Connection success!") else: print("No Pozyx port was found") exit() try: ##Setup anchors #Clear previous setup pozyx.clearDevices(remote_id=r_id) print("List cleared.") #Anchor variables anchor1 = DeviceCoordinates(anchor1, 1, Coordinates(anchor1X, anchor1Y, anchor1Z)) anchor2 = DeviceCoordinates(anchor2, 1,
#change the ID's for checking coordinates of another device anchor1 = int(cParser.get('default','anchor1_id'),16) anchor2 = int(cParser.get('default','anchor2_id'),16) anchor3 = int(cParser.get('default','anchor3_id'),16) anchor4 = int(cParser.get('default','anchor4_id'),16) #Setup variables coordinatesAnchor1 = Coordinates() coordinatesAnchor2 = Coordinates() coordinatesAnchor3 = Coordinates() coordinatesAnchor4 = Coordinates() #Check if connected serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) print("Connection success!") else: print("No Pozyx port was found") exit() try: #Obtain the device coordinates from the device list pozyx.getDeviceCoordinates(anchor1,coordinatesAnchor1,r_id) print(hex(anchor1) + ": " + str(coordinatesAnchor1)) pozyx.getDeviceCoordinates(anchor2,coordinatesAnchor2,r_id) print(hex(anchor2) + ": " + str(coordinatesAnchor2)) pozyx.getDeviceCoordinates(anchor3,coordinatesAnchor3,r_id) print(hex(anchor3) + ": " + str(coordinatesAnchor3)) pozyx.getDeviceCoordinates(anchor4,coordinatesAnchor4,r_id) print(hex(anchor4) + ": " + str(coordinatesAnchor4))
DeviceCoordinates(0x6951, 1, Coordinates(0, 0, 1500)), DeviceCoordinates(0x6e59, 2, Coordinates(5340, 0, 2000)), DeviceCoordinates(0x695d, 3, Coordinates(6812, -8923, 2500)), DeviceCoordinates(0x690b, 4, Coordinates(-541, -10979, 3000)), DeviceCoordinates(0x6748, 5, Coordinates(6812, -4581, 200)) ] POZYX_CONNECTED_TO_BASE = True serial_port = get_pozyx_serial_port() if serial_port is None: POZYX_CONNECTED_TO_BASE = False else: try: pozyx = PozyxSerial(serial_port) status = pozyx.clearDevices() for anchor in anchors: status &= pozyx.addDevice(anchor) remote_check = SingleRegister() pozyx.getWhoAmI(remote_check, remote_id=remote_id) if remote_check.data == [0]: remote_id = None MAX_TRIES = 1000 except PozyxConnectionError: BUSY_SERIAL = True
#import pypozyx import sys, time from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings #pozyx = PozyxLib() # PozyxSerial has PozyxLib's functions, just for generality CURSOR_UP_ONE = '\x1b[1A' ERASE_LINE = '\x1b[2K' is_cursor_up = False #print(pypozyx.get_first_pozyx_serial_port()) pozyx = PozyxSerial(get_first_pozyx_serial_port()) who_am_i = SingleRegister() # get the data, passing along the container status = pozyx.getWhoAmI(who_am_i) acceleration = Acceleration() euler_angles = EulerAngles() uwb_settings = UWBSettings() # check the status to see if the read was successful. Handling failure is covered later. if status == POZYX_SUCCESS: # print the container. Note how a SingleRegister will print as a hex string by default. print('Who Am I: {}'.format(who_am_i)) # will print '0x43' while True: # initalize the Pozyx as above # initialize the data container # and repeat # initialize the data container # get the data, passing along the container if is_cursor_up: sys.stdout.write(CURSOR_UP_ONE)
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, UWBSettings serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) uwb_settings = UWBSettings() pozyx.getUWBSettings(uwb_settings) print(uwb_settings) else: print("No Pozyx port was found")
def __init__(self, mpstate): """Initialise module""" super(BeaconToGPS, self).__init__(mpstate, "BeaconToGPS", "") self.anchor_config = None self.config_file_parser = ConfigParser.ConfigParser() self.config_file_parser.read(os.getcwd() + '/config/uwb_config.conf') self.anchor_config = self.config_file_parser.get( "Anchor", "anchor_coordinates") if self.anchor_config is None: print("Need set the anchor coordinate!") return self.yaw_deg = self.config_file_parser.getfloat( "NED", "yaw_form_ned_to_uwb") if self.yaw_deg is None: print("Need set the yaw from ned to uwb!") return else: print("NED to UWB yaw:" + str(self.yaw_deg) + " deg") self.debug = 0 self.debug = self.config_file_parser.getint("SYS", "debug") if self.debug is None: self.debug = 0 serial_port_dev = get_first_pozyx_serial_port() if serial_port_dev is None: print("No Pozyx connected. Check your USB cable or your driver!") return self.pozyx = PozyxSerial(serial_port_dev) self.anchors = self.anchor_config[1:len(self.anchor_config) - 1].split(";") self.anchor_list = [] self.position = Coordinates() self.velocity = Coordinates() self.pos_last = Coordinates() self.pos_last_time = 0 self.setup_pozyx() self.CONSTANTS_RADIUS_OF_EARTH = 6378100.0 self.DEG_TO_RAD = 0.01745329251994329576 self.RAD_TO_DEG = 57.29577951308232087679 self.reference_lat = 36.26586666666667 self.reference_lon = 120.27563333333333 self.reference_lat_rad = self.reference_lat * self.DEG_TO_RAD self.reference_lon_rad = self.reference_lon * self.DEG_TO_RAD self.cos_lat = math.cos(self.reference_lat_rad) self.target_lon_param = self.CONSTANTS_RADIUS_OF_EARTH * self.cos_lat self.current_lat = 0 self.current_lon = 0 self.tag_pos_ned = Coordinates() self.tag_velocity_ned = Coordinates() self.yaw = math.radians(self.yaw_deg) self.cos_yaw = math.cos(self.yaw) self.sin_yaw = math.sin(self.yaw) self.location_update = False self.location_update_time = 0 self.pos_update_time = 0 self.location_update_freq = 8 self.data = { 'time_usec': 0, # (uint64_t) Timestamp (micros since boot or Unix epoch) 'gps_id': 0, # (uint8_t) ID of the GPS for multiple GPS inputs 'ignore_flags': self. IGNORE_FLAG_ALL, # (uint16_t) Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided. 'time_week_ms': 0, # (uint32_t) GPS time (milliseconds from start of GPS week) 'time_week': 0, # (uint16_t) GPS week number 'fix_type': 0, # (uint8_t) 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK 'lat': 0, # (int32_t) Latitude (WGS84), in degrees * 1E7 'lon': 0, # (int32_t) Longitude (WGS84), in degrees * 1E7 'alt': 0, # (float) Altitude (AMSL, not WGS84), in m (positive for up) 'hdop': 0, # (float) GPS HDOP horizontal dilution of position in m 'vdop': 0, # (float) GPS VDOP vertical dilution of position in m 'vn': 0, # (float) GPS velocity in m/s in NORTH direction in earth-fixed NED frame 've': 0, # (float) GPS velocity in m/s in EAST direction in earth-fixed NED frame 'vd': 0, # (float) GPS velocity in m/s in DOWN direction in earth-fixed NED frame 'speed_accuracy': 0, # (float) GPS speed accuracy in m/s 'horiz_accuracy': 0, # (float) GPS horizontal accuracy in m 'vert_accuracy': 0, # (float) GPS vertical accuracy in m 'satellites_visible': 0 # (uint8_t) Number of satellites visible. }
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)
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)))
from pypozyx import PozyxSerial port = '/dev/ttyACM0' # on UNIX systems this will be '/dev/ttyACMX' p = PozyxSerial(port)
class BeaconToGPS(mp_module.MPModule): IGNORE_FLAG_ALL = (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_ALT | mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VDOP | mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT | mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY) def __init__(self, mpstate): """Initialise module""" super(BeaconToGPS, self).__init__(mpstate, "BeaconToGPS", "") self.anchor_config = None self.config_file_parser = ConfigParser.ConfigParser() self.config_file_parser.read(os.getcwd() + '/config/uwb_config.conf') self.anchor_config = self.config_file_parser.get( "Anchor", "anchor_coordinates") if self.anchor_config is None: print("Need set the anchor coordinate!") return self.yaw_deg = self.config_file_parser.getfloat( "NED", "yaw_form_ned_to_uwb") if self.yaw_deg is None: print("Need set the yaw from ned to uwb!") return else: print("NED to UWB yaw:" + str(self.yaw_deg) + " deg") self.debug = 0 self.debug = self.config_file_parser.getint("SYS", "debug") if self.debug is None: self.debug = 0 serial_port_dev = get_first_pozyx_serial_port() if serial_port_dev is None: print("No Pozyx connected. Check your USB cable or your driver!") return self.pozyx = PozyxSerial(serial_port_dev) self.anchors = self.anchor_config[1:len(self.anchor_config) - 1].split(";") self.anchor_list = [] self.position = Coordinates() self.velocity = Coordinates() self.pos_last = Coordinates() self.pos_last_time = 0 self.setup_pozyx() self.CONSTANTS_RADIUS_OF_EARTH = 6378100.0 self.DEG_TO_RAD = 0.01745329251994329576 self.RAD_TO_DEG = 57.29577951308232087679 self.reference_lat = 36.26586666666667 self.reference_lon = 120.27563333333333 self.reference_lat_rad = self.reference_lat * self.DEG_TO_RAD self.reference_lon_rad = self.reference_lon * self.DEG_TO_RAD self.cos_lat = math.cos(self.reference_lat_rad) self.target_lon_param = self.CONSTANTS_RADIUS_OF_EARTH * self.cos_lat self.current_lat = 0 self.current_lon = 0 self.tag_pos_ned = Coordinates() self.tag_velocity_ned = Coordinates() self.yaw = math.radians(self.yaw_deg) self.cos_yaw = math.cos(self.yaw) self.sin_yaw = math.sin(self.yaw) self.location_update = False self.location_update_time = 0 self.pos_update_time = 0 self.location_update_freq = 8 self.data = { 'time_usec': 0, # (uint64_t) Timestamp (micros since boot or Unix epoch) 'gps_id': 0, # (uint8_t) ID of the GPS for multiple GPS inputs 'ignore_flags': self. IGNORE_FLAG_ALL, # (uint16_t) Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided. 'time_week_ms': 0, # (uint32_t) GPS time (milliseconds from start of GPS week) 'time_week': 0, # (uint16_t) GPS week number 'fix_type': 0, # (uint8_t) 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK 'lat': 0, # (int32_t) Latitude (WGS84), in degrees * 1E7 'lon': 0, # (int32_t) Longitude (WGS84), in degrees * 1E7 'alt': 0, # (float) Altitude (AMSL, not WGS84), in m (positive for up) 'hdop': 0, # (float) GPS HDOP horizontal dilution of position in m 'vdop': 0, # (float) GPS VDOP vertical dilution of position in m 'vn': 0, # (float) GPS velocity in m/s in NORTH direction in earth-fixed NED frame 've': 0, # (float) GPS velocity in m/s in EAST direction in earth-fixed NED frame 'vd': 0, # (float) GPS velocity in m/s in DOWN direction in earth-fixed NED frame 'speed_accuracy': 0, # (float) GPS speed accuracy in m/s 'horiz_accuracy': 0, # (float) GPS horizontal accuracy in m 'vert_accuracy': 0, # (float) GPS vertical accuracy in m 'satellites_visible': 0 # (uint8_t) Number of satellites visible. } def setAnchorsManual(self): ''' config anchor''' status = self.pozyx.clearDevices() for temp_anchor in self.anchors: anchor = temp_anchor[1:len(temp_anchor) - 1].split(",") pozyx_anchor = DeviceCoordinates( int(anchor[0], 16), 1, Coordinates(int(anchor[1]), int(anchor[2]), int(anchor[3]))) status &= self.pozyx.addDevice(pozyx_anchor) self.anchor_list.append(pozyx_anchor) if len(self.anchors) > 4: status &= self.pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO, len(self.anchors)) def print_anchor_config(self): print("Anchor coordinate config:") anchor_coordinate = Coordinates() uwb_setting = UWBSettings() for i in range(len(self.anchor_list)): status = self.pozyx.getDeviceCoordinates( self.anchor_list[i].network_id, anchor_coordinate) if status == POZYX_SUCCESS: print("anchor " + hex(self.anchor_list[i].network_id) + " coordinate is X: " + str(anchor_coordinate.x) + " mm; Y: " + str(anchor_coordinate.y) + " mm; Z: " + str(anchor_coordinate.z) + " mm;") else: print("get anchor" + hex(self.anchor_list[i].network_id) + " coordinate config err") status = self.pozyx.getUWBSettings(uwb_setting, self.anchor_list[i].network_id) if status == POZYX_SUCCESS: print("UWB Setting, channel: " + str(uwb_setting.channel) + " Bitrate: " + str(uwb_setting.bitrate) + " Prf: " + str(uwb_setting.prf) + " Plen: " + str(uwb_setting.plen) + " Gain: " + str(uwb_setting.gain_db) + " DB") else: print("get UWB Setting err") def setup_pozyx(self): self.setAnchorsManual() if self.debug > 0: self.print_anchor_config() self.pozyx.doPositioning(self.pos_last, POZYX_3D, 1000, POZYX_POS_ALG_TRACKING) self.pos_last_time = time.time() def get_location(self): """Performs positioning and displays/exports the results.""" pos_mm = Coordinates() status = self.pozyx.doPositioning(pos_mm, POZYX_3D, 1000, POZYX_POS_ALG_TRACKING) now = time.time() if status == POZYX_SUCCESS: pos_err = PositionError() self.pozyx.getPositionError(pos_err) self.position.x = pos_mm.x * 0.001 #mm-->m self.position.y = pos_mm.y * 0.001 self.position.z = pos_mm.z * 0.001 self.get_tag_velocity(pos_mm, now) self.location_update = True if self.debug == 2: print(" Postion is X: " + str(self.position.x) + " m; Y: " + str(self.position.y) + " m; Z: " + str(self.position.z) + " m;" + " err: " + str(pos_err.xy)) else: if self.debug == 2: print("Do not get tag position") def get_tag_velocity(self, position_now, time_now): delt_pos = Coordinates() #unit: mm delt_pos.x = position_now.x - self.pos_last.x delt_pos.y = position_now.y - self.pos_last.y delt_pos.z = position_now.z - self.pos_last.z delt_time = (time_now - self.pos_last_time) * 1000.0 #s-->ms self.pos_last = position_now self.pos_last_time = time_now self.velocity.x = delt_pos.x / delt_time #m/s self.velocity.y = delt_pos.y / delt_time self.velocity.z = delt_pos.z / delt_time if self.debug == 2: print("Tag velocity is X=" + str(self.velocity.x) + "m/s; Y=" + str(self.velocity.y) + "m/s Z=" + str(self.velocity.z) + "m/s") def mavlink_packet(self, m): '''handle mavlink packets''' pass def send_gps_message(self): '''send gps message to fc ''' self.data['lat'] = self.current_lat * 1e7 self.data['lon'] = self.current_lon * 1e7 self.data['alt'] = self.tag_pos_ned.z self.data['vn'] = self.tag_velocity_ned.x self.data['ve'] = self.tag_velocity_ned.y self.data['vd'] = self.tag_velocity_ned.z self.data['speed_accuracy'] = 0.05 self.data['horiz_accuracy'] = 0.1 self.data['vert_accuracy'] = 0.1 self.data['satellites_visible'] = 20 self.data['time_week_ms'] = 0 self.data['time_usec'] = time.time() * 1e6 self.data['gps_id'] = 0 self.data['time_week'] = 0 self.data['fix_type'] = 5 self.master.mav.gps_input_send( self.data['time_usec'], self.data['gps_id'], self.data['ignore_flags'], self.data['time_week_ms'], self.data['time_week'], self.data['fix_type'], self.data['lat'], self.data['lon'], self.data['alt'], self.data['hdop'], self.data['vdop'], self.data['vn'], self.data['ve'], self.data['vd'], self.data['speed_accuracy'], self.data['horiz_accuracy'], self.data['vert_accuracy'], self.data['satellites_visible']) def global_point_from_vector(self): self.current_lat = (self.reference_lat_rad + self.tag_pos_ned.x / self.CONSTANTS_RADIUS_OF_EARTH) * self.RAD_TO_DEG self.current_lon = (self.reference_lon_rad + self.tag_pos_ned.y / self.target_lon_param) * self.RAD_TO_DEG if self.debug == 2: print("Current lat:" + str(self.current_lat) + "; lon:" + str(self.current_lon)) def convert_to_ned(self, vector): ned_vector = Coordinates() ned_vector.x = vector.x * self.cos_yaw - vector.y * self.sin_yaw ned_vector.y = vector.x * self.sin_yaw + vector.y * self.cos_yaw ned_vector.z = vector.z return ned_vector def idle_task(self): '''get location by uwb''' if self.pozyx == None: print("pozyx dev is none") return now = time.time() if (now - self.pos_update_time) > 1 / self.location_update_freq: self.get_location() if self.location_update: self.pos_update_time = now # just location update done,then update time self.tag_pos_ned = self.convert_to_ned(self.position) #m self.tag_velocity_ned = self.convert_to_ned( self.velocity) #m/s self.tag_velocity_ned.z = -self.tag_velocity_ned.z #ned self.global_point_from_vector() self.send_gps_message() self.location_update = False if self.debug == 1: print("update hz:" + str(1 / (now - self.location_update_time))) self.location_update_time = now
class MultitagPositioning(object): """Continuously performs multitag positioning""" def nicksThing(): def __init__(self, pozyx, osc_udp_client, tag_ids, anchors, algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, dimension=PozyxConstants.DIMENSION_3D, height=1000): self.pozyx = pozyx self.osc_udp_client = osc_udp_client self.tag_ids = tag_ids self.anchors = anchors self.algorithm = algorithm self.dimension = dimension self.height = height def setup(self): """Sets up the Pozyx for positioning by calibrating its anchor list.""" print("------------POZYX MULTITAG POSITIONING V{} -------------". format(version)) print("") print(" - System will manually calibrate the tags") print("") print(" - System will then auto start positioning") print("") if None in self.tag_ids: for device_id in self.tag_ids: self.pozyx.printDeviceInfo(device_id) else: for device_id in [None] + self.tag_ids: self.pozyx.printDeviceInfo(device_id) print("") print("------------POZYX MULTITAG POSITIONING V{} -------------". format(version)) print("") self.setAnchorsManual() self.printPublishAnchorConfiguration() def loop(self): """Performs positioning and prints the results.""" for tag_id in self.tag_ids: position = Coordinates() status = self.pozyx.doPositioning(position, self.dimension, self.height, self.algorithm, remote_id=tag_id) if status == POZYX_SUCCESS: self.printPublishPosition(position, tag_id) else: self.printPublishErrorCode("positioning", tag_id) # WORK IN PROGRESS: First attempt at making a function to collect and store the x, y, and z coordinates globally """def giveUsDemCoordyBois(self, position, network_id): #hopefully gives us the coordinates to be able to access outside of the class x_position = position.x; y_position = position.y; z_position = position.z; print(x_position); print(y_position); print(z_position);""" def printPublishPosition(self, position, network_id): """Prints the Pozyx's position and possibly sends it as a OSC packet""" if network_id is None: network_id = 0 s = "POS ID: {}, x(mm): {}, y(mm): {}, z(mm): {}".format( "0x%0.4x" % network_id, position.x, position.y, position.z) print(s) if self.osc_udp_client is not None: self.osc_udp_client.send_message( "/position", [network_id, position.x, position.y, position.z]) #NOTE: front tag is the tag connected to the Pi(0x673c/0x0000) and back tag is connected to external power source if network_id == 0x0000: #if the tag that is having its coordinates measured is the one connected to the Pi global x_position_front #indicate to program that the global variable for the front tag's x-position is to be used global y_position_front #indicate to program that the global variable for the front tag's y-position is to be used global z_position_front #indicate to program that the global variable for the front tag's z-position is to be used x_position_front = position.x #set the x-position of the front tag to the x-value output by the Pozyx y_position_front = position.y #set the y-position of the front tag to the y-value output by the Pozyx z_position_front = position.z #set the z-position of the front tag to the z-value output by the Pozyx print(x_position_front) #output the front tag's x-position print(y_position_front) #output the front tag's y-position print(z_position_front) #output the front tag's z-position else: #otherwise, if the tag that is having its coordinates measured is the one connected to the external power source global x_position_back #indicate to program that the global variable for the back tag's x-position is to be used global y_position_back #indicate to program that the global variable for the back tag's y-position is to be used global z_position_back #indicate to program that the global variable for the back tag's z-position is to be used x_position_back = position.x #set the x-position of the back tag to the x-value output by the Pozyx y_position_back = position.y #set the y-position of the back tag to the y-value output by the Pozyx z_position_back = position.z #set the z-position of the back tag to the z-value output by the Pozyx print(x_position_back) #output the back tag's x-position print(y_position_back) #output the back tag's y-position print(z_position_back) #output the back tag's z-position #print("THE LOOP HAS BEEN EXITED"); FOR TESTING: print a statement that will allow us to see how exactly the loop # running the main body is working and where it is at in its execution '''NOTE: The following print statements were used previously to check if the global variables for x, y, and z were being set to the Pozyx's outputs: print(x_position); print(y_position); print(z_position);''' 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(anchors) > 4: status &= self.pozyx.setSelectionOfAnchors( PozyxConstants.ANCHOR_SELECT_AUTO, len(anchors)) # enable these if you want to save the configuration to the devices. 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: print("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: print("Error %s on ID %s, %s" % (operation, "0x%0.4x" % network_id, self.pozyx.getErrorMessage(error_code))) if self.osc_udp_client is not None: self.osc_udp_client.send_message( "/error_%s" % operation, [network_id, error_code[0]]) else: # should only happen when not being able to communicate with a remote Pozyx. self.pozyx.getErrorCode(error_code) print("Error % s, local error code %s" % (operation, str(error_code))) if self.osc_udp_client is not None: self.osc_udp_client.send_message("/error_%s" % operation, [0, error_code[0]]) def printPublishAnchorConfiguration(self): for anchor in self.anchors: print("ANCHOR,0x%0.4x,%s" % (anchor.network_id, str(anchor.pos))) if self.osc_udp_client is not None: self.osc_udp_client.send_message("/anchor", [ anchor.network_id, anchor.pos.x, anchor.pos.y, anchor.pos.z ]) sleep(0.025) if __name__ == "__main__": # Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False. check_pypozyx_version = True if check_pypozyx_version: perform_latest_version_check() # shortcut to not have to find out the port yourself. serial_port = get_first_pozyx_serial_port() if serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") quit() # enable to send position data through OSC use_processing = True # configure if you want to route OSC to outside your localhost. Networking knowledge is required. ip = "127.0.0.1" network_port = 8888 # IDs of the tags to position, add None to position the local tag as well. tag_ids = [None, 0x6728] # necessary data for calibration anchors = [ DeviceCoordinates(0x6e09, 1, Coordinates(0, 0, 0)), DeviceCoordinates(0x674c, 1, Coordinates(1650, 0, 0)), DeviceCoordinates(0x6729, 1, Coordinates(0, 480, 0)), DeviceCoordinates(0x6765, 1, Coordinates(1650, 480, 0)) ] # positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY # positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D dimension = PozyxConstants.DIMENSION_2D # height of device, required in 2.5D positioning height = 1000 osc_udp_client = None if use_processing: osc_udp_client = SimpleUDPClient(ip, network_port) pozyx = PozyxSerial(serial_port) r = MultitagPositioning(pozyx, osc_udp_client, tag_ids, anchors, algorithm, dimension, height) r.setup() while True: r.loop() print("THE FRONT X POSITION IS:" + str(x_position_front)) #used to see if the x-positions of the front tag were correct print("THE FRONT Y POSITION IS:" + str(y_position_front)) #used to see if the y-positions of the front tag were correct print("THE FRONT Z POSITION IS:" + str(z_position_front)) #used to see if the z-positions of the front tag were correct print("THE BACK X POSITION IS:" + str(x_position_front)) #used to see if the x-positions of the back tag were correct print("THE BACK Y POSITION IS:" + str(y_position_front)) #used to see if the y-positions of the back tag were correct print("THE BACK Z POSITION IS:" + str(z_position_front)) #used to see if the z-positions of the back tag were correct y = int(y_position_front) x = int(x_position_front) #break; #NOTE: this was previously used to end the execution of the Pozyx measuring to see if the new variables were working #Previously used to check if the global variables for the x, y, and z coordinates were functioning properly: #print(x_position); #print(y_position); #print(z_position);''' import RPi.GPIO as GPIO #Pin setup for Entire Pi import time import curses #User Interface import serial #pin setup GPIO.setmode(GPIO.BOARD) GPIO.setup(13, GPIO.OUT) GPIO.setup(22, GPIO.OUT) GPIO.setup(15, GPIO.OUT) GPIO.setup(18, GPIO.OUT) #motor varibles FR = GPIO.PWM(13, 50) #Front Right Motor #The value 50 is the Frequency FL = GPIO.PWM(22, 50) #Front Left Motor #The value 12 is the GPIO pin RR = GPIO.PWM(15, 50) #Rear Right Motor RL = GPIO.PWM(18, 50) #Rear Left Motor FR.start(100) FL.start(100) RR.start(100) RL.start(100) #curses setup #screen = curses.initscr() #curses.noecho() #curses.cbreak() #screen.keypad(True) #User Interface print('...Loading...') while True: #getUpdatedCoordinates() print(y_position_front) nicksThing() if int(y_position_front) > 1400: FR.ChangeDutyCycle(100) FL.ChangeDutyCycle(100) RR.ChangeDutyCycle(100) RL.ChangeDutyCycle(100) print('Almost there') break else: print(y_position_front) FR.ChangeDutyCycle(6.5) FL.ChangeDutyCycle(8) RR.ChangeDutyCycle(6.5) RL.ChangeDutyCycle(8) while True: print("turning 90 degrees left") FR.ChangeDutyCycle(5) FL.ChangeDutyCycle(5) RR.ChangeDutyCycle(5) RL.ChangeDutyCycle(5) time.sleep(.68) FR.ChangeDutyCycle(100) FL.ChangeDutyCycle(100) RR.ChangeDutyCycle(100) RL.ChangeDutyCycle(100) break while True: #getUpdatedCoordinates() print(x_position_front) nicksThing() if (x_position_front) > (1400): FR.ChangeDutyCycle(100) FL.ChangeDutyCycle(100) RR.ChangeDutyCycle(100) RL.ChangeDutyCycle(100) print('Arrived') break else: print(x_position_front) FR.ChangeDutyCycle(6.5) FL.ChangeDutyCycle(8) RR.ChangeDutyCycle(6.5) RL.ChangeDutyCycle(8) print("tada!") #cleanup GPIO.cleanup curses.nobreak() screen.keypad(0) curses.echo() curses.endwin()
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
self.anchor2_truepose = Pose() self.anchor2_truepose = states.pose[index] found +=1 if found == 4: return True else: return False if __name__ == '__main__': try: sim = (sys.argv[1] == "True" or sys.argv[1] == "true") if not sim: p = None ports = get_pozyx_ports() for i in range(len(ports)): try: if ports[i] != os.environ['CORE_PORT']: p = PozyxSerial(ports[i]) break except: pass if p is None: quit() p.printDeviceInfo() else: p = None Pozyx(sim,p) except rospy.ROSInterruptException: pass
class IPozyx(object): """Continuously calls the Pozyx positioning function and prints its position.""" def __init__(self, anchors): self.serial_port = get_first_pozyx_serial_port() print(self.serial_port) if self.serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") self.pozyx = PozyxSerial(self.serial_port) #print(self.serial_port) if (anchors.get('count') == 4): self.anchors = [ DeviceCoordinates( 0x6110, 1, Coordinates( anchors.get('0x6110')[0], anchors.get('0x6110')[1], anchors.get('0x6110')[2])), DeviceCoordinates( 0x6115, 1, Coordinates( anchors.get('0x6115')[0], anchors.get('0x6115')[1], anchors.get('0x6115')[2])), DeviceCoordinates( 0x6117, 1, Coordinates( anchors.get('0x6117')[0], anchors.get('0x6117')[1], anchors.get('0x6117')[2])), DeviceCoordinates( 0x611e, 1, Coordinates( anchors.get('0x611e')[0], anchors.get('0x611e')[1], anchors.get('0x611e')[2])) ] self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY self.dimension = PozyxConstants.DIMENSION_3D self.height = 1000 #self.pub = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10) self.pub = rospy.Publisher('/pose', PoseStamped, queue_size=10) self.pose = PoseStamped() self.subZ = rospy.Subscriber('range', Int16, self.rangeCallback) def setup(self): self.setAnchorsManual() #self.printPublishConfigurationResult() def rangeCallback(self, msg): if (msg.data == 500): self.height = 0 elif (msg.data == 30): self.height = 0 else: self.height = msg.data #sleep(0.025) def run(self, height): """Performs positioning and displays/exports the results.""" success = False while (success != True): position = Coordinates() status = self.pozyx.doPositioning(position, self.dimension, height, self.algorithm) #position, self.dimension, self.height, self.algorithm) #position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id) if status == POZYX_SUCCESS: success = True return position else: pass #sleep(0.025) def pubPozyx(self): #success=False #while(success!=True): position = Coordinates() status = self.pozyx.doPositioning( #position, self.dimension, height, self.algorithm) position, self.dimension, self.height, self.algorithm) #position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id) if status == POZYX_SUCCESS: success = True #self.pose.timestamp = datetime.datetime.now() self.pose.header.stamp = rospy.Time.now() self.pose.header.frame_id = 'map' self.pose.pose.position.x = position.x self.pose.pose.position.y = position.y #self.pose.pose.position.x = 1.5 #self.pose.pose.position.y = 1.5 self.pose.pose.position.z = self.height self.pose.pose.orientation.w = 1.0 self.pub.publish(self.pose) else: pass #rospy.spin() def setAnchorsManual(self): """Adds the manually measured anchors to the Pozyx's device list one for one.""" #status = self.pozyx.clearDevices(remote_id=self.remote_id) status = self.pozyx.clearDevices() for anchor in self.anchors: #status &= self.pozyx.addDevice(anchor, remote_id=self.remote_id) status &= self.pozyx.addDevice(anchor) if len(self.anchors) > 4: #status &= self.pozyx.setSelectionOfAnchors(PozyxConstants.ANCHOR_SELECT_AUTO, len(self.anchors), remote_id=self.remote_id) status &= self.pozyx.setSelectionOfAnchors( PozyxConstants.ANCHOR_SELECT_AUTO, len(self.anchors)) return status
#change the ID's for setting coordinates of devices from config file anchor1 = anchor1ID if connectedDevice != anchor1ID else None anchor2 = anchor2ID if connectedDevice != anchor2ID else None anchor3 = anchor3ID if connectedDevice != anchor3ID else None anchor4 = anchor4ID if connectedDevice != anchor4ID else None #Setup variables coordinatesAnchor1 = Coordinates() coordinatesAnchor2 = Coordinates() coordinatesAnchor3 = Coordinates() coordinatesAnchor4 = Coordinates() #Check if connected serial_port = get_first_pozyx_serial_port() if serial_port is not None: pozyx = PozyxSerial(serial_port) print("Connection success!") else: print("No Pozyx port was found") exit() try: #Set the coordinates in the device pozyx.setCoordinates(Coordinates(anchor1X,anchor1Y,anchor1Z), anchor1) pozyx.setCoordinates(Coordinates(anchor2X,anchor2Y,anchor2Z), anchor2) pozyx.setCoordinates(Coordinates(anchor3X,anchor3Y,anchor3Z), anchor3) pozyx.setCoordinates(Coordinates(anchor4X,anchor4Y,anchor4Z), anchor4) print("Coordinates set.") #get the coordinates of the device pozyx.getCoordinates(coordinatesAnchor1, anchor1)
if check_pypozyx_version: perform_latest_version_check() # hardcoded way to assign a serial port of the Pozyx serial_port = 'COM12' # the easier way serial_port = get_first_pozyx_serial_port() if serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") quit() remote_id = 0x605D # the network ID of the remote device remote = False # whether to use the given remote device for ranging if not remote: remote_id = None destination_id = 0x6e66 # network ID of the ranging destination # distance that separates the amount of LEDs lighting up. range_step_mm = 1000 # the ranging protocol, other one is PozyxConstants.RANGE_PROTOCOL_PRECISION ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION pozyx = PozyxSerial(serial_port) r = ReadyToRange(pozyx, destination_id, range_step_mm, ranging_protocol, remote_id) r.setup() while True: r.loop()
for uwb_settings in all_discovery_uwb_settings(): pozyx.setUWBSettings(uwb_settings) pozyx.clearDevices() pozyx.doDiscoveryAll(slots=3, slot_duration=0.1) device_list = get_device_list(pozyx) if device_list: print("Found on {}".format(uwb_settings)) for device_id in device_list: uwbOnDevice = UWBSettings() status = pozyx.getUWBSettings(uwbOnDevice, device_id) if status == PozyxConstants.STATUS_SUCCESS: print("\t- {}, {}".format(hex(device_id), uwbOnDevice)) else: print("\t- {}".format(hex(device_id))) pozyx.setUWBSettings(original_uwb_settings) if __name__ == '__main__': from pypozyx import PozyxSerial, get_first_pozyx_serial_port pozyx = PozyxSerial(get_first_pozyx_serial_port()) uwb = UWBSettings() pozyx.getUWBSettings(uwb) print("Local device on {}".format(uwb)) discover_all_devices(pozyx)