def set_uwb_settings(pozyx, set_all=True, **kwargs): new_uwb_settings = pypozyx.UWBSettings(**kwargs) gain_db = kwargs.get('gain', 15.0) devices_met = [] uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F] rospy.loginfo("Start to configure with UWB settings: %s" % str(kwargs)) if set_all: rospy.loginfo('Will scan for all remote devices and configure them') 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: rospy.loginfo( "Looking for devices on channel %d, bitrate 0x%x, " "prf 0x%x, plen 0x%x", channel, bitrate, prf, plen) uwb_settings = pypozyx.UWBSettings( channel, bitrate, prf, plen, gain_db) pozyx.clearDevices() pozyx.setUWBSettings(uwb_settings) for device in get_devices(pozyx): if device not in devices_met: pozyx.setUWBSettings(new_uwb_settings, device) pozyx.saveRegisters(uwb_registers, device) rospy.loginfo('Device with ID 0x%0.4x set', device) devices_met.append(device) rospy.loginfo("Scanning end. Configured remote devices %s", ["0x%x" % d for d in devices_met]) pozyx.setUWBSettings(new_uwb_settings) pozyx.saveRegisters(uwb_registers) rospy.loginfo("Local device set. Configuration done.")
def expose_pozyx_config(self): sr = px.SingleRegister() # Changed: Not available in firware 1.1 (at least in the corresp. python lib) # self.pozyx.setOperationMode(0, self.remote_id) # work as a tag self.pozyx.getWhoAmI(sr, self.remote_id) _id = '0x%.2x' % sr.data[0] rospy.set_param('~id', _id) self.pozyx.getFirmwareVersion(sr, self.remote_id) rospy.set_param('~firmware', register2version(sr)) self.pozyx.getHardwareVersion(sr, self.remote_id) rospy.set_param('~hardware', register2version(sr)) ni = px.NetworkID() self.pozyx.getNetworkId(ni) rospy.set_param('~uwb/network_id', str(ni)) s = px.UWBSettings() self.pozyx.getUWBSettings(s, self.remote_id) rospy.set_param('~uwb/channel', s.channel) rospy.set_param('~uwb/bitrate', s.parse_bitrate()) rospy.set_param('~uwb/prf', s.parse_prf()) rospy.set_param('~uwb/plen', s.parse_plen()) rospy.set_param('~uwb/gain', s.gain_db) self.pozyx.getOperationMode(sr, self.remote_id) rospy.set_param('~uwb/mode', 'anchor' if (sr.data[0] & 1) == 1 else 'tag') self.pozyx.getSensorMode(sr, self.remote_id) rospy.set_param('~sensor_mode', sensor_mode(sr)) self.self_test()
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...")
This should be run once every time you change settings etc but no more than that, as it saves to flash. It's a ROS node that runs until done then dies, so running this until it completes and then starting the other nodes is valid. PS: Don't forget the anchor configuration. """ import pypozyx import rospy # device_ids = [0x0001, 0x0002, 0x0003, 0x0004] device_ids = [0x6902, 0x6E44, 0x6E7A, 0x6E6C] # our device IDs [0 1 2 3] new_uwb_settings = pypozyx.UWBSettings( channel=2, bitrate=2, prf=2, plen=0x04, gain_db=15.0) gain_db = 15.0 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
def __init__(self, device): rospy.init_node('pozyx_driver', anonymous=True) updater = diagnostic_updater.Updater() self.ps = deque(maxlen=20) self.es = deque(maxlen=20) self.remote_id = rospy.get_param('~remote_id', None) self.base_frame_id = rospy.get_param('~base_frame_id', 'base_link') debug = rospy.get_param('~debug', False) self.enable_orientation = rospy.get_param('~enable_orientation', True) self.enable_position = rospy.get_param('~enable_position', True) self.enable_raw_sensors = rospy.get_param('~enable_sensors', True) las = rospy.get_param("~linear_acceleration_stddev", 0.0) avs = rospy.get_param("~angular_velocity_stddev", 0.0) mfs = rospy.get_param("~magnetic_field_stddev", 0.0) os = rospy.get_param("~orientation_stddev", 0.0) ps = rospy.get_param('~position_stddev', 0.0) self.pose_cov = cov([ps] * 3 + [os] * 3) self.orientation_cov = cov([os] * 3) self.angular_velocity_cov = cov([avs] * 3) self.magnetic_field_cov = cov([mfs] * 3) self.linear_acceleration_cov = cov([las] * 3) _a = rospy.get_param('~algorithm', 'uwb_only') _d = rospy.get_param('~dimension', '3d') rate = rospy.get_param('~rate', 1.0) # Hz self.algorithm = _algorithms.get(_a, px.POZYX_POS_ALG_UWB_ONLY) self.dimension = _dimensions.get(_d, px.POZYX_3D) baudrate = rospy.get_param('~baudrate', 115200) # height of device, required in 2.5D positioning self.height = rospy.get_param('~height', 0.0) # mm p = None while not p and not rospy.is_shutdown(): try: p = px.PozyxSerial(device, baudrate=baudrate, print_output=debug) except SystemExit: rospy.sleep(1) if not p: return self.pozyx = PozyxProxy(p, self.remote_id) self.pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1) self.pose_cov_pub = rospy.Publisher('pose_cov', PoseWithCovarianceStamped, queue_size=1) self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1) self.mag_pub = rospy.Publisher('mag', MagneticField, queue_size=1) self.frame_id = rospy.get_param('~anchors/frame_id') anchors = [] if rospy.has_param('~anchors/positions'): anchors_data = rospy.get_param('~anchors/positions') anchors = [ px.DeviceCoordinates(dev_id, 1, px.Coordinates(x, y, z)) for [dev_id, x, y, z] in anchors_data ] self.check_visible_devices() rospy.sleep(0.1) self.set_anchors(anchors) rospy.sleep(0.1) self.check_config(anchors) rospy.sleep(0.1) self.tfBuffer = tf2_ros.Buffer() self.tf = tf2_ros.TransformListener(self.tfBuffer) if self.enable_orientation: try: t = self.tfBuffer.lookup_transform(self.frame_id, 'utm', rospy.Time(), rospy.Duration(5.0)) # r = t.transform.rotation # self.rotation = [r.x, r.y, r.z, r.w] self.rotation = quaternion_from_euler(0, 0, 0) # = r # self.rotation = quaternion_multiply(r, self.rotation) rospy.loginfo('rotation map-> pozyx %s', self.rotation) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logerr('Can not tranform from utm to map frame') self.enable_orientation = False sr = px.SingleRegister() self.pozyx.setOperationMode(0, self.remote_id) # work as a tag self.pozyx.getWhoAmI(sr, self.remote_id) _id = '0x%.2x' % sr.data[0] rospy.set_param('~id', _id) updater.setHardwareID('Pozyx %s' % _id) self.pozyx.getFirmwareVersion(sr, self.remote_id) rospy.set_param('~firmware', register2version(sr)) self.pozyx.getHardwareVersion(sr, self.remote_id) rospy.set_param('~hardware', register2version(sr)) ni = px.NetworkID() self.pozyx.getNetworkId(ni) rospy.set_param('~uwb/network_id', str(ni)) s = px.UWBSettings() self.pozyx.getUWBSettings(s, self.remote_id) rospy.set_param('~uwb/channel', s.channel) rospy.set_param('~uwb/bitrate', s.parse_bitrate()) rospy.set_param('~uwb/prf', s.parse_prf()) rospy.set_param('~uwb/plen', s.parse_plen()) rospy.set_param('~uwb/gain', s.gain_db) self.pozyx.getOperationMode(sr, self.remote_id) rospy.set_param('~uwb/mode', 'anchor' if (sr.data[0] & 1) == 1 else 'tag') self.pozyx.getSensorMode(sr, self.remote_id) rospy.set_param('~sensor_mode', sensor_mode(sr)) self_test = self.check_self_test() if not all(self_test.values()): rospy.logwarn('Failed Self Test %s', self_test) else: rospy.loginfo('Passed Self Test %s', self_test) freq_bounds = {'min': rate * 0.8, 'max': rate * 1.2} freq_param = diagnostic_updater.FrequencyStatusParam( freq_bounds, 0.1, 10) stamp_param = diagnostic_updater.TimeStampStatusParam() self.pose_pub_stat = diagnostic_updater.DiagnosedPublisher( self.pose_pub, updater, freq_param, stamp_param) updater.add("Sensor calibration", self.update_sensor_diagnostic) updater.add("Localization", self.update_localization_diagnostic) rospy.on_shutdown(self.cleanup) continuous = rospy.get_param('~continuous', False) rospy.Timer(rospy.Duration(1), lambda evt: updater.update()) rospy.Subscriber('set_anchors', Anchors, self.has_updated_anchors) if continuous: if self.enable_position: ms = int(1000.0 / rate) # self.pozyx.setUpdateInterval(200) msr = px.SingleRegister(ms, size=2) self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id) self.pozyx.setPositionAlgorithm(self.algorithm, self.dimension) rospy.sleep(0.1) else: msr = px.SingleRegister(0, size=2) self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id) if self.enable_position or self.enable_raw_sensors: Updater(self, ms * 0.001) # PositionUpdater(self, ms / 1000.0) # if self.enable_raw_sensors: # IMUUpdater(self) # # position = px.Coordinates() # # while not rospy.is_shutdown(): # # try: # # self.pozyx.checkForFlag(bm.POZYX_INT_STATUS_POS, 0.2) # # self.pozyx.getCoordinates(position) # # x = np.array([position.x, position.y, position.z])/1000.0 # # self.publish_pose(x) # # except PozyxException as e: # # rospy.logerr(e.message) # # rospy.sleep(1) rospy.spin() else: rospy.Timer(rospy.Duration(1 / rate), self.update) rospy.spin()