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 has_updated_anchors(self, msg): self.frame_id = msg.header.frame_id anchors = [ px.DeviceCoordinates( anchor.id, 1, px.Coordinates(anchor.position.x, anchor.position.y, anchor.position.z)) for anchor in msg.anchors ] self.pozyx.lock.acquire(True) try: self.set_anchors(anchors) self.check_config(anchors) except PozyxException as e: rospy.logerr(e.message) finally: self.pozyx.lock.release()
def has_updated_anchors(self, msg): if not self.ready: return self.frame_id = msg.header.frame_id anchors = [ px.DeviceCoordinates(anchor.id, 1, px.Coordinates( anchor.position.x * 1000.0, anchor.position.y * 1000.0, anchor.position.z * 1000.0)) for anchor in msg.anchors] self.pozyx.lock.acquire(True) try: self.set_anchors(anchors) self.check_config(anchors) except PozyxException as e: log_pozyx_exception(e) self.reset_or_exit() finally: self.pozyx.lock.release()
def init(): global anchors, tags_id rospy.init_node('pozyx_configure') rospy.loginfo("Configuring device list.") tags_param = rospy.get_param("tags/addresses", []) tags_id = [None] + tags_param anchors_param = rospy.get_param("anchors", []) anchors = [] rospy.loginfo(str(anchors_param)) rospy.loginfo(str(tags_param)) for anchor in anchors_param: rospy.loginfo(str(anchor)) rospy.loginfo(str(anchor["address"])) anchors.append( pypozyx.DeviceCoordinates( anchor["address"], anchor["flag"], pypozyx.Coordinates(anchor["position"][0], anchor["position"][1], anchor["position"][2])))
This is intended to be highly customisable, but will also be made with parameters and a launch file in the future. Again, help/suggestions/feedback on these things are highly appreciated. Run this after running the UWB configuration. Automatic calibration at this point in time is highly discouraged. """ import pypozyx import rospy from serial.tools.list_ports import comports # adding None will cause the local device to be configured for the anchors as well. tag_ids = [None, 0x0001, 0x0002, 0x0003, 0x0004] anchors = [ pypozyx.DeviceCoordinates(0x0001, 1, pypozyx.Coordinates(0, 0, 5000)), pypozyx.DeviceCoordinates(0x0002, 1, pypozyx.Coordinates(5000, 0, 1000)), pypozyx.DeviceCoordinates(0x0003, 1, pypozyx.Coordinates(0, 5000, 1000)), pypozyx.DeviceCoordinates(0x0004, 1, pypozyx.Coordinates(5000, 5000, 1000)) ] 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(str(comports()[0]).split(' ')[0]) except: rospy.loginfo("Pozyx not connected")
self.smoothData(pos) self.tb.sendTransform(self.pos, self.quat, rospy.Time.now(), "pozyx", "map" ) if __name__ == '__main__': rospy.init_node('pozyx_pose_node') port = '/dev/ttyACM1' #port = pypozyx.get_serial_ports()[0].device # necessary data for calibration, change the IDs and coordinates yourself anchors = [pypozyx.DeviceCoordinates(0x6e49, 1, pypozyx.Coordinates(0, 0, 590)), pypozyx.DeviceCoordinates(0x6e57, 1, pypozyx.Coordinates(2900, 400, 720)), pypozyx.DeviceCoordinates(0x6e32, 1, pypozyx.Coordinates(100, 3000, 1210)), pypozyx.DeviceCoordinates(0x6e0f, 1, pypozyx.Coordinates(5500, 3200, 910))] algorithm = pypozyx.POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use dimension = pypozyx.POZYX_3D # positioning dimension height = 100 # height of device, required in 2.5D positioning pozyx = pypozyx.PozyxSerial(port) remote_id = None #"0x6e3a" r = ROSPozyx(pozyx, anchors, algorithm, dimension, height,remote_id) try: r.pozyx_pose_pub() except rospy.ROSInterruptException: pass
This is intended to be highly customisable, but will also be made with parameters and a launch file in the future. Again, help/suggestions/feedback on these things are highly appreciated. Run this after running the UWB configuration. Automatic calibration at this point in time is highly discouraged. """ import pypozyx import rospy # adding None will cause the local device to be configured for the anchors as well. #tag_ids = [None, 0x6955, 0x6940, 0x0003, 0x0004] #tag_ids = [None,0x6955, 0x6940, 0x6937, 0x6935] tag_ids = [None] anchors = [ pypozyx.DeviceCoordinates(0x6955, 1, pypozyx.Coordinates(0, 0, 1000)), pypozyx.DeviceCoordinates(0x6940, 1, pypozyx.Coordinates(5000, 0, 1000)), pypozyx.DeviceCoordinates(0x6937, 1, pypozyx.Coordinates(0, 5000, 1000)), pypozyx.DeviceCoordinates(0x6935, 1, pypozyx.Coordinates(5000, 5000, 1000)) ] 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")
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()
#!/usr/bin/env python """ Performs discovery for all tags in range and then configures the positioning anchor list on all the discovered devices. This requires all devices to be on the same UWB settings first, so I highly recommend to run the uwb_configurator node first. """ import pypozyx import rospy # anchors IDs for 0,1,2,3 [0x6902, 0x6E44, 0x6E7A, 0x6E6C] # x y z anchors = [ pypozyx.DeviceCoordinates(0x6902, 1, pypozyx.Coordinates(0, 0, 0)), # o pypozyx.DeviceCoordinates(0x6E44, 1, pypozyx.Coordinates(4340, -321, 0)), # x pypozyx.DeviceCoordinates(0x6E7A, 1, pypozyx.Coordinates(0, 9440, 0)), # y pypozyx.DeviceCoordinates(0x6E6C, 1, pypozyx.Coordinates(4618, 9334, 1028)) ] # z 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
for device_id in self.anchors: try: device_range = self.cluster.rangeTo(device_id, self.tag) h = Header() h.stamp = rospy.Time.from_sec(device_range[0] / 1000) self.range_pub.publish(h, device_id, device_range[1], device_range[2]) except Exception as e: rospy.logerr(e) if __name__ == "__main__": anchors = [ pypozyx.DeviceCoordinates(0x971a, 0, pypozyx.Coordinates(0.05, -0.035, 0.03)), pypozyx.DeviceCoordinates(0x972d, 0, pypozyx.Coordinates(0.05, -0.525, 0.03)), pypozyx.DeviceCoordinates(0x9751, 0, pypozyx.Coordinates(-0.02, -0.035, -0.05)), pypozyx.DeviceCoordinates(0x9733, 0, pypozyx.Coordinates(1.5, -0.035, 0.03)) ] tag = pypozyx.DeviceCoordinates(0x671d, 0, pypozyx.Coordinates(0, 0, 0)) stationaryAnchors = StationaryAnchors(anchors, tag) try: stationaryAnchors.start_node() except rospy.ROSInterruptException: