def init_pozyx(self): if False and rospy.has_param('~gain_db'): gain = rospy.get_param('~gain_db') configure.set_gain(self.pozyx, gain, set_all=False) 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) if self.continuous: if self.enable_position: ms = int(1000.0 / self.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) else: msr = px.SingleRegister(0, size=2) self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id) rospy.sleep(0.1)
def run(self): position = px.Coordinates() error = px.PositionError() sensor_data = px.SensorData() pozyx = self.driver.pozyx interrupt = px.SingleRegister() while not rospy.is_shutdown(): try: if pozyx.checkForFlag(self.flag, self.DELAY, interrupt): if self.driver.enable_position and interrupt.data[ 0] & bm.POZYX_INT_STATUS_POS: pozyx.getCoordinates(position) pozyx.getPositionError(error) if self.driver.accept_position(position, error): x = np.array([position.x, position.y, position.z ]) / 1000.0 self.driver.publish_pose(x) if (self.driver.enable_raw_sensors and interrupt.data[0] & bm.POZYX_INT_STATUS_IMU): pozyx.getAllSensorData(sensor_data) self.driver.publish_sensor_data(sensor_data) rospy.sleep(self.period) except PozyxExceptionTimeout: pass except PozyxException as e: rospy.logerr(e.message) pozyx.ser.reset_output_buffer() pozyx.ser.reset_input_buffer() rospy.sleep(1)
def run(self): position = px.Coordinates() error = px.PositionError() sensor_data = px.SensorData() pozyx = self.driver.pozyx interrupt = px.SingleRegister() while not rospy.is_shutdown(): try: if pozyx.checkForFlag(self.flag, self.period, interrupt): if self.driver.enable_position and interrupt.data[0] & bm.POZYX_INT_STATUS_POS: pozyx.getCoordinates(position) pozyx.getPositionError(error) if self.driver.accept_position(position, error): x = np.array([position.x, position.y, position.z]) / 1000.0 self.driver.publish_pose(x, error=error) if (self.driver.enable_raw_sensors and interrupt.data[0] & bm.POZYX_INT_STATUS_IMU): pozyx.getAllSensorData(sensor_data) self.driver.publish_sensor_data(sensor_data) # except PozyxExceptionTimeout as e: # rospy.logwarn('%s: %s', e.__class__.__name__, e.message) except PozyxException as e: log_pozyx_exception(e) # pozyx.ser.reset_output_buffer() # pozyx.ser.reset_input_buffer() # rospy.sleep(1) self.driver.reset() finally: rospy.sleep(self.period * 0.5)
def pozyx_ranging_pub(): pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100) rospy.init_node('range_info_pub_0x6940') r = rospy.Rate(10) try: #pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device) pozyx = pypozyx.PozyxSerial('/dev/ttyACM0') 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, str(destination_id)) rospy.loginfo(device_range) r.sleep() else: error_code = pypozyx.SingleRegister() pozyx.getErrorCode(error_code) rospy.loginfo('ERROR: RANGING, error code %s' % error_code) if str(error_code) in '0x1': rospy.loginfo('ERROR: RANGING, killing publisher') return
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 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(str(comports()[0]).split(' ')[0]) except: rospy.loginfo("Pozyx not connected") return pozyx.doDiscovery(discovery_type=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(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 get_devices(pozyx): 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) return device_list else: return []
def check_self_test(self): mask = {5: 'UWB', 4: 'PRESSURE', 3: 'IMU', 2: 'GYRO', 1: 'MAGNETOMETER', 0: 'ACCELEROMETER'} sr = px.SingleRegister() self.pozyx.getSelftest(sr, self.remote_id) v = sr.data[0] return {s: ((1 << i) & v) != 0 for i, s in mask.items()}
def check_sensors_calibration(self): try: sr = px.SingleRegister() self.pozyx.getCalibrationStatus(sr) except PozyxException as e: log_pozyx_exception(e) self.reset_or_exit() return v = sr.data[0] # print('calib {0:b} {0}'.format(v)) mask = {3: 'SYS', 2: 'GYRO', 1: 'ACCELEROMETER', 0: 'MAGNETOMETER'} return {s: (((3 << (2 * i)) & v) >> (2 * i)) == 3 for i, s in mask.items()}
def check_visible_devices(self): try: self.pozyx.doDiscovery() list_size = px.SingleRegister() self.pozyx.getDeviceListSize(list_size) if list_size[0] == 0: return device_list = px.DeviceList(list_size=list_size[0]) self.pozyx.getDeviceIds(device_list) for d in device_list.data: rospy.loginfo("Found device 0x%0.4x" % d) except Exception as e: rospy.logerr(e)
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 check_config(self, anchors): list_size = px.SingleRegister() self.pozyx.getDeviceListSize(list_size, self.remote_id) if list_size[0] != len(anchors): rospy.logerr('anchors were not properly configured') return device_list = px.DeviceList(list_size=list_size[0]) self.pozyx.getDeviceIds(device_list, self.remote_id) for anchor in device_list: anchor_coordinates = px.Coordinates() self.pozyx.getDeviceCoordinates( anchor, anchor_coordinates, self.remote_id) rospy.loginfo("anchor 0x%0.4x set to %s", anchor, anchor_coordinates)
def _get_device_list(self): # gets the device list device_list = [] device_list_size = pypozyx.SingleRegister() status = self.local.getDeviceListSize(device_list_size) self._check_status("get device list size", status) devices = pypozyx.DeviceList(device_list_size.value) status = self.local.getDeviceIds(devices) self._check_status("get device ids", status) for id in devices: device_list.append(id) return device_list
def _check_status(self, operation_name, status): # checks the status of the operation if status == pypozyx.POZYX_SUCCESS: return if status == pypozyx.POZYX_TIMEOUT: raise Exception(operation_name + " timed out") error_code = pypozyx.SingleRegister() stat = self.local.getErrorCode(error_code) if stat != pypozyx.POZYX_SUCCESS: raise Exception("failed to get error code") code = error_code[0] raise Exception(getErrorMessage(operation_name, 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: # 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 __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()