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 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 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 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('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_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 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 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 pozyx_positioning_pub(): pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100) rospy.init_node('positioning_pub') try: pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0]) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) pub.publish(Point32(coords.x, coords.y, coords.z))
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_euler_pub(): pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100) rospy.init_node('pozyx_euler_pub') try: pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0]) 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) pub.publish(euler_angles.heading, euler_angles.roll, euler_angles.pitch)
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 pozyx_pose_pub(): pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40) rospy.init_node('pozyx_pose_node') try: pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0]) except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() pozyx.doPositioning(coords, POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) pub.publish(Point(coords.x, coords.y, coords.z), Quaternion(quat.x, quat.y, quat.z, quat.w))
def pozyx_device(**kwargs): p = None device = get_device() while True: device = get_device() if device: break else: rospy.sleep(1) rospy.loginfo('Found device %s', device) while not p and not rospy.is_shutdown(): try: p = px.PozyxSerial(device, **kwargs) except SystemExit: rospy.sleep(1) return p
def pozyx_ranging_pub(): pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100) rospy.init_node('range_info_pub') try: pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0]) 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(str(device_range)) else: rospy.loginfo('ERROR: RANGING')
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 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_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 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 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 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(str(comports()[0]).split(' ')[0]) 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) 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(port): rospy.init_node('uwb_configurator') rospy.loginfo("Configuring device list.") settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS try: pozyx = pypozyx.PozyxSerial(port) 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...")
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))
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()
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() rate.sleep()
def pozyx_pose_pub(): # ------------------ PUBLISHERS ------------------ # pose_pub_0 = rospy.Publisher(tag0_topic_ID, PoseStamped, queue_size=100) pose_pub_1 = rospy.Publisher(tag1_topic_ID, PoseStamped, queue_size=100) rospy.init_node('pozyx_pose_node_2_tags') # Inizializzazione delle porte seriali, cerca automaticament 2 tag (se si lascia una sola tag connessa non funziona e richiede di connettere un dispositivo Pozyx) try: serial_port_0 = pypozyx.get_pozyx_ports()[0] #serial_port_1 = pypozyx.get_pozyx_ports()[1] pozyx_0 = pypozyx.PozyxSerial(serial_port_0) #pozyx_1 = pypozyx.PozyxSerial(serial_port_1) ###################################################################### """ Verifichiamo che il tag posizionato in testa corrisponda effettivamente a pozyx_0 da cui leggeremo altrimenti i due tag vengono scambiati. Questo perche' ad ogni riavvio le porte seriali vengono reinizializzate system_details = DeviceDetails() pozyx_0.getDeviceDetails(system_details, remote_id=None) #Serve per ottenere l'ID della tag che in questo momento viene vista come pozyx_0 e che dovrebbe essere la testa tag_ID = system_details.id print("La testa dovrebbe essere con id 0x%0.4x, attualmente misurata e' id 0x%0.4x" % (tag0_ID, system_details.id)) """ # Se l'ID trovato non corrisponde a quello desiderato per la tag frontale, i due vengono scambiati ''' if tag_ID != tag0_ID : print("Aggiusto le tag") pozyx_0_tmp = pozyx_0 pozyx_0 = pozyx_1 pozyx_1 = pozyx_0_tmp system_details = DeviceDetails() pozyx_0.getDeviceDetails(system_details, remote_id=None) print("Adesso la testa e' 0x%0.4x" % (system_details.id)) ''' ###################################################################### except: rospy.loginfo("Pozyx not connected") return # Stampa le porte seriali attive con tag connesse, poi attende un comando per iniziare a pubblicare rospy.loginfo("Using tag connected to : \n %s", serial_port_0) raw_input("Press any key to continue\n") while not rospy.is_shutdown(): # Inizializzazione delle variabili coords = pypozyx.Coordinates() quat = pypozyx.Quaternion() euler_angles = pypozyx.EulerAngles() # Preparo una lista contenente le due tag da cui leggere pozyx_tag_list = [pozyx_0] # Leggo da una tag alla volta for pozyx in pozyx_tag_list : pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id) pozyx.getQuaternion(quat, remote_id=remote_id) pozyx.getEulerAngles_deg(euler_angles) pose.header.frame_id = UWB_frame_ID pose.header.stamp = rospy.Time.now() # Converto da [mm] a [m] pose.pose.position.x = coords.x / 1000.0 pose.pose.position.y = coords.y / 1000.0 pose.pose.position.z = coords.z / 1000.0 pose.pose.orientation.x = quat.x pose.pose.orientation.y = quat.y pose.pose.orientation.z = quat.z pose.pose.orientation.w = quat.w # Converto da gradi a radianti euler_angles.heading = numpy.deg2rad(euler_angles.heading) euler_angles.roll = numpy.deg2rad(euler_angles.roll) euler_angles.pitch = numpy.deg2rad(euler_angles.pitch) # Il quaternione per la trasformazione tiene conto solo della rotazione su z tf_quat = tf.transformations.quaternion_from_euler(0, 0, -euler_angles.heading) # in base al tag dal quale si sta leggendo, vengono pubblicate posa e tf sui rispettivi topic pose_pub_0.publish(pose) br_0 = tf.TransformBroadcaster() br_0.sendTransform((pose.pose.position.x, pose.pose.position.y, 0), tf_quat, rospy.Time.now(), tag0_frame_ID, UWB_frame_ID ) rospy.loginfo("tag_0|| POS-> X: %f Y: %f Z: %f \t | ANGLE-> roll: %f pitch: %f yaw: %f ||" , pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, euler_angles.roll, euler_angles.pitch, euler_angles.heading)
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
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())