def set_anchor_configuration(): global anchors, tags_id settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) serial_port = rospy.get_param("port", pypozyx.get_serial_ports()[0].device) local_device = pypozyx.PozyxSerial(serial_port) except: rospy.loginfo("Pozyx not connected") return for tag in tags_id: for anchor in anchors: pozyx.addDevice(anchor, tag) if len(anchors) > 4: pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO, len(anchors), remote_id=tag) pozyx.saveRegisters(settings_registers, remote_id=tag) pozyx.saveNetwork(remote_id=tag) if tag is None: rospy.loginfo("Local device configured") else: rospy.loginfo("Device with ID 0x%0.4x configured." % tag) rospy.loginfo("Configuration completed! Shutting down node now...")
def start(self): try: print(pypozyx.get_serial_ports()[1].device) self.pozyx = pypozyx.PozyxSerial( pypozyx.get_serial_ports()[1].device) whoami = pypozyx.SingleRegister() self.pozyx.getWhoAmI(whoami) rospy.loginfo( "Pozyx WhoAmI {0}".format(whoami)) # will return 0x43 # self.setNewId(0xA003, None) # rospy.sleep(1) networkid = pypozyx.NetworkID() self.pozyx.getNetworkId(networkid) rospy.loginfo("Network ID {0}".format(networkid)) self.pozyx.setPositionFilter( pypozyx.PozyxConstants.FILTER_TYPE_NONE, 0) # self.pozyx.setPositionFilter(pypozyx.PozyxConstants.FILTER_TYPE_MOVING_MEDIAN, 10) # self.setAnchorsManual(save_to_flash=False) self.setAnchorsManual(save_to_flash=True) self.pozyx.printDeviceInfo() self.printPublishConfigurationResultMore(None) except rospy.ROSException as e: rospy.loginfo("Pozyx not connected: {0}", e) return self.read_thread = Thread(target=self.read, args=()) self.read_thread.daemon = True self.read_thread.start() rospy.sleep(3) # wait to get stable pozyx position
def set_anchor_configuration(): tag_ids = [None] rospy.init_node('uwb_configurator') rospy.loginfo("Configuring device list.") settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return pozyx.doDiscovery(discovery_type=pypozyx.POZYX_DISCOVERY_TAGS_ONLY) device_list_size = pypozyx.SingleRegister() pozyx.getDeviceListSize(device_list_size) if device_list_size[0] > 0: device_list = pypozyx.DeviceList(list_size=device_list_size[0]) pozyx.getDeviceIds(device_list) tag_ids += device_list.data for tag in tag_ids: for anchor in anchors: pozyx.addDevice(anchor, tag) if len(anchors) > 4: pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO, len(anchors), remote_id=tag) pozyx.saveRegisters(settings_registers, remote_id=tag) pozyx.saveNetwork(remote_id=tag) if tag is None: rospy.loginfo("Local device configured") else: rosply.loginfo("Device with ID 0x%0.4x configured." % tag) rospy.loginfo("Configuration completed! Shutting down node now...")
def pozyx_pose_pub(): pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40) pubStamped = rospy.Publisher('pozyx_pose_stamped', PoseStamped, queue_size=40) rospy.init_node('pozyx_pose_node') h = std_msgs.msg.Header() h.frame_id = "pozyx" try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat))) h.stamp = rospy.Time.now() pub.publish(Point(coords.x, coords.y, coords.z), Quaternion(quat.x, quat.y, quat.z, quat.w)) pubStamped.publish( h, Pose( Point( float(coords.x) / 1000, float(coords.y) / 1000, float(coords.z) / 1000), Quaternion(quat.x, quat.y, quat.z, quat.w)))
def pozyx_positioning_pub(): pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100) rospy.init_node('positioning_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) rospy.loginfo(coords) pub.publish(Point32(coords.x, coords.y, coords.z))
def pozyx_euler_pub(): pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100) rospy.init_node('pozyx_euler_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id) rospy.loginfo(euler_angles) pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch)
def pozyx_pose_pub(): pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40) rospy.init_node('pozyx_pose_node') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat))) pub.publish(Point(coords.x, coords.y, coords.z), Quaternion(quat.x, quat.y, quat.z, quat.w))
def pozyx_ranging_pub(): pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100) rospy.init_node('range_info_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): device_range = pypozyx.DeviceRange() if pozyx.doRanging(destination_id, device_range, remote_id=remote_id): pub.publish(device_range.timestamp, device_range.distance, device_range.RSS) rospy.loginfo(device_range) else: rospy.loginfo('ERROR: RANGING')
def range_publisher(): global msg, device_range, range_pub try: range_pub = rospy.Publisher('pozyx/range', Range, queue_size=1) serial_port = rospy.get_param("port", pypozyx.get_serial_ports()[0].device) local_device = pypozyx.PozyxSerial(serial_port) remote = rospy.get_param("remote", None) # From where, None is local destination = rospy.get_param("destination", 0x6042) # To where except: rospy.loginfo("Serial device Pozyx not found") return while not rospy.is_shutdown(): # doRanging : Destination, Device_range, Remote local_device.doRanging(destination, device_range, remote) msg.range = device_range.distance range_pub.publish(msg)
def pozyx_position_euler_pub(): position_pub = rospy.Publisher( 'pozyx_positioning', Point32, queue_size=100) euler_pub = rospy.Publisher( 'pozyx_euler_angles', EulerAngles, queue_size=100) rospy.init_node('position_euler_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) pub.publish(coords.x, coords.y, coords.z) euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles) rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles))) pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch)
def pozyx_ranging_pub(): pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100) rospy.init_node('range_info_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): device_range = pypozyx.DeviceRange() # temporary fix for the issue pozyx.checkForFlag(pypozyx.definitions.bitmasks.POZYX_INT_MASK_RX_DATA, pypozyx.POZYX_DELAY_INTERRUPT) if pozyx.doRanging(destination_id, device_range, remote_id=remote_id): pub.publish(device_range.timestamp, device_range.distance, device_range.RSS) rospy.loginfo(device_range) else: error_code = SingleRegister() pozyx.getErrorCode(error_code) rospy.loginfo('ERROR: RANGING, error code %s' % error_code)
def set_same_uwb_settings(): rospy.init_node('uwb_configurator') devices_met = [] uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F] s = '' for device in device_ids: s += '0x%0.4x ' % device rospy.loginfo("Setting devices with IDs:%s to UWB settings: %s" % (s, str(new_uwb_settings))) try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return for channel in pypozyx.POZYX_ALL_CHANNELS: for bitrate in pypozyx.POZYX_ALL_BITRATES: for prf in pypozyx.POZYX_ALL_PRFS: for plen in pypozyx.POZYX_ALL_PLENS: uwb_settings = pypozyx.UWBSettings( channel, bitrate, prf, plen, gain_db) pozyx.clearDevices() pozyx.setUWBSettings(uwb_settings) pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES) device_list_size = pypozyx.SingleRegister() pozyx.getDeviceListSize(device_list_size) if device_list_size[0] > 0: device_list = pypozyx.DeviceList( list_size=device_list_size[0]) pozyx.getDeviceIds(device_list) for device in device_list: if device not in devices_met and device in device_ids: pozyx.setUWBSettings(new_uwb_settings, device) pozyx.saveRegisters(uwb_registers, device) rospy.loginfo( 'Device with ID 0x%0.4x is set' % device) devices_met.append(device) pozyx.setUWBSettings(new_uwb_settings) pozyx.saveRegisters(uwb_registers) rospy.loginfo("Local device set! Shutting down configurator node now...")
def pozyx_position_euler_pub(): position_pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100) euler_pub = rospy.Publisger('pozyx_euler_angles', EulerAngles, queue_size=100) rospy.init_node('position_euler_pub') try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) pub.publish(coords.x, coords.y, coords.z) euler_angles = pypozyx.EulerAngles() pozyx.getEulerAngles_deg(euler_angles) rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles))) pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch)
def init(): global pozyx_coords, pozyx_coords_err, pozyx_sensors global ros_pose, ros_mag, ros_imu, local_device, lock global height, algorithm, remote, serial_port, posehz, imuhz rospy.init_node('pozyx_pose_node') rospy.Subscriber("pozyx/height", Int16, height_cb) serial_port = rospy.get_param("port", pypozyx.get_serial_ports()[0].device) local_device = pypozyx.PozyxSerial(serial_port) # 0 is raw, 1 is fused but not yet implemented, 2 is filtered algorithm = rospy.get_param("algorithm", 2) remote = rospy.get_param("remote", None) height = rospy.get_param("height", 100) posehz = rospy.Rate(rospy.get_param("posehz", 2)) imuhz = rospy.Rate(rospy.get_param("imuhz", 100)) ros_pose = PoseWithCovarianceStamped() ros_mag = MagneticField() ros_imu = Imu() ros_pose.header.frame_id = "pozyx" ros_mag.header.frame_id = "pozyx" ros_imu.header.frame_id = "pozyx" pozyx_coords = pypozyx.Coordinates() pozyx_coords_err = pypozyx.PositionError() pozyx_sensors = pypozyx.SensorData() imu_thread = Thread(target=imu_publisher) coord_thread = Thread(target=coordinate_publisher) lock = Lock() imu_thread.start() coord_thread.start() rospy.spin()
def set_anchor_configuration(): rospy.init_node('uwb_configurator') rospy.loginfo("Configuring device list.") settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS try: pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) except: rospy.loginfo("Pozyx not connected") return for tag in tag_ids: for anchor in anchors: pozyx.addDevice(anchor, tag) if len(anchors) > 4: pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO, len(anchors), remote_id=tag) pozyx.saveRegisters(settings_registers, remote_id=tag) pozyx.saveNetwork(remote_id=tag) if tag is None: rospy.loginfo("Local device configured") else: rospy.loginfo("Device with ID 0x%0.4x configured." % tag) rospy.loginfo("Configuration completed! Shutting down node now...")
return ans2 # if np.abs(r4 - dist1) < np.abs(r4 - dist2): # return ans1 # else: # return ans2 def getCov(self): return # if __name__ == "__main__": rospy.init_node('pozyx_node_trilateration') frequency = float(rospy.get_param('~frequency')) dt = 1 / frequency serial_port = get_serial_ports()[2].device if serial_port is None: print("No Pozyx connected. Check your USB cable or your driver!") quit() remote = False if not remote: remote_id = None anchor0_id = int(rospy.get_param('~anchor0_id'), 16) anchor1_id = int(rospy.get_param('~anchor1_id'), 16) anchor2_id = int(rospy.get_param('~anchor2_id'), 16) anchor3_id = int(rospy.get_param('~anchor3_id'), 16) anchor0_coordinates = np.array(
def get_device(): devices = [d for d in px.get_serial_ports() if is_pozyx(d)] if devices: return devices[0].device return None
def get_serial_port_names(): return [port.device for port in get_serial_ports()]
def get_pozyx_serial_port(): for port in get_serial_ports(): if inspect_port(port): return port.device