def convert_to_ned(self, vector):
ned_vector = Coordinates()
ned_vector.x = vector.x * self.cos_yaw - vector.y * self.sin_yaw
ned_vector.y = vector.x * self.sin_yaw + vector.y * self.cos_yaw
ned_vector.z = vector.z
return ned_vector
#read setup file
cParser = ConfigParser()
cParser.read('config.ini')
#Change the remote ID for getting coordinates from another device from config file
r_id = int(cParser.get('default','remote_id'),16)
#change the ID's for checking coordinates of another device
anchor1 = int(cParser.get('default','anchor1_id'),16)
anchor2 = int(cParser.get('default','anchor2_id'),16)
anchor3 = int(cParser.get('default','anchor3_id'),16)
anchor4 = int(cParser.get('default','anchor4_id'),16)
#Setup variables
coordinatesAnchor1 = Coordinates()
coordinatesAnchor2 = Coordinates()
coordinatesAnchor3 = Coordinates()
coordinatesAnchor4 = 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:
#Obtain the device coordinates from the device list
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(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))]
# 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)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id)
r.setup()
while True:
remote_id = None
# 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
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(0x6110, 1, Coordinates(2875, 0, 1455)),
DeviceCoordinates(0x6115, 1, Coordinates(0, 2875, 2587)),
DeviceCoordinates(0x6117, 1, Coordinates(0, 0, 2024)),
DeviceCoordinates(0x611E, 1, Coordinates(2875, 2875, 1057))
]
# 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)
#data = [0,0,0,0,0]
#pozyx.getRead(PozyxRegisters.WHO_AM_I, data, remote_id=remote_id)
def get_serial_port_names():
return [port.device for port in get_serial_ports()]
def send_error_msg(msg):
return {'error': msg + ' then refresh the Pozyx tab.'}
if PYPOZYX_INSTALLED:
remote_id = 0x6758
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
dimension = PozyxConstants.DIMENSION_3D
height = 1000
anchors = [
DeviceCoordinates(0x6951, 1, Coordinates(0, 0, 1500)),
DeviceCoordinates(0x6e59, 2, Coordinates(5340, 0, 2000)),
DeviceCoordinates(0x695d, 3, Coordinates(6812, -8923, 2500)),
DeviceCoordinates(0x690b, 4, Coordinates(-541, -10979, 3000)),
DeviceCoordinates(0x6748, 5, Coordinates(6812, -4581, 200))
]
POZYX_CONNECTED_TO_BASE = True
serial_port = get_pozyx_serial_port()
if serial_port is None:
POZYX_CONNECTED_TO_BASE = False
else:
try:
pozyx = PozyxSerial(serial_port)
def observe_control_inputs(self) -> ControlInput:
controlInput = ControlInput()
start_time = time.time()
controlInput.Wheel1aTargetSpeed = self.wheel1a.targetSpeed
controlInput.Wheel2aTargetSpeed = self.wheel2a.targetSpeed
controlInput.Wheel3aTargetSpeed = self.wheel3a.targetSpeed
controlInput.Wheel1bTargetSpeed = self.wheel1b.targetSpeed
controlInput.Wheel2bTargetSpeed = self.wheel2b.targetSpeed
controlInput.Wheel3bTargetSpeed = self.wheel3b.targetSpeed
end_time = time.time()
print("CTRL INPUT wheels:", end_time - start_time)
start_time = time.time()
# Don't store values beyond stop limit
slider1_position = self.slider1.sliderPosition
if slider1_position <= 300 and slider1_position >= -300:
controlInput.Slider1Position = slider1_position
elif slider1_position >= 300:
controlInput.Slider1Position = 300
else:
controlInput.Slider1Position = -300
slider2_position = self.slider2.sliderPosition
if slider2_position <= 300 and slider2_position >= -300:
controlInput.Slider2Position = slider2_position
elif slider2_position >= 300:
controlInput.Slider2Position = 300
else:
controlInput.Slider2Position = -300
slider3_position = self.slider3.sliderPosition
if slider3_position <= 300 and slider3_position >= -300:
controlInput.Slider3Position = slider3_position
elif slider3_position >= 300:
controlInput.Slider3Position = 300
else:
controlInput.Slider3Position = -300
controlInput.Slider1TargetSpeed = self.slider1.targetSpeed
controlInput.Slider2TargetSpeed = self.slider2.targetSpeed
controlInput.Slider3TargetSpeed = self.slider3.targetSpeed
end_time = time.time()
print("CTRL INPUT sliders:", end_time - start_time)
start_time = time.time()
position = Coordinates()
# it seems that sampling the pozyx multiple times in quick succession
# leads to better results >> moved to Inertiameter.py
# for i in range(10):
# new_position = self.inertiaMeter.position
# # position.x = new_position.x if abs(new_position.x) > abs(position.x) else position.x
# # position.y = new_position.y if abs(new_position.y) > abs(position.y) else position.y
# # position.z = new_position.z if abs(new_position.z) > abs(position.z) else position.z
# position.x += new_position.x / 10.0
# position.y += new_position.y / 10.0
# position.z += new_position.z / 10.0
# linear_acceleration = 0 # self.inertiaMeter.linearAcceleration
# angular_position = self.inertiaMeter.angularPosition
# angular_velocity = 0 # self.inertiaMeter.angularVelocity
new_position = self.inertiaMeter.position
position.x += new_position[0]
position.y += new_position[1]
position.z += new_position[2]
linear_acceleration = 0 # self.inertiaMeter.linearAcceleration
angular_position = self.inertiaMeter.angularPosition
angular_velocity = 0 # self.inertiaMeter.angularVelocity
if position is not None:
controlInput.XPos = position.x
controlInput.YPos = position.y
controlInput.ZPos = position.z
if angular_position is not None:
controlInput.PitchAngle = angular_position[1]
controlInput.RollAngle = angular_position[0]
controlInput.YawAngle = angular_position[2]
controlInput.XSpeed = 0.0
controlInput.YSpeed = 0.0
controlInput.ZSpeed = 0.0
controlInput.YawSpeed = 0.0 #angular_velocity.x
controlInput.PitchSpeed = 0.0 #angular_velocity.y
controlInput.RollSpeed = 0.0 #angular_velocity.z
controlInput.XAccel = 0.0 #linear_acceleration.x / 1000
controlInput.YAccel = 0.0 #linear_acceleration.y / 1000
controlInput.ZAccel = 0.0 #linear_acceleration.z / 1000
controlInput.YawAccel = 0.0
controlInput.PitchAccel = 0.0
controlInput.RollAccel = 0.0
controlInput.ObstacleDist_1a2a3a = self.sonar1a2a3a.obstacleDistance
controlInput.ObstacleDist_1a2b3a = self.sonar1a2b3a.obstacleDistance
controlInput.ObstacleDist_1a2b3b = self.sonar1a2b3b.obstacleDistance
controlInput.ObstacleDist_1a2a3b = self.sonar1a2a3b.obstacleDistance
controlInput.ObstacleDist_1b2a3a = self.sonar1b2a3a.obstacleDistance
controlInput.ObstacleDist_1b2b3a = self.sonar1b2b3a.obstacleDistance
controlInput.ObstacleDist_1b2b3b = self.sonar1b2b3b.obstacleDistance
controlInput.ObstacleDist_1b2a3b = self.sonar1b2a3b.obstacleDistance
controlInput.ObstacleDanger_1a2a3a = 0.0
controlInput.ObstacleDanger_1a2b3a = 0.0
controlInput.ObstacleDanger_1a2b3b = 0.0
controlInput.ObstacleDanger_1a2a3b = 0.0
controlInput.ObstacleDanger_1b2a3a = 0.0
controlInput.ObstacleDanger_1b2b3a = 0.0
controlInput.ObstacleDanger_1b2b3b = 0.0
controlInput.ObstacleDanger_1b2a3b = 0.0
controlInput.SpatialDangerXMin = self._safety_module.check_xMin_bounds(
controlInput.XPos)
controlInput.SpatialDangerXMax = self._safety_module.check_xMax_bounds(
controlInput.XPos)
controlInput.SpatialDangerYMin = self._safety_module.check_yMin_bounds(
controlInput.YPos)
controlInput.SpatialDangerYMax = self._safety_module.check_yMax_bounds(
controlInput.YPos)
end_time = time.time()
print("CTRL INPUT pozyx:", end_time - start_time)
start_time = time.time()
removed_input = ControlInput()
if (len(self._experience_memory) > 1000):
removed_input = self._experience_memory.popleft()
self._experience_memory.append(controlInput)
self._update_averages(removed_input, controlInput,
len(self._experience_memory))
end_time = time.time()
print("CTRL INPUT final bit:", end_time - start_time)
return controlInput
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
tag_id = None
# OSC configuration
ip = "127.0.0.1"
network_port = 8888
use_OSC = True # enable to send position data through OSC
osc_udp_client = None
if use_OSC:
osc_udp_client = SimpleUDPClient(ip, network_port)
'''*****************************************************************************************************************'''
# Manually assign anchor ids and coordiante data for calibration
# Coordinates (x, y, z) are in mm
# Coordinates last updated 5/12/18 - see C:\Projects\Pozyx\Documentation\Pozyx_Anchor_Location_Coordinates.xlsx
anchors = [
DeviceCoordinates(0x675a, 1, Coordinates(0, 0, 1300)),
DeviceCoordinates(0x674d, 1, Coordinates(3290, -2670, 130)),
DeviceCoordinates(0x675b, 1, Coordinates(3290, 3510, 2110)),
DeviceCoordinates(0x6717, 1, Coordinates(-280, 3670, 120))
]
'''*****************************************************************************************************************'''
# 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 [does not apply for 3D positioning]
height = 1
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension,
quit()
remote_id = 0x6e69 # remote device network ID
remote = False # whether to use a remote device
if not remote:
remote_id = None
use_processing = False # 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(0x6827, 1, Coordinates(-665, -600, 450)),
DeviceCoordinates(0x6815, 1, Coordinates(1690, -687, 510)),
DeviceCoordinates(0x6102, 1, Coordinates(780, 3136, 460))
]
algorithm = POZYX_POS_ALG_TRACKING # positioning algorithm to use
dimension = POZYX_2D # 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()
pub = rospy.Publisher("beacon", Point, queue_size=10)
rospy.init_node('beacon', anonymous=False)
except Exception as e:
print(e)
return cur.lastrowid
################################################ BEGIN POZYX CODE ################################################
# Set this to true if you are running the app in the CVSS basketball court, if not, set it to false
runInCVSS = False
if runInCVSS:
# IDs of the CVSS tags to position.
tag_ids = [0x6744, 0x6747, 0x6719, 0x6745, 0x6764, 0x671c, 0x671e]
# necessary CVSS anchor data for calibration
anchors = [DeviceCoordinates(0x676d, 1, Coordinates(0, 0, 1850)),
DeviceCoordinates(0x6738, 1, Coordinates(0, 17460, 1850)),
DeviceCoordinates(0x6725, 1, Coordinates(12903, 0, 1850)),
DeviceCoordinates(0x6730, 1, Coordinates(12903, 17460, 1850))
##Provision for the last 2 anchors
#DeviceCoordinates(0x0000, 1, Coordinates(x, y, z)),
#DeviceCoordinates(0x0000, 1, Coordinates(x, y, z)),
]
else:
# IDs of the NYP tags to position.
tag_ids = [0x697d, 0x6946, 0x6973, 0x6969, 0x6960, 0x6e65]
# necessary NYP anchor data for calibration
anchors = [DeviceCoordinates(0x6932, 1, Coordinates(0, 0, 0)),
DeviceCoordinates(0x6e58, 1, Coordinates(7700, 0, 0)),
DeviceCoordinates(0x6e10, 1, Coordinates(0, 5440, 0)),
anchor_ids.append(int(rospy.get_param('~anchor3_id', '0x6E15'), 16))
anchor_coordinates = []
anchor_coordinates.append(
eval(rospy.get_param('~anchor0_coordinates', '0, 0, 1700')))
anchor_coordinates.append(
eval(rospy.get_param('~anchor1_coordinates', '2300, 0, 1700')))
anchor_coordinates.append(
eval(rospy.get_param('~anchor2_coordinates', '0, 4930, 1700')))
anchor_coordinates.append(
eval(rospy.get_param('~anchor3_coordinates', '2300, 4930, 1700')))
anchors = [
DeviceCoordinates(
anchor_ids[i], 1,
Coordinates(anchor_coordinates[i][0], anchor_coordinates[i][1],
anchor_coordinates[i][2])) for i in range(4)
]
algorithm = int(rospy.get_param('~algorithm', POZYX_POS_ALG_UWB_ONLY))
dimension = int(rospy.get_param('~dimension', POZYX_3D))
height = int(rospy.get_param('~height', '1000'))
frequency = int(rospy.get_param('~frequency', '10'))
world_frame_id = str(rospy.get_param('~world_frame_id', 'world'))
tag_frame_id = str(rospy.get_param('~tag_frame_id', 'pozyx'))
# Creating publishers
pose_with_cov_publisher = rospy.Publisher('~pose_with_cov',
PoseWithCovarianceStamped,
queue_size=1)
pose_publisher = rospy.Publisher('~pose', PoseStamped, queue_size=1)
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, 0x6720, 0x6729]
# necessary data for calibration
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
osc_udp_client = None
if use_processing:
osc_udp_client = SimpleUDPClient(ip, network_port)
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]
tag_ids = [0xa001, 0xa002, 0xa003]
# 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
# 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
# [PH]: Coordinates (x, y, z) in mm
#'''
anchors = [
DeviceCoordinates(0x675a, 1, Coordinates(0, 0, 2050)),
DeviceCoordinates(0x674d, 1, Coordinates(7000, 0, 1675)),
DeviceCoordinates(0x675b, 1, Coordinates(7000, 4000, 2095)),
DeviceCoordinates(0x6717, 1, Coordinates(0, 4000, 1855))
]
''' # Relative Height (z) dimensions
anchors = [DeviceCoordinates(0x675a, 1, Coordinates(0, 0, 0)),
DeviceCoordinates(0x674d, 1, Coordinates(7000, 0, -375)),
DeviceCoordinates(0x675b, 1, Coordinates(7000, 4000, 45)),
DeviceCoordinates(0x6717, 1, Coordinates(0, 4000, -195))]
'''
# 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
tag_distance = 0.4
# Relative rotation, degrees
tag_rot = -3.
# Pozyx measurement error, m
pozyx_error = 0.3
# Maximum copter speed, m/s
max_speed = 10.
# Maximum copter rotation speed, deg/s
max_rot_speed = 400
# Enable or disable logging
enable_logging = False
# Global variable to collect data from lazer
distance = 0.0
# Necessary data for calibration
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))
]
# 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
#filter type
filter = PozyxConstants.FILTER_TYPE_MOVING_AVERAGE
def callback(data):
def mockedPosition(cls):
# return Coordinates(random.randint(0, 1000), random.randint(0, 1000), random.randint(0, 1000))
return Coordinates(random.randint(0, 2000), random.randint(0, 2000), random.randint(0, 2000))
def __init__(self):
self.pozyx = PozyxSerial(get_first_pozyx_serial_port())
self.direct = EulerAngles()
self.position = Coordinates()
if not remote:
remote_id = None
# 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
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(0x6E63, 1, Coordinates(0, 0, 2220)),
DeviceCoordinates(0x694A, 1, Coordinates(2200, 5000, 1560)),
DeviceCoordinates(0x6E57, 1, Coordinates(6500, 5000, 2430)),
DeviceCoordinates(0x6957, 1, Coordinates(5350, 0, 1520))]
# 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)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, remote_id)
r.setup()
# Anchor definitions
from pypozyx import DeviceCoordinates, Coordinates
ANCHORS = [
DeviceCoordinates(0x6e64, 1, Coordinates(0, 4200, 2450)),
DeviceCoordinates(0x6e5a, 1, Coordinates(3300, 3460, 1740)),
DeviceCoordinates(0x6e6a, 1, Coordinates(0, 0, 1550)),
DeviceCoordinates(0x6941, 1, Coordinates(4800, 800, 2320))
]
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)
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(0x6140, 1, Coordinates(0, 0, 250)),
DeviceCoordinates(0x6112, 1, Coordinates(0, 2750, 2150)),
DeviceCoordinates(0x6114, 1, Coordinates(3350, 0, 1430)),
DeviceCoordinates(0x611d, 1, Coordinates(3150, 2800, 2270))
]
# 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 = 0
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension,
height, remote_id)
class MultitagPositioning(object):
"""Continuously performs multitag positioning"""
def nicksThing():
def __init__(self,
pozyx,
osc_udp_client,
tag_ids,
anchors,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
dimension=PozyxConstants.DIMENSION_3D,
height=1000):
self.pozyx = pozyx
self.osc_udp_client = osc_udp_client
self.tag_ids = tag_ids
self.anchors = anchors
self.algorithm = algorithm
self.dimension = dimension
self.height = height
def setup(self):
"""Sets up the Pozyx for positioning by calibrating its anchor list."""
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
print(" - System will manually calibrate the tags")
print("")
print(" - System will then auto start positioning")
print("")
if None in self.tag_ids:
for device_id in self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
else:
for device_id in [None] + self.tag_ids:
self.pozyx.printDeviceInfo(device_id)
print("")
print("------------POZYX MULTITAG POSITIONING V{} -------------".
format(version))
print("")
self.setAnchorsManual()
self.printPublishAnchorConfiguration()
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:
self.printPublishPosition(position, tag_id)
else:
self.printPublishErrorCode("positioning", tag_id)
# WORK IN PROGRESS: First attempt at making a function to collect and store the x, y, and z coordinates globally
"""def giveUsDemCoordyBois(self, position, network_id):
#hopefully gives us the coordinates to be able to access outside of the class
x_position = position.x;
y_position = position.y;
z_position = position.z;
print(x_position);
print(y_position);
print(z_position);"""
def printPublishPosition(self, position, network_id):
"""Prints the Pozyx's position and possibly sends it as a OSC packet"""
if network_id is None:
network_id = 0
s = "POS ID: {}, x(mm): {}, y(mm): {}, z(mm): {}".format(
"0x%0.4x" % network_id, position.x, position.y, position.z)
print(s)
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/position",
[network_id, position.x, position.y, position.z])
#NOTE: front tag is the tag connected to the Pi(0x673c/0x0000) and back tag is connected to external power source
if network_id == 0x0000: #if the tag that is having its coordinates measured is the one connected to the Pi
global x_position_front #indicate to program that the global variable for the front tag's x-position is to be used
global y_position_front #indicate to program that the global variable for the front tag's y-position is to be used
global z_position_front #indicate to program that the global variable for the front tag's z-position is to be used
x_position_front = position.x
#set the x-position of the front tag to the x-value output by the Pozyx
y_position_front = position.y
#set the y-position of the front tag to the y-value output by the Pozyx
z_position_front = position.z
#set the z-position of the front tag to the z-value output by the Pozyx
print(x_position_front)
#output the front tag's x-position
print(y_position_front)
#output the front tag's y-position
print(z_position_front)
#output the front tag's z-position
else: #otherwise, if the tag that is having its coordinates measured is the one connected to the external power source
global x_position_back #indicate to program that the global variable for the back tag's x-position is to be used
global y_position_back #indicate to program that the global variable for the back tag's y-position is to be used
global z_position_back #indicate to program that the global variable for the back tag's z-position is to be used
x_position_back = position.x
#set the x-position of the back tag to the x-value output by the Pozyx
y_position_back = position.y
#set the y-position of the back tag to the y-value output by the Pozyx
z_position_back = position.z
#set the z-position of the back tag to the z-value output by the Pozyx
print(x_position_back)
#output the back tag's x-position
print(y_position_back)
#output the back tag's y-position
print(z_position_back)
#output the back tag's z-position
#print("THE LOOP HAS BEEN EXITED"); FOR TESTING: print a statement that will allow us to see how exactly the loop
# running the main body is working and where it is at in its execution
'''NOTE: The following print statements were used previously to check if the global variables for x, y, and z
were being set to the Pozyx's outputs:
print(x_position);
print(y_position);
print(z_position);'''
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
for tag_id in self.tag_ids:
status = self.pozyx.clearDevices(tag_id)
for anchor in self.anchors:
status &= self.pozyx.addDevice(anchor, tag_id)
if len(anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_AUTO, len(anchors))
# enable these if you want to save the configuration to the devices.
def printPublishConfigurationResult(self, status, tag_id):
"""Prints the configuration explicit result, prints and publishes error if one occurs"""
if tag_id is None:
tag_id = 0
if status == POZYX_SUCCESS:
print("Configuration of tag %s: success" % tag_id)
else:
self.printPublishErrorCode("configuration", tag_id)
def printPublishErrorCode(self, operation, network_id):
"""Prints the Pozyx's error and possibly sends it as a OSC packet"""
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code, network_id)
if network_id is None:
network_id = 0
if status == POZYX_SUCCESS:
print("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message(
"/error_%s" % operation, [network_id, error_code[0]])
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
print("Error % s, local error code %s" %
(operation, str(error_code)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/error_%s" % operation,
[0, error_code[0]])
def printPublishAnchorConfiguration(self):
for anchor in self.anchors:
print("ANCHOR,0x%0.4x,%s" %
(anchor.network_id, str(anchor.pos)))
if self.osc_udp_client is not None:
self.osc_udp_client.send_message("/anchor", [
anchor.network_id, anchor.pos.x, anchor.pos.y,
anchor.pos.z
])
sleep(0.025)
if __name__ == "__main__":
# Check for the latest PyPozyx version. Skip if this takes too long or is not needed by setting to False.
check_pypozyx_version = True
if check_pypozyx_version:
perform_latest_version_check()
# shortcut to not have to find out the port yourself.
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
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)
pozyx = PozyxSerial(serial_port)
r = MultitagPositioning(pozyx, osc_udp_client, tag_ids, anchors,
algorithm, dimension, height)
r.setup()
while True:
r.loop()
print("THE FRONT X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the front tag were correct
print("THE FRONT Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the front tag were correct
print("THE FRONT Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the front tag were correct
print("THE BACK X POSITION IS:" + str(x_position_front))
#used to see if the x-positions of the back tag were correct
print("THE BACK Y POSITION IS:" + str(y_position_front))
#used to see if the y-positions of the back tag were correct
print("THE BACK Z POSITION IS:" + str(z_position_front))
#used to see if the z-positions of the back tag were correct
y = int(y_position_front)
x = int(x_position_front)
#break; #NOTE: this was previously used to end the execution of the Pozyx measuring to see if the new variables were working
#Previously used to check if the global variables for the x, y, and z coordinates were functioning properly:
#print(x_position);
#print(y_position);
#print(z_position);'''
import RPi.GPIO as GPIO #Pin setup for Entire Pi
import time
import curses #User Interface
import serial
#pin setup
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)
#motor varibles
FR = GPIO.PWM(13,
50) #Front Right Motor #The value 50 is the Frequency
FL = GPIO.PWM(22, 50) #Front Left Motor #The value 12 is the GPIO pin
RR = GPIO.PWM(15, 50) #Rear Right Motor
RL = GPIO.PWM(18, 50) #Rear Left Motor
FR.start(100)
FL.start(100)
RR.start(100)
RL.start(100)
#curses setup
#screen = curses.initscr()
#curses.noecho()
#curses.cbreak()
#screen.keypad(True)
#User Interface
print('...Loading...')
while True:
#getUpdatedCoordinates()
print(y_position_front)
nicksThing()
if int(y_position_front) > 1400:
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Almost there')
break
else:
print(y_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
while True:
print("turning 90 degrees left")
FR.ChangeDutyCycle(5)
FL.ChangeDutyCycle(5)
RR.ChangeDutyCycle(5)
RL.ChangeDutyCycle(5)
time.sleep(.68)
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
break
while True:
#getUpdatedCoordinates()
print(x_position_front)
nicksThing()
if (x_position_front) > (1400):
FR.ChangeDutyCycle(100)
FL.ChangeDutyCycle(100)
RR.ChangeDutyCycle(100)
RL.ChangeDutyCycle(100)
print('Arrived')
break
else:
print(x_position_front)
FR.ChangeDutyCycle(6.5)
FL.ChangeDutyCycle(8)
RR.ChangeDutyCycle(6.5)
RL.ChangeDutyCycle(8)
print("tada!")
#cleanup
GPIO.cleanup
curses.nobreak()
screen.keypad(0)
curses.echo()
curses.endwin()
def __init__(self, mpstate):
"""Initialise module"""
super(BeaconToGPS, self).__init__(mpstate, "BeaconToGPS", "")
self.anchor_config = None
self.config_file_parser = ConfigParser.ConfigParser()
self.config_file_parser.read(os.getcwd() + '/config/uwb_config.conf')
self.anchor_config = self.config_file_parser.get(
"Anchor", "anchor_coordinates")
if self.anchor_config is None:
print("Need set the anchor coordinate!")
return
self.yaw_deg = self.config_file_parser.getfloat(
"NED", "yaw_form_ned_to_uwb")
if self.yaw_deg is None:
print("Need set the yaw from ned to uwb!")
return
else:
print("NED to UWB yaw:" + str(self.yaw_deg) + " deg")
self.debug = 0
self.debug = self.config_file_parser.getint("SYS", "debug")
if self.debug is None:
self.debug = 0
serial_port_dev = get_first_pozyx_serial_port()
if serial_port_dev is None:
print("No Pozyx connected. Check your USB cable or your driver!")
return
self.pozyx = PozyxSerial(serial_port_dev)
self.anchors = self.anchor_config[1:len(self.anchor_config) -
1].split(";")
self.anchor_list = []
self.position = Coordinates()
self.velocity = Coordinates()
self.pos_last = Coordinates()
self.pos_last_time = 0
self.setup_pozyx()
self.CONSTANTS_RADIUS_OF_EARTH = 6378100.0
self.DEG_TO_RAD = 0.01745329251994329576
self.RAD_TO_DEG = 57.29577951308232087679
self.reference_lat = 36.26586666666667
self.reference_lon = 120.27563333333333
self.reference_lat_rad = self.reference_lat * self.DEG_TO_RAD
self.reference_lon_rad = self.reference_lon * self.DEG_TO_RAD
self.cos_lat = math.cos(self.reference_lat_rad)
self.target_lon_param = self.CONSTANTS_RADIUS_OF_EARTH * self.cos_lat
self.current_lat = 0
self.current_lon = 0
self.tag_pos_ned = Coordinates()
self.tag_velocity_ned = Coordinates()
self.yaw = math.radians(self.yaw_deg)
self.cos_yaw = math.cos(self.yaw)
self.sin_yaw = math.sin(self.yaw)
self.location_update = False
self.location_update_time = 0
self.pos_update_time = 0
self.location_update_freq = 8
self.data = {
'time_usec':
0, # (uint64_t) Timestamp (micros since boot or Unix epoch)
'gps_id': 0, # (uint8_t) ID of the GPS for multiple GPS inputs
'ignore_flags': self.
IGNORE_FLAG_ALL, # (uint16_t) Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided.
'time_week_ms':
0, # (uint32_t) GPS time (milliseconds from start of GPS week)
'time_week': 0, # (uint16_t) GPS week number
'fix_type':
0, # (uint8_t) 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
'lat': 0, # (int32_t) Latitude (WGS84), in degrees * 1E7
'lon': 0, # (int32_t) Longitude (WGS84), in degrees * 1E7
'alt':
0, # (float) Altitude (AMSL, not WGS84), in m (positive for up)
'hdop': 0, # (float) GPS HDOP horizontal dilution of position in m
'vdop': 0, # (float) GPS VDOP vertical dilution of position in m
'vn':
0, # (float) GPS velocity in m/s in NORTH direction in earth-fixed NED frame
've':
0, # (float) GPS velocity in m/s in EAST direction in earth-fixed NED frame
'vd':
0, # (float) GPS velocity in m/s in DOWN direction in earth-fixed NED frame
'speed_accuracy': 0, # (float) GPS speed accuracy in m/s
'horiz_accuracy': 0, # (float) GPS horizontal accuracy in m
'vert_accuracy': 0, # (float) GPS vertical accuracy in m
'satellites_visible': 0 # (uint8_t) Number of satellites visible.
}
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()
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(0x6e1a, 1, Coordinates(0, 0, 1740)),
# #DeviceCoordinates(0x6e2d, 1, Coordinates(10147, -4136, 1780)),
# DeviceCoordinates(0x6e32, 1, Coordinates(3385, 5349, 1200)),
# DeviceCoordinates(0x6e37, 1, Coordinates(7809, 4967, 1200)),
# DeviceCoordinates(0x6e4e, 1, Coordinates(10526, 2008, 1100)),
# DeviceCoordinates(0x6e56, 1, Coordinates(-2366, 2743, 1800))]
anchors = [DeviceCoordinates(0x6e4e, 1, Coordinates(0, 0, 2270)),
DeviceCoordinates(0x6e32, 1, Coordinates(3021, 7231, 2250)),
DeviceCoordinates(0x6e1a, 1, Coordinates(0, 2520, 830)),
DeviceCoordinates(0x6e37, 1, Coordinates(-557, 7813, 1540)),
DeviceCoordinates(0x6e2d, 1, Coordinates(1400, 0, 2070)),
DeviceCoordinates(0x6e56, 1, Coordinates(2800, 0, 1800))]
algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY # positioning algorithm to use
dimension = PozyxConstants.DIMENSION_3D # positioning dimension
height = 1000 # default 1000, height of device, required in 2.5D positioning
filter_type = PozyxConstants.FILTER_TYPE_MOVING_MEDIAN
filter_strength = 5
pozyx = PozyxSerial(serial_port)
r = ReadyToLocalize(pozyx, osc_udp_client, anchors, algorithm, dimension, height, filter_type, filter_strength, remote_id)
r.setup()