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()