def startRecording(): if get_first_pozyx_serial_port() is None: return json.dumps({'success':False, 'errorType':'NO_POZYX_CONNECTED'}), 200, {'ContentType':'application/json'} my_var1 = session.get('my_var1', None) mid1 = my_var1 now = datetime.datetime.now() sql = '''INSERT INTO recordings (Match_ID, startTime) VALUES(%s,%s)''' recordingData = ([mid1], now) cur = conn.cursor() cur.execute(sql, recordingData) try: conn.commit() global RID RID = cur.lastrowid print("currentRecordingID after commit: " + str(RID)) except Exception as e: print(e) print("Start recording!avc") global isRecording isRecording = True pozyxThread = PozyxThread() pozyxThread.start() return json.dumps({'success':True}), 200, {'ContentType':'application/json'}
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 pozyx_pose_pub(): pub = rospy.Publisher('/charlie_pose', PoseWithCovarianceStamped, vicon_cb, queue_size=1000) pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=1000) pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=1000) rospy.init_node('pozyx_pose_node') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() tags = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) pose.pose.pose.position.x = coords.x / 1000.0 pose.pose.pose.position.y = coords.y / 1000.0 pose.pose.pose.position.z = coords.z / 1000.0 # ho messo come messaggio PoseWithCovarianceStamped ed ho aggiunto la covarianza a mano, quindi per accedere alle variabili di pos e orient bisogna fare pose.pose.pose, se fosse solo PoseStamped basta pose.pose pose.pose.covariance[0] = 0.01 pose.pose.covariance[7] = 0.01 pose.pose.covariance[14] = 99999 pose.pose.covariance[21] = 99999 pose.pose.covariance[28] = 99999 pose.pose.covariance[35] = 0.01 tags.x = coords.x / 1000.0 tags.y = coords.y / 1000.0 tags.z = coords.z / 1000.0 pose.pose.pose.orientation.x = quat.x pose.pose.pose.orientation.y = quat.y pose.pose.pose.orientation.z = quat.z pose.pose.pose.orientation.w = quat.w rospy.loginfo("POS: %s, QUAT: %s" % (str(tags), str(quat))) pub.publish(pose) pub1.publish(Point(tags.x, tags.y, tags.z)) pub2.publish(Point(tags.x, tags.y, tags.z)) euler_angles = pypozyx.EulerAngles() euler_angles.heading = numpy.deg2rad(euler_angles.heading) tf_quat = tf.transformations.quaternion_from_euler( 0, 0, -euler_angles.heading) br = tf.TransformBroadcaster() br.sendTransform((coords.x / 1000.0, coords.y / 1000.0, 0), tf_quat, rospy.Time.now(), tag0_frame_ID, UWB_frame_ID)
def pozyx_pose_pub(): pub = rospy.Publisher('amcl_pose', PoseStamped, vicon_cb, queue_size=100) rospy.init_node('pozyx_pose_node') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat))) pose.pose.position.x = coords.x pose.pose.position.y = coords.y pose.pose.position.z = coords.z pose.pose.orientation.x = quat.x pose.pose.orientation.y = quat.y pose.pose.orientation.z = quat.z pose.pose.orientation.w = quat.w pub.publish(pose)
def pozyx_position_euler_pub(): #position_pub = rospy.Publisher( # 'pozyx_positioning', Point32, queue_size=100) position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100) #euler_pub = rospy.Publisher( # 'pozyx_euler_angles', EulerAngles, queue_size=100) euler_pub = rospy.Publisher('/orientation', Point, queue_size=100) vx = 0.0 vy = 0.0 vth = 0.0 rospy.init_node('position_euler_pub') #pub odom msg for move_base odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) odom_broadcaster = tf.TransformBroadcaster() current_time = rospy.Time.now() last_time = rospy.Time.now() try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) #BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) position_pub.publish(coords.x, coords.y, coords.z) euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles) rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles))) euler_pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch) # combine pos and angle info and publish as odom msg current_time = rospy.Time.now() # odometry is 6DOF, created quaternion from yaw odom_quat = tf.transformations.quaternion_from_euler( 0, 0, euler_angles.heading) # publish the transform over tf odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat, current_time, "base_footprint", "odom") #base_link # publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" odom.pose.pose = Pose(Point(coords.x, coords.y, 0.), Quaternion(*odom_quat)) odom.child_frame_id = "base_footprint" #base_link odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) # set the velocity odom_pub.publish(odom) last_time = current_time
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 pozyx_position_euler_pub(): global rcvd_msgs global rec_x global rec_y #position_pub = rospy.Publisher( # 'pozyx_positioning', Point32, queue_size=100) position_pub = rospy.Publisher('/charlie_pose', Point, queue_size=100) #euler_pub = rospy.Publisher( # 'pozyx_euler_angles', EulerAngles, queue_size=100) euler_pub = rospy.Publisher('/orientation', Point, queue_size=100) pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=100) pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=100) rospy.init_node('position_euler_pub') rate=rospy.Rate(1) try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) #BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() tags = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) pozyx.doPositioning(tags, remote_id=remote_id) euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles) euler_pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch) rospy.loginfo("X: %f Y: %f" , tags.x, tags.y) rospy.loginfo(euler_angles) pub1.publish(tags.x/1000.0,tags.y/1000.0,tags.z/1000.0) pub2.publish(tags.x/1000.0, tags.y/1000.0,tags.z/1000.0) position_pub.publish(coords.x/1000.0, coords.y/1000.0, coords.z/1000.0) euler_angles.heading = numpy.deg2rad(euler_angles.heading) tf_quat = tf.transformations.quaternion_from_euler(0, 0, -euler_angles.heading) br = tf.TransformBroadcaster() br.sendTransform((coords.x/1000.0, coords.y/1000.0, 0), tf_quat, rospy.Time.now(), tag0_frame_ID, UWB_frame_ID ) rate.sleep()
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 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, tags, serial_port=None): if (serial_port is None): serial_port = pypozyx.get_first_pozyx_serial_port() assert (serial_port is not None, "Could not find local pozyx device") self.local = pypozyx.PozyxSerial(serial_port) # get local id local_id = pypozyx.NetworkID() status = self.local.getNetworkId(local_id) self._check_status("get network id", status) self.local_id = local_id[0] # check for nessary devices and set coordinates self._find_devices() device_list = self._get_device_list() device_list.append(self.local_id) for device in anchors: id = device.network_id cord = device.pos if id in device_list: status = self.local.setCoordinates( cord, None if id == self.local_id else id) self._check_status("set coordinates", status) else: raise Exception( "failed to find a pozyx device with the id {:02x}".format( id)) for id in tags: if id not in device_list: raise Exception( "failed to find a pozyx device with the id {:02x}".format( id)) # remove any unknown devices anchor_ids = map(lambda device: device.network_id, anchors) for id in device_list: if (id not in anchor_ids) and (id not in tags): # TODO: log removal of unknown device self.local.removeDevice(id) # store lists of anchors and tags self.anchors = anchor_ids self.tags = tags
def pozyx_range(): pub = rospy.Publisher('pozyx_range', DeviceRange, queue_size=100) rospy.init_node('range_pub') try: serial_port = pypozyx.get_first_pozyx_serial_port() if serial_port == None: rospy.loginfo("Pozyx not connected") return pozyx = pypozyx.PozyxSerial(serial_port) except: rospy.loginfo("Could not connect to pozyx") return # get network id of connected pozyx self_id = pypozyx.NetworkID() if pozyx.getNetworkId(self_id) != pypozyx.POZYX_SUCCESS: rospy.loginfo("Could not get network id") return # discover all other pozyx pozyx.doDiscoveryAll() devices_size = pypozyx.SingleRegister() pozyx.getDeviceListSize(devices_size) devices = pypozyx.DeviceList(devices_size) pozyx.getDeviceIds(devices) for i in range(devices_size[0]): rospy.loginfo("Found pozyx device with the id {}".format( pypozyx.NetworkID(devices[i]))) # publish data while not rospy.is_shutdown(): for i in range(devices_size[0]): device_range = pypozyx.DeviceRange() remote_id = pypozyx.NetworkID(devices[i]) if pozyx.doRanging(self_id[0], device_range, remote_id[0]): h = Header() h.stamp = rospy.Time.from_sec(device_range[0] / 1000) pub.publish(h, remote_id[0], device_range[1], device_range[2]) else: rospy.loginfo("Error while ranging")
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 pozyx_euler_pub(): pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100) rospy.init_node('pozyx_euler_pub') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id) rospy.loginfo(euler_angles) pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch)
def pozyx_ranging_pub(): pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100) rospy.init_node('range_info_pub') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): device_range = pypozyx.DeviceRange() if pozyx.doRanging(destination_id, device_range, remote_id=remote_id): pub.publish(device_range.timestamp, device_range.distance, device_range.RSS) rospy.loginfo(device_range) else: rospy.loginfo('ERROR: RANGING')
def __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 pozyx_pose_pub(): pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40) rospy.init_node('pozyx_pose_node') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat))) pub.publish(Point(coords.x, coords.y, coords.z), Quaternion(quat.x, quat.y, quat.z, quat.w))
def set_same_uwb_settings(): rospy.init_node('uwb_configurator') devices_met = [] uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F] s = '' for device in device_ids: s += '0x%0.4x ' % device rospy.loginfo("Setting devices with IDs:%s to UWB settings: %s" % (s, str(new_uwb_settings))) try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) #BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return for channel in pypozyx.POZYX_ALL_CHANNELS: for bitrate in pypozyx.POZYX_ALL_BITRATES: for prf in pypozyx.POZYX_ALL_PRFS: for plen in pypozyx.POZYX_ALL_PLENS: uwb_settings = pypozyx.UWBSettings( channel, bitrate, prf, plen, gain_db) pozyx.clearDevices() pozyx.setUWBSettings(uwb_settings) pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES) device_list_size = pypozyx.SingleRegister() pozyx.getDeviceListSize(device_list_size) if device_list_size[0] > 0: device_list = pypozyx.DeviceList( list_size=device_list_size[0]) pozyx.getDeviceIds(device_list) for device in device_list: if device not in devices_met and device in device_ids: pozyx.setUWBSettings(new_uwb_settings, device) pozyx.saveRegisters(uwb_registers, device) rospy.loginfo( 'Device with ID 0x%0.4x is set' % device) devices_met.append(device) pozyx.setUWBSettings(new_uwb_settings) pozyx.saveRegisters(uwb_registers) rospy.loginfo("Local device set! Shutting down configurator node now...")
def set_anchor_configuration(): tag_ids = [None] rospy.init_node('uwb_configurator') rospy.loginfo("Configuring device list.") settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return pozyx.doDiscovery(discovery_type=pypozyx.POZYX_DISCOVERY_TAGS_ONLY) device_list_size = pypozyx.SingleRegister() pozyx.getDeviceListSize(device_list_size) if device_list_size[0] > 0: device_list = pypozyx.DeviceList(list_size=device_list_size[0]) pozyx.getDeviceIds(device_list) tag_ids += device_list.data for tag in tag_ids: for anchor in anchors: pozyx.addDevice(anchor, tag) if len(anchors) > 4: pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO, len(anchors), remote_id=tag) pozyx.saveRegisters(settings_registers, remote_id=tag) pozyx.saveNetwork(remote_id=tag) if tag is None: rospy.loginfo("Local device configured") else: rospy.loginfo("Device with ID 0x%0.4x configured." % tag) rospy.loginfo("Configuration completed! Shutting down node now...")
def pozyx_positioning_pub(): # pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100) pub = rospy.Publisher('/tag_pose0', Point, queue_size=100) pub1 = rospy.Publisher('/tag_pose1', Point, queue_size=100) rospy.init_node('positioning_pub') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) # BKP-> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) rospy.loginfo(coords) #pub.publish(Point32(coords.x, coords.y, coords.z)) #pub.publish(Point(coords.x, coords.y, coords.z)) pub.publish( Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0)) pub1.publish( Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0))
#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)
def __init__(self): self.pozyx = PozyxSerial(get_first_pozyx_serial_port()) self.direct = EulerAngles() self.position = Coordinates()
def __init__(self, i2c=False, bus=1, port=None): super().__init__() self._sensor = pypozyx.PozyxI2C(bus) if i2c else \ pypozyx.PozyxSerial(port or pypozyx.get_first_pozyx_serial_port())
"/anchor", [device_list[i], int(anchor_coordinates.x), int(anchor_coordinates.y), int(anchor_coordinates.z)]) sleep(0.025) def printPublishAnchorConfiguration(self): """Prints and potentially publishes the anchor configuration""" for anchor in self.anchors: print("ANCHOR,0x%0.4x,%s" % (anchor.network_id, str(anchor.coordinates))) if self.osc_udp_client is not None: self.osc_udp_client.send_message( "/anchor", [anchor.network_id, int(anchor.coordinates.x), int(anchor.coordinates.y), int(anchor.coordinates.z)]) sleep(0.025) if __name__ == "__main__": # 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() remote_id = 0x6069 # remote device network ID remote = False # whether to use a remote device if not remote: remote_id = None use_processing = False # 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)
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 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()
def pozyx_position_euler_pub(): #position_pub = rospy.Publisher( # 'pozyx_positioning', Point32, queue_size=100) position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100) #euler_pub = rospy.Publisher( # 'pozyx_euler_angles', EulerAngles, queue_size=100) euler_pub = rospy.Publisher('/orientation', Point, queue_size=100) vx = 0.0 vy = 0.0 vth = 0.0 rospy.init_node('position_euler_pub') #PROVA #pub odom msg for move_base odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) odom_broadcaster = tf.TransformBroadcaster() current_time = rospy.Time.now() last_time = rospy.Time.now() #rospy.init_node('position_euler_pub') try: # MOD @ 12-08-2020 serial_port = pypozyx.get_first_pozyx_serial_port() pozyx = pypozyx.PozyxSerial(serial_port) #BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): #################### COORDS #################### coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) # convert from mm to m #rospy.loginfo("POS_1: %s m" % (str(coords))) coords.x = coords.x / 1000.0 coords.y = coords.y / 1000.0 coords.z = coords.z / 1000.0 #rospy.loginfo("POS_1: %s m" % (str(coords))) ''' if records < n_samples: coords.x += coords.x coords.y += coords.y coords.z += coords.z records = 'done' continue elif records == 'done': coords.x /= n_samples coords.y /= n_samples coords.z /= n_samples records = 'finish' continue ''' position_pub.publish(coords.x, coords.y, coords.z) #################### ANGLES ##################### euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles) # convert from degrees to radians ( *3.14/180 ) euler_angles.heading = numpy.deg2rad(euler_angles.heading) euler_angles.roll = numpy.deg2rad(euler_angles.roll) euler_angles.pitch = numpy.deg2rad(euler_angles.pitch) rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles))) euler_pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch) #################### PUBLISHING ODOM TF ################ # combine pos and angle info and publish as odom msg current_time = rospy.Time.now() # odometry is 6DOF, created quaternion from yaw odom_quat = tf.transformations.quaternion_from_euler( 0, 0, euler_angles.heading) # publish the transform over tf odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat, current_time, tag_frame_ID, UWB_frame_ID) # publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = UWB_frame_ID odom.pose.pose = Pose(Point(coords.x, coords.y, 0.), Quaternion(*odom_quat)) odom.child_frame_id = tag_frame_ID odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) # set the velocity odom_pub.publish(odom) last_time = current_time
def main(): try: odom_data = com.rxData() pub.publish(odom_data) except Exception as e: rospy.logwarn(e) pass com.txData() if __name__ == "__main__": rospy.init_node('uwb_node') serial_port = str( rospy.get_param('~serial_port', pzx.get_first_pozyx_serial_port())) frequency = float(rospy.get_param('~frequency', 10)) rate = rospy.Rate(frequency) pozyx = pzx.PozyxSerial(serial_port) destination = rospy.get_param('~destination', 0x6e2f) tx_topic = str(rospy.get_param('~tx_topic', 'uwb_server_tx')) rx_topic = str(rospy.get_param('~rx_topic', 'uwb_server_rx')) pub = rospy.Publisher(rx_topic, Odometry, queue_size=10) com = Communicate(pozyx, destination) rospy.Subscriber(tx_topic, Odometry, com.odomData) while not rospy.is_shutdown(): main()
status &= self.pozyx.setLed(2, (distance < 3 * range_step_mm), id) status &= self.pozyx.setLed(1, (distance < 4 * range_step_mm), id) return status 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() # 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
status &= self.pozyx.setLed(2, (distance < 3 * range_step_mm), id) status &= self.pozyx.setLed(1, (distance < 4 * range_step_mm), id) return status 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() # hardcoded way to assign a serial port of the Pozyx serial_port = 'COM12' # the easier way serial_port = get_first_pozyx_serial_port( ) # should be /dev/ttyACM0 for my computer if serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") quit() remote_id = 0x766e # the network ID of the remote device (device 1) remote = True # whether to use the given remote device for ranging if not remote: remote_id = None destination_id = 0x7607 # network ID of the ranging destination (device 2) # 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