def printPublishConfigurationResult(self):
"""Prints and potentially publishes the anchor configuration result in a human-readable way."""
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates, self.remote_id)
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/anchor", [device_list[i], int(anchor_coordinates.x), int(anchor_coordinates.y), int(anchor_coordinates.z)])
sleep(0.025)
def __init__(self,
pozyx,
osc_udp_client,
anchors,
algorithm=POZYX_POS_ALG_UWB_ONLY,
dimension=POZYX_3D,
height=1000,
remote_id=None):
self.pozyx = pozyx
self.osc_udp_client = osc_udp_client
self.anchors = anchors
self.algorithm = algorithm
self.dimension = dimension
self.height = height
self.remote_id = remote_id
self.measurement = 0
self.totalPosition = Coordinates()
def read_calibration_file(filepath):
anchors = []
try:
calibration_file = open(filepath)
except IOError:
rospy.logerror("File {} can't be opened".format(filepath))
quit()
else:
with calibration_file:
csv_reader = csv.reader(calibration_file,
delimiter=';',
quotechar='"')
row_0 = csv_reader.next()
for row in csv_reader: #id;hexId;x;y;z;
id, hexId, x, y, z = row[:5]
anchors.append(
DeviceCoordinates(int(id), 1,
Coordinates(int(x), int(y), int(z))))
return anchors
def loop(self):
"""Performs positioning and prints the results."""
for tag_id in self.tag_ids:
position = Coordinates()
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=tag_id)
if status == POZYX_SUCCESS:
if tag_id:
room_position = get_4d_coordinates(position)
self.tag_dist_traveled[tag_id] = get_distance(room_position, self.listeners[tag_id]) # determines how far tag has traveled since the last update
self.listeners[tag_id] = room_position
self.printPublishPosition(position, tag_id)
self.publish_distance_traveled(tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
self.distance, self.angle, self.middle = get_orientation_info(self.listeners[TAG_IDS[0]],
self.listeners[TAG_IDS[1]])
self.publish_orientation_info()
def pubPozyx(self):
#success=False
#while(success!=True):
position = Coordinates()
status = self.pozyx.doPositioning(
#position, self.dimension, height, self.algorithm)
position,
self.dimension,
self.height,
self.algorithm)
#position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)
if status == POZYX_SUCCESS:
success = True
self.pose.timestamp = datetime.datetime.now()
self.pose.posx = position.x
self.pose.posy = position.y
self.pose.posz = self.height
self.pub.publish(self.pose)
else:
pass
def printPublishConfigurationResult(self):
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
pass
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def get_tag_velocity(self, position_now, time_now):
delt_pos = Coordinates()
#unit: mm
delt_pos.x = position_now.x - self.pos_last.x
delt_pos.y = position_now.y - self.pos_last.y
delt_pos.z = position_now.z - self.pos_last.z
delt_time = (time_now - self.pos_last_time) * 1000.0 #s-->ms
self.pos_last = position_now
self.pos_last_time = time_now
self.velocity.x = delt_pos.x / delt_time
#m/s
self.velocity.y = delt_pos.y / delt_time
self.velocity.z = delt_pos.z / delt_time
if self.debug == 2:
print("Tag velocity is X=" + str(self.velocity.x) + "m/s; Y=" +
str(self.velocity.y) + "m/s Z=" + str(self.velocity.z) +
"m/s")
def printPublishConfigurationResultMore(self, remote_id):
if not (remote_id is None):
print("Pozyx ID: 0x%0.4x" % remote_id)
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def printPublishConfigurationResult(self):
"""Prints and potentially publishes the anchor configuration result in a human-readable way."""
list_size = SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
print("List size: {0}".format(list_size[0]))
if list_size[0] != len(self.anchors):
self.printPublishErrorCode("configuration")
return
device_list = DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.pozyx.getDeviceCoordinates(device_list[i], anchor_coordinates,
self.remote_id)
print("ANCHOR, 0x%0.4x, %s" %
(device_list[i], str(anchor_coordinates)))
def setup(self):
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
print("------------POZYX POSITIONING V{} -------------".format(version))
print("")
print("- System will manually configure tag")
print("")
print("- System will auto start positioning")
print("")
if self.remote_id is None:
self.pozyx.printDeviceInfo(self.remote_id)
else:
for device_id in [None, self.remote_id]:
self.pozyx.printDeviceInfo(device_id)
print("")
print("------------POZYX POSITIONING V{} -------------".format(version))
print("")
self.setAnchorsManual(save_to_flash=False)
self.printPublishConfigurationResult()
position = Coordinates()
measurement = 0
while (measurement < self.numberOfMeasurements):
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)
if status == POZYX_SUCCESS:
if (measurement == 0):
self.measurementsX = deque([position.x])
self.measurementsY = deque([position.y])
self.measurementsZ = deque([position.z])
else:
self.measurementsX.append(position.x)
self.measurementsY.append(position.y)
self.measurementsZ.append(position.z)
self.totalPositionX = self.totalPositionX + self.measurementsX[-1]
self.totalPositionY = self.totalPositionY + self.measurementsY[-1]
self.totalPositionZ = self.totalPositionZ + self.measurementsZ[-1]
measurement = measurement + 1
def get_location(self):
"""Performs positioning and displays/exports the results."""
pos_mm = Coordinates()
status = self.pozyx.doPositioning(pos_mm, POZYX_3D, 1000,
POZYX_POS_ALG_TRACKING)
now = time.time()
if status == POZYX_SUCCESS:
pos_err = PositionError()
self.pozyx.getPositionError(pos_err)
self.position.x = pos_mm.x * 0.001 #mm-->m
self.position.y = pos_mm.y * 0.001
self.position.z = pos_mm.z * 0.001
self.get_tag_velocity(pos_mm, now)
self.location_update = True
if self.debug == 2:
print(" Postion is X: " + str(self.position.x) + " m; Y: " +
str(self.position.y) + " m; Z: " + str(self.position.z) +
" m;" + " err: " + str(pos_err.xy))
else:
if self.debug == 2:
print("Do not get tag position")
def loop(self):
"""Performs positioning and displays/exports the results."""
position = Coordinates()
sensor_data = SensorData()
self.publishData(position, sensor_data) #nur ohne anchors
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
self.publishData(position, sensor_data)
calibration_status = SingleRegister()
if self.remote_id is not None or self.pozyx.checkForFlag(
POZYX_INT_MASK_IMU, 0.01) == POZYX_SUCCESS:
status = self.pozyx.getAllSensorData(sensor_data, self.remote_id)
status &= self.pozyx.getCalibrationStatus(calibration_status,
self.remote_id)
def loop(self):
"""Performs positioning and prints the results."""
for i, tag in enumerate(self.tags):
position = Coordinates()
angles = EulerAngles()
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=tag)
if status == POZYX_SUCCESS:
#BUFFERx.pop(0)
#BUFFERx.append(int(position.x))
#BUFFERy.pop(0)
#BUFFERy.append(int(position.y))
#tempy = AverageMinMax(BUFFERy)
# tempx = AverageMinMax(BUFFERx)
# BUFFERx[1] = BUFFERx[0]
# BUFFERx[0] = position.x
# BUFFERy[1] = BUFFERy[0]
# BUFFERy[0] = position.y
# tempx = Exponentialfilter(BUFFERx)
# tempy = Exponentialfilter(BUFFERy)
theta= -2*pi/3
position.x = cos(theta)*position.x - sin(theta)*position.y
position.y = sin(theta)*position.x + cos(theta)*position.y
uwb_pos = Twist()
uwb_pos.linear.x = int(position.x) / 1000.
uwb_pos.linear.y = int(position.y) / 1000.
uwb_pos.linear.z = int(position.z) / 1000.
self.printPublishPosition(position, tag)
self.uwb_target_pub.publish(uwb_pos)
return False
else:
self.printPublishErrorCode("positioning", tag)
return True
def get_position(self):
"""Get position of tag."""
position = Coordinates()
pos_err = PositionError()
status_pos = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status_pos == POZYX_SUCCESS:
status_err = self.pozyx.getPositionError(pos_err,
remote_id=self.remote_id)
if status_err == POZYX_SUCCESS:
self.printPublishPosition(
position) #display position on remote computer
print(position)
self.send_vehicle_position(position,
pos_err) #send to ardupilot
else:
self.printPublishErrorCode("positioning")
def loop(self):
"""Performs positioning and prints the results."""
for tag_id in self.tag_ids:
position = Coordinates()
orientation = EulerAngles()
acceleration = Acceleration()
try:
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
status &= self.pozyx.getEulerAngles_deg(orientation, tag_id)
status &= self.pozyx.getAcceleration_mg(acceleration, tag_id)
if status == POZYX_SUCCESS:
#self.printPublishPosition(position, tag_id)
self.printPublishPositionWithOrientation(
position, orientation, acceleration, tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
except struct.error as err:
print(err)
def getTargetData(self, currentPosition, targetPosition):
positionError = Coordinates()
positionError.x = targetPosition.x - currentPosition.x
positionError.y = targetPosition.y - currentPosition.y
targetDistance = math.sqrt(
math.pow(positionError.x, 2) + math.pow(positionError.y, 2))
targetAngle = 0
if positionError.x > 0:
targetAngle = 180 - math.degrees(
math.atan(positionError.x / positionError.y))
else:
if positionError.x < 0:
targetAngle = -math.degrees(
math.atan(positionError.x / positionError.y))
else:
if positionError.y > 0:
targetAngle = 90
else:
targetAngle = 270
return [targetDistance, targetAngle]
def pozyx_pose_pub(pozyx):
global distance
pub = rospy.Publisher('/mavros/vision_pose/pose',
PoseStamped,
queue_size=40)
pozyx.setPositionAlgorithm(algorithm=algorithm, dimension=dimension)
pozyx.setPositionFilter(filter_type=filter_type,
filter_strength=filter_strength)
pos = Coordinates()
pose = PoseStamped()
pose.pose.orientation = Quaternion(0, 0, 0, 1)
pose.header.frame_id = "map"
while not rospy.is_shutdown():
status = pozyx.doPositioning(pos,
dimension=dimension,
algorithm=algorithm,
remote_id=remote_id)
if status == POZYX_SUCCESS:
pose.pose.position = Point(pos.x / 1000., pos.y / 1000., distance)
pose.header.stamp = rospy.Time.now()
pub.publish(pose)
if enable_logging:
rospy.loginfo("POS: %s" % str(pos))
def printConfig(self):
# prints the anchor configuration result
list_size = SingleRegister()
# prints the anchors list size
self.serial.getDeviceListSize(list_size, None)
if list_size[0] != len(self.anchors):
self.printError("configuration")
return
# prints the anchors list
device_list = DeviceList(list_size=list_size[0])
self.serial.getDeviceIds(device_list, None)
print("Calibration result:")
print("Anchors found: {0}".format(list_size[0]))
print("Anchor IDs: ", device_list)
for i in range(list_size[0]):
anchor_coordinates = Coordinates()
self.serial.getDeviceCoordinates(device_list[i], anchor_coordinates, None)
print("ANCHOR: 0x%0.4x, %s" % (device_list[i], str(anchor_coordinates)))
sleep(0.025)
remote_id = None
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
# necessary data for calibration, change the IDs and coordinates yourself according to your measurement
anchors = [
DeviceCoordinates(0x670c, 1, Coordinates(5933, 9600, 2200)),
DeviceCoordinates(0x6711, 1, Coordinates(200, 0, 2989)),
DeviceCoordinates(0x674b, 1, Coordinates(5409, 0, 2220))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_2_5D
# height of device, required in 2.5D positioning
height = 1000
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension,
height, remote_id)
r.setup()
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
# IDs of the tags to position, add None to position the local tag as well.
tag_ids = [None, 0x6728]
# necessary data for calibration
anchors = [
DeviceCoordinates(0x6e09, 1, Coordinates(0, 0, 0)),
DeviceCoordinates(0x674c, 1, Coordinates(1650, 0, 0)),
DeviceCoordinates(0x6729, 1, Coordinates(0, 480, 0)),
DeviceCoordinates(0x6765, 1, Coordinates(1650, 480, 0))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_2D
# height of device, required in 2.5D positioning
height = 1000
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
# NIDPerformance = Performance([Coordinates(-4295, -10331, 2000),
# Coordinates(4295, -10046, 1752),
# Coordinates(4295, 10046, 268),
# Coordinates(-4295, 10331, 500),],
# # Coordinates(-4325, 0, 1800),
# # Coordinates(0, -3980, 2000),
# # Coordinates(4385, -130, 1700),
# # Coordinates(0, 3880, 850)],
# 8.0,
# 12.0,
# 60.0)
# # ---------- COPY PASTE END ----------
# ---------- COPY PASTE START ----------
# Modify Coordinates(X,Y,Z) and performance space X,Y (!size/2) and duration of each run here.
# Anchor order is sequential 1, 2, 3, 4 ... and should match InertiaMeter.py
CourtyardPerformance = Performance(
[
Coordinates(-6500, -5600, 1515),
Coordinates(4500, -5600, 2000),
Coordinates(4500, 4400, 1000),
Coordinates(-6500, 4400, 480),
],
# Coordinates(-4325, 0, 1800),
# Coordinates(0, -3980, 2000),
# Coordinates(4385, -130, 1700),
# Coordinates(0, 3880, 850)],
9.0,
6.0,
60.0)
# ---------- COPY PASTE END ----------
quit()
remote_id = 0x672d # remote device network ID
remote = False # whether to use a remote device
if not remote:
remote_id = None
use_processing = True # enable to send position data through OSC
ip = "127.0.0.1" # IP for the OSC UDP
network_port = 8888 # network port for the OSC UDP
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
# necessary data for calibration, change the IDs and coordinates yourself
anchors = [
DeviceCoordinates(0x6e4b, 1, Coordinates(0, 0, 300)),
DeviceCoordinates(0x6e67, 1, Coordinates(3630, 0, 300)),
DeviceCoordinates(0x6e3a, 1, Coordinates(0, 2430, 300)),
DeviceCoordinates(0x6e68, 1, Coordinates(3630, 2430, 300))
]
algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = POZYX_3D # positioning dimension
height = 1000 # height of device, required in 2.5D positioning
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension,
height, remote_id)
r.setup()
while True:
r.loop()
"""
import pypozyx
import rospy
import argparse
from pypozyx import (POZYX_POS_ALG_UWB_ONLY, POZYX_3D, Coordinates, POZYX_SUCCESS, PozyxConstants, version,
DeviceCoordinates, PozyxSerial, get_first_pozyx_serial_port, SingleRegister, DeviceList, PozyxRegisters)
# Set serial port or leave it empty to make auto connect
serial_port = ''
# adding None will cause the local device to be configured for the anchors as well.
tag_ids = [None]
anchors = [DeviceCoordinates(0x6a11, 1, Coordinates(-108, 12145, 2900)),
DeviceCoordinates(0x6a19, 1, Coordinates(5113, 11617, 2900)),
DeviceCoordinates(0x6a6b, 1, Coordinates(0, 0, 2900)),
DeviceCoordinates(0x676d, 1, Coordinates(4339, 0, 2800)),
DeviceCoordinates(0x6a40, 1, Coordinates(672, 5127, 100))]
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")
if not remote:
remote_id = None
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
# necessary data for calibration, change the IDs and coordinates yourself according to your measurement
anchors = [DeviceCoordinates(0x674c, 1, Coordinates(1660, 0, 2750)),
DeviceCoordinates(0x6765, 1, Coordinates(1660, -520, 2750)),
DeviceCoordinates(0x6e09, 1, Coordinates(0, 0, 2750)),
DeviceCoordinates(0x6729, 1, Coordinates(0, -520, 2750))]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_2D
# height of device, required in 2.5D positioning
height = 2750
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id)
r.setup()
while True:
#Change the remote ID for positioning from another device from config file
r_id = int(cParser.get('default', 'remote_id'), 16)
#Set Export properties
exportFile = eParser.get('default', 'export_file')
fileName = eParser.get('default', 'file_name')
exportMQTT = eParser.get('default', 'export_mqtt')
host = eParser.get('default', 'mqtt_host')
topic = eParser.get('default', 'mqtt_topic')
clientID = eParser.get('default', 'mqtt_clientid')
exportAPI = eParser.get('default', 'export_api')
apiUrl = eParser.get('default', 'api_url')
apiTag = eParser.get('default', 'api_tag')
#Setup variables
positionTag = Coordinates()
#Check if connected
serial_port = get_first_pozyx_serial_port()
if serial_port is not None:
pozyx = PozyxSerial(serial_port)
print("Connection success!")
else:
print("No Pozyx port was found")
exit()
try:
if exportFile is "y": f = open("position.txt", "a+")
if exportMQTT is "y":
client = mqtt.Client(client_id=clientID)
client.connect("broker.mqttdashboard.com")
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings, Coordinates, DeviceCoordinates
# initalize the Pozyx as above
serial_port = get_first_pozyx_serial_port()
if serial_port is not None:
pozyx = PozyxSerial(serial_port)
uwb_settings = UWBSettings()
pozyx.getUWBSettings(uwb_settings)
print(f'UWB Settings: {uwb_settings}')
else:
print("No Pozyx port was found")
# assume an anchor 0x6140 that we want to add to the device list and immediately save the device list after.
anchor = DeviceCoordinates(0x6140, 0, Coordinates(000, 0000, 0))
print(anchor)
pozyx.addDevice(anchor)
pozyx.saveNetwork()
# after, we can start positioning. Positioning takes its parameters from the configuration in the tag's
# registers, and so we only need the coordinates.
position = Coordinates()
print(f'Position before positioning: {position}')
pozyx.doPositioning(position)
print(f'Position after positioning: {position}')
# initialize the data container
who_am_i = SingleRegister()
# get the data, passing along the container
status = pozyx.getWhoAmI(who_am_i)
# check the status to see if the read was successful. Handling failure is covered later.
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
# enable to send position data through OSC
use_processing = True
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
# IDs of the tags to position, add None to position the local tag as well.
tag_ids = [None, 0x6e66]
# necessary data for calibration
anchors = [
DeviceCoordinates(0x6058, 1, Coordinates(0, 0, 2210)),
DeviceCoordinates(0x6005, 1, Coordinates(9200, -700, 2400)),
DeviceCoordinates(0x605C, 1, Coordinates(0, 17000, 2470)),
DeviceCoordinates(0x6027, 1, Coordinates(2700, 17000, 2470)),
DeviceCoordinates(0x682e, 1, Coordinates(2700, 2500, 2350)),
DeviceCoordinates(0x6044, 1, Coordinates(11230, 2750, 2300))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_3D
# height of device, required in 2.5D positioning
height = 1000
osc_udp_client = None
osc_udp_client = SimpleUDPClient(ip, network_port)
# -----------------------------------------------------------------------------------------------------------------------------------------
""" [PH]: Manually assign TAG IDs (hexadecimal value) for remote positioning """
# NOTES:
# If a local tag is connected (ie. to PC via USB), change its tag id to "None"
# eg: tag_ids = [None, 0x6720, 0x6729] # TDMA order: Local, 0x6720, 0x6729
# *Locally connected tag may not transmit orientation data
tag_ids = [0x0001, 0x0002]
#tag_ids = [0x6720, 0x6729, 0x6735] # 3 x Tags
""" [PH]: Manually assign ANCHOR IDs and coordinate data for calibration """
# Coordinates (x, y, z) are in mm
# Coordinates last updated 15/11/18
# refer C:\Projects\Pozyx\Anchor Location Coordinates\Pozyx_Anchor_Location_Coordinates.xlsx
anchors = [
DeviceCoordinates(0x675a, 1, Coordinates(0, 0, 1675)),
DeviceCoordinates(0x674d, 1, Coordinates(7000, -1810, 1175)),
DeviceCoordinates(0x675b, 1, Coordinates(7000, 2190, 1990)),
DeviceCoordinates(0x6717, 1, Coordinates(0, 2190, 2225))
]
# -----------------------------------------------------------------------------------------------------------------------------------------
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_3D
# height of device, required in 2.5D positioning
height = 1000
pozyx = PozyxSerial(serial_port)
quit()
remote_id = 0x6069 # remote device network ID
remote = False # whether to use a remote device
if not remote:
remote_id = None
use_processing = True # enable to send position data through OSC
ip = "127.0.0.1" # IP for the OSC UDP
network_port = 8888 # network port for the OSC UDP
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
# necessary data for calibration, change the IDs and coordinates yourself
anchors = [
DeviceCoordinates(0xA001, 1, Coordinates(0, 0, 2790)),
DeviceCoordinates(0xA002, 1, Coordinates(10490, 0, 2790)),
DeviceCoordinates(0xA003, 1, Coordinates(-405, 6000, 2790)),
DeviceCoordinates(0xA004, 1, Coordinates(10490, 6500, 2790))
]
algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = POZYX_3D # positioning dimension
height = 1000 # height of device, required in 2.5D positioning
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension,
height, remote_id)
r.setup()
while True:
r.loop()
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
# enable to send position data through OSC
use_processing = False
# configure if you want to route OSC to outside your localhost. Networking knowledge is required.
ip = "127.0.0.1"
network_port = 8888
# IDs of the tags to position, add None to position the local tag as well.
tag_ids = [0x671a, 0x676e]
# necessary data for calibration
anchors = [
DeviceCoordinates(0x6714, 1, Coordinates(0, 0, 1500)),
DeviceCoordinates(0x6711, 1, Coordinates(785, 2900, 1200)),
DeviceCoordinates(0x6717, 1, Coordinates(5416, 3670, 2000)),
DeviceCoordinates(0x675b, 1, Coordinates(5000, 0, 1800)),
DeviceCoordinates(0x6743, 1, Coordinates(3140, 4252, 900)),
DeviceCoordinates(0x671b, 1, Coordinates(6050, 1400, 1000))
]
# positioning algorithm to use, other is PozyxConstants.POSITIONING_ALGORITHM_TRACKING
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
#algorithm = PozyxConstants.POSITIONING_ALGORITHM_TRACKING
# positioning dimension. Others are PozyxConstants.DIMENSION_2D, PozyxConstants.DIMENSION_2_5D
dimension = PozyxConstants.DIMENSION_3D
# height of device, required in 2.5D positioning
height = 1000