def pozyx_pose_pub(port1, port2):
global distance
pub = rospy.Publisher('/mavros/vision_pose/pose',
PoseStamped,
queue_size=40)
try:
pozyx1 = PozyxSerial(port1)
except:
rospy.loginfo("Pozyx 1 not connected")
return
try:
pozyx2 = PozyxSerial(port2)
except:
rospy.loginfo("Pozyx 2 not connected")
return
pozyx1.setPositionFilter(filter, 3)
pozyx2.setPositionFilter(filter, 3)
pos1 = Coordinates()
pos2 = Coordinates()
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
yaw = 0.
pos1_old = copy.copy(pos1)
pos2_old = copy.copy(pos2)
yaw_old = 0.
while not rospy.is_shutdown():
status1 = pozyx1.doPositioning(pos1,
dimension=dimension,
algorithm=algorithm)
time_delta1 = (rospy.Time.now() - pose.header.stamp).to_sec()
if (status1 == POZYX_SUCCESS
and distance_2d(pos1, pos1_old) < time_delta1 * max_speed):
status2 = pozyx2.doPositioning(pos2,
dimension=dimension,
algorithm=algorithm)
time_delta2 = (rospy.Time.now() - pose.header.stamp).to_sec()
yaw = atan2(pos2.y - pos1.y, pos2.x - pos1.x) + radians(tag_rot)
if (status2 == POZYX_SUCCESS
and distance_2d(pos2, pos2_old) < time_delta2 * max_speed
and
distance_2d(pos1, pos2) < tag_distance + 2 * pozyx_error
and abs(degrees(yaw - yaw_old)) < time_delta2 *
max_rot_speed): # simple out-of-range value filter
pose.pose.position = Point((pos1.x + pos2.x) / 1000.,
(pos1.y + pos2.y) / 1000., distance)
pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, yaw))
pose.header.stamp = rospy.Time.now()
pub.publish(pose)
#print distance_2d(pos1, pos1_old), time_delta1*max_speed, distance_2d(pos2, pos2_old), time_delta2*max_speed, distance_2d(pos1, pos2), tag_distance + 2*pozyx_error, yaw_old, yaw
pos1_old = copy.copy(pos1)
pos2_old = copy.copy(pos2)
yaw_old = yaw
if enable_logging:
rospy.loginfo(
"POS: %s, QUAT: %s" %
(str(pose.pose.position), str(pose.pose.orientation)))
def __init__(self, anchors):
self.serial = PozyxSerial(self.getSerialport)
self.anchors = anchors
# position calculation algorithm and tracking dimension
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
class DataSet():
def __init__(self):
self.pozyx = PozyxSerial(get_first_pozyx_serial_port())
self.direct = EulerAngles()
self.position = Coordinates()
def get_angle(self, req):
if req.sigin == True:
self.pozyx.getEulerAngles_deg(self.direct, remote_id=None)
return int(self.direct.heading)
def doSomething():
port = get_first_pozyx_serial_port()
print('Port:', port)
p = PozyxSerial(port)
whoami = SingleRegister()
p.regRead(POZYX_WHO_AM_I, whoami)
print('WhoAmI:', whoami)
return port, whoami.data
def __init__(self, anchors):
self.serial_port = get_first_pozyx_serial_port()
print(self.serial_port)
if self.serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
self.pozyx = PozyxSerial(self.serial_port)
#print(self.serial_port)
if (anchors.get('count') == 4):
self.anchors = [
DeviceCoordinates(
0x6110, 1,
Coordinates(
anchors.get('0x6110')[0],
anchors.get('0x6110')[1],
anchors.get('0x6110')[2])),
DeviceCoordinates(
0x6115, 1,
Coordinates(
anchors.get('0x6115')[0],
anchors.get('0x6115')[1],
anchors.get('0x6115')[2])),
DeviceCoordinates(
0x6117, 1,
Coordinates(
anchors.get('0x6117')[0],
anchors.get('0x6117')[1],
anchors.get('0x6117')[2])),
DeviceCoordinates(
0x611e, 1,
Coordinates(
anchors.get('0x611e')[0],
anchors.get('0x611e')[1],
anchors.get('0x611e')[2]))
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
self.height = 1000
#self.pub = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10)
self.pub = rospy.Publisher('/pose', PoseStamped, queue_size=10)
self.pose = PoseStamped()
self.subZ = rospy.Subscriber('range', Int16, self.rangeCallback)
def __init__(self, anchors):
self.anchors = [DeviceCoordinates()]
self.anchors = anchors
self.port = get_first_pozyx_serial_port()
if self.port is None:
self.error = "No Pozyx connected"
return
self.pozyx = PozyxSerial(self.port)
networkId = NetworkID()
status = self.pozyx.getNetworkId(networkId)
self.id = networkId.id
self.CheckStatus(status)
self.ConfigureAnchor(self.id)
def main():
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x1234 # 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)
anchors = [DeviceCoordinates(0x6E2A, 1, Coordinates(0, 0, 3175)),
DeviceCoordinates(0x6E0E, 1, Coordinates(0, 4114, 3175)),
DeviceCoordinates(0x697F, 1, Coordinates(3429, 0, 3175)),
DeviceCoordinates(0x6E6F, 1, Coordinates(3429, 4114, 3175))]
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 1:
r.loop()
if not GPIO.input(buttonPin):
in_use()
maintenance = format_time()
data = {"ID": Id, "X": x, "Y": y, "InUse": inUse, "Maintenance": maintenance}
firebase.post('/Ventilator', data)
time.sleep(5)
def __init__(self):
super().__init__("range_debugger")
self.range_pub = self.create_publisher(String, "range", 10)
self.position_pub = self.create_publisher(Odometry, "odometry/pozyx",
1000)
self.markers_pub = self.create_publisher(MarkerArray,
"odometry/pozyx/markers", 10)
# serial port setting
serial_port = "/dev/ttyACM0"
seiral_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. CHeck your USB cable or your driver!")
quit()
self.pozyx = PozyxSerial(serial_port)
# remote and destination
# But sorry, just 1 tag is useable.
# "None" is setting for use USB-connected tag, "0xXX"(tag id) is to use remote tag.
self.tag_ids = [None] # TODO: To use multiple tags
self.ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
self.range_timer_ = self.create_timer(0.02, self.range_callback)
self.anchors = [
# DeviceCoordinates(0x605b, 1, Coordinates( 0, 0, 0)), # test
# DeviceCoordinates(0x603b, 1, Coordinates( 800, 0, 0)), # test
DeviceCoordinates(0x6023, 1, Coordinates(-13563, -8838,
475)), # ROOM
DeviceCoordinates(0x6e23, 1, Coordinates(-3327, -8849,
475)), # ROOM
DeviceCoordinates(0x6e49, 1, Coordinates(-3077, -2959,
475)), # ROOM
# DeviceCoordinates(0x6e58, 1, Coordinates( -7238, -3510, 475)), # ROOM
DeviceCoordinates(0x6050, 1, Coordinates(-9214, -9102,
475)), # ROOM
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_2D
self.height = 475
self.setup()
def run(self):
pozyx = PozyxSerial(get_first_pozyx_serial_port())
global RID
r = PozyxControl(pozyx, tag_ids, anchors, RID, algorithm, dimension, height)
r.setup()
while isRecording:
r.loop()
def setup_poszyx(config_data):
# TODO: clean out
# shortcut to not have to find out the port yourself
serial_port = get_serial_ports()[1].device
remote_id = hex_string_to_hex(config_data.get(
"remote_device")) if config_data.get("remote_device") else None
# TODO: still not sure what it does
# 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)
anchors = []
for anchor in config_data["anchors"]:
# necessary data for calibration, change the IDs and coordinates yourself
device_coordinates = DeviceCoordinates(
hex_string_to_hex(anchor["label"]), anchor["type"],
Coordinates(anchor["coordinates"]["x-value"],
anchor["coordinates"]["y-value"],
anchor["coordinates"]["z-value"]))
anchors.append(device_coordinates)
# TODO check which algorithm is BEST
algorithm = POZYX_POS_ALG_TRACKING # positioning algorithm to use
dimension = POZYX_3D # positioning dimension
#algorithm = POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
#dimension = POZYX_2_5D
# TODO check what is needed
height = config_data[
"moving_device_height"] # height of device, required in 2.5D positioning
# setup
pozyx = PozyxSerial(serial_port)
localizer_instance = ReadyToLocalize(pozyx, osc_udp_client, anchors,
algorithm, dimension, height,
remote_id)
localizer_instance.setup()
sensor_data = None
if config_data.get("sensor_data"):
sensor_data = Orientation3D(pozyx, osc_udp_client, remote_id)
sensor_data.setup()
return {"localizer": localizer_instance, "sensor_data": sensor_data}
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")
client.loop_start()
while True:
pozyx.doPositioning(positionTag, remote_id=r_id)
publishString = hex(r_id) + ": " + str(positionTag)
print(publishString)
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.
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, Coordinates
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")
position = Coordinates()
pozyx.doPositioning(position)
print(position)
with open('/home/pi/pozyx/position.txt', 'a') as f:
f.write(str(position) + "\n")
anchor1Y = int(cParser.get('default', 'anchor1_Y'))
anchor1Z = int(cParser.get('default', 'anchor1_Z'))
anchor2X = int(cParser.get('default', 'anchor2_X'))
anchor2Y = int(cParser.get('default', 'anchor2_Y'))
anchor2Z = int(cParser.get('default', 'anchor2_Z'))
anchor3X = int(cParser.get('default', 'anchor3_X'))
anchor3Y = int(cParser.get('default', 'anchor3_Y'))
anchor3Z = int(cParser.get('default', 'anchor3_Z'))
anchor4X = int(cParser.get('default', 'anchor4_X'))
anchor4Y = int(cParser.get('default', 'anchor4_Y'))
anchor4Z = int(cParser.get('default', 'anchor4_Z'))
#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:
##Setup anchors
#Clear previous setup
pozyx.clearDevices(remote_id=r_id)
print("List cleared.")
#Anchor variables
anchor1 = DeviceCoordinates(anchor1, 1,
Coordinates(anchor1X, anchor1Y, anchor1Z))
anchor2 = DeviceCoordinates(anchor2, 1,
#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
pozyx.getDeviceCoordinates(anchor1,coordinatesAnchor1,r_id)
print(hex(anchor1) + ": " + str(coordinatesAnchor1))
pozyx.getDeviceCoordinates(anchor2,coordinatesAnchor2,r_id)
print(hex(anchor2) + ": " + str(coordinatesAnchor2))
pozyx.getDeviceCoordinates(anchor3,coordinatesAnchor3,r_id)
print(hex(anchor3) + ": " + str(coordinatesAnchor3))
pozyx.getDeviceCoordinates(anchor4,coordinatesAnchor4,r_id)
print(hex(anchor4) + ": " + str(coordinatesAnchor4))
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)
status = pozyx.clearDevices()
for anchor in anchors:
status &= pozyx.addDevice(anchor)
remote_check = SingleRegister()
pozyx.getWhoAmI(remote_check, remote_id=remote_id)
if remote_check.data == [0]:
remote_id = None
MAX_TRIES = 1000
except PozyxConnectionError:
BUSY_SERIAL = True
#import pypozyx
import sys, time
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, POZYX_SUCCESS, SingleRegister, EulerAngles, Acceleration, UWBSettings
#pozyx = PozyxLib() # PozyxSerial has PozyxLib's functions, just for generality
CURSOR_UP_ONE = '\x1b[1A'
ERASE_LINE = '\x1b[2K'
is_cursor_up = False
#print(pypozyx.get_first_pozyx_serial_port())
pozyx = PozyxSerial(get_first_pozyx_serial_port())
who_am_i = SingleRegister()
# get the data, passing along the container
status = pozyx.getWhoAmI(who_am_i)
acceleration = Acceleration()
euler_angles = EulerAngles()
uwb_settings = UWBSettings()
# check the status to see if the read was successful. Handling failure is covered later.
if status == POZYX_SUCCESS:
# print the container. Note how a SingleRegister will print as a hex string by default.
print('Who Am I: {}'.format(who_am_i)) # will print '0x43'
while True:
# initalize the Pozyx as above
# initialize the data container
# and repeat
# initialize the data container
# get the data, passing along the container
if is_cursor_up:
sys.stdout.write(CURSOR_UP_ONE)
from pypozyx import PozyxSerial, get_first_pozyx_serial_port, UWBSettings
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(uwb_settings)
else:
print("No Pozyx port was found")
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.
}
class RangeDebugger(Node):
def __init__(self):
super().__init__("range_debugger")
self.range_pub = self.create_publisher(String, "range", 10)
self.position_pub = self.create_publisher(Odometry, "odometry/pozyx",
1000)
self.markers_pub = self.create_publisher(MarkerArray,
"odometry/pozyx/markers", 10)
# serial port setting
serial_port = "/dev/ttyACM0"
seiral_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. CHeck your USB cable or your driver!")
quit()
self.pozyx = PozyxSerial(serial_port)
# remote and destination
# But sorry, just 1 tag is useable.
# "None" is setting for use USB-connected tag, "0xXX"(tag id) is to use remote tag.
self.tag_ids = [None] # TODO: To use multiple tags
self.ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
self.range_timer_ = self.create_timer(0.02, self.range_callback)
self.anchors = [
# DeviceCoordinates(0x605b, 1, Coordinates( 0, 0, 0)), # test
# DeviceCoordinates(0x603b, 1, Coordinates( 800, 0, 0)), # test
DeviceCoordinates(0x6023, 1, Coordinates(-13563, -8838,
475)), # ROOM
DeviceCoordinates(0x6e23, 1, Coordinates(-3327, -8849,
475)), # ROOM
DeviceCoordinates(0x6e49, 1, Coordinates(-3077, -2959,
475)), # ROOM
# DeviceCoordinates(0x6e58, 1, Coordinates( -7238, -3510, 475)), # ROOM
DeviceCoordinates(0x6050, 1, Coordinates(-9214, -9102,
475)), # ROOM
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_2D
self.height = 475
self.setup()
def setup(self):
self.setAnchorsManual()
for anchor in self.anchors:
self.get_logger().info("ANCHOR,0x%0.4x,%s" %
(anchor.network_id, str(anchor.pos)))
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(self.anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_MANUAL,
len(self.anchors),
remote_id=tag_id)
self.printPublishConfigurationResult(status, tag_id)
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:
self.get_logger().info("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:
self.get_logger().error("Error %s on ID %s, %s" %
(operation, "0x%0.4x" % network_id,
self.pozyx.getErrorMessage(error_code)))
else:
# should only happen when not being able to communicate with a remote Pozyx.
self.pozyx.getErrorCode(error_code)
self.get_logger().error("Error % s, local error code %s" %
(operation, str(error_code)))
def range_callback(self):
"""Do ranging periodically, and publish visualizasion_msg MarkerArray"""
for tag_id in self.tag_ids:
for anchor in self.anchors:
device_range = DeviceRange()
status = self.pozyx.doRanging(anchor.network_id, device_range,
tag_id)
if status == POZYX_SUCCESS:
self.publishMarkerArray(device_range.distance, anchor)
# self.get_logger().info(f"{device_range.distance}")
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
self.get_logger().error(
"ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
self.get_logger().error(
"ERROR Ranging, couldn't retrieve local error")
self.doPositioning()
def doPositioning(self):
for tag_id in self.tag_ids:
position = Coordinates()
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=tag_id)
quat = Quaternion()
status &= self.pozyx.getNormalizedQuaternion(quat, tag_id)
if status == POZYX_SUCCESS:
self.printPublishPosition(position, tag_id, quat)
else:
self.printPublishErrorCode("positioning", tag_id)
def printPublishPosition(self, position, network_id, quat):
if network_id is None:
network_id = 0
odom = Odometry()
odom.header.stamp = self.get_clock().now().to_msg()
odom.header.frame_id = "pozyx"
odom.pose.pose.position.x = position.x * 0.001
odom.pose.pose.position.y = position.y * 0.001
if self.dimension == PozyxConstants.DIMENSION_3D:
odom.pose.pose.position.z = position.z * 0.001
else:
odom.pose.pose.position.z = float(self.height) / 1000
odom.pose.pose.orientation.x = quat.x
odom.pose.pose.orientation.y = quat.y
odom.pose.pose.orientation.z = quat.z
odom.pose.pose.orientation.w = quat.w
odom.pose.covariance = [
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.03,
]
self.position_pub.publish(odom)
def publishMarkerArray(self, distance, anchor):
"""
Visualization pozyx anchors for Rviz2
Parameters
----------
distance : float
"""
is_3D_estimation = (self.dimension == PozyxConstants.DIMENSION_3D)
pos_x = anchor.pos.x
pos_y = anchor.pos.y
pos_z = anchor.pos.z
markerArray = MarkerArray()
m_id = anchor.network_id
# marker of pozyx pos
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_pos" # namespace
marker_pos.id = m_id
m_id += 1
marker_pos.type = Marker.CUBE
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = 0.07
marker_pos.scale.y = 0.07
marker_pos.scale.z = 0.02
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 0.0
marker_pos.color.g = 1.0
marker_pos.color.b = 0.0
marker_pos.color.a = 1.0
markerArray.markers.append(marker_pos)
# marker of pozyx distance
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_distance" # namespace
marker_pos.id = m_id
m_id += 1
if is_3D_estimation:
marker_pos.type = Marker.SPHERE
marker_pos.scale.z = float(distance) * 2 / 1000
else:
marker_pos.type = Marker.CYLINDER
marker_pos.scale.z = 0.001
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = float(distance) * 2 / 1000
marker_pos.scale.y = float(distance) * 2 / 1000
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 0.0
marker_pos.color.g = 0.5
marker_pos.color.b = 0.5
marker_pos.color.a = 0.1
markerArray.markers.append(marker_pos)
# marker of pozyx label
marker_pos = Marker()
marker_pos.header.frame_id = "/pozyx"
marker_pos.header.stamp = self.get_clock().now().to_msg()
marker_pos.ns = "pozyx_distance_label" # namespace
marker_pos.id = m_id
m_id += 1
marker_pos.type = Marker.TEXT_VIEW_FACING
marker_pos.action = Marker.ADD
marker_pos.lifetime.sec = 1
marker_pos.scale.x = 1.0
marker_pos.scale.y = 1.0
marker_pos.scale.z = 0.2
marker_pos.pose.position.x = pos_x / 1000
marker_pos.pose.position.y = pos_y / 1000
marker_pos.pose.position.z = pos_z / 1000 + 0.5
marker_pos.pose.orientation.x = 0.0
marker_pos.pose.orientation.y = 0.0
marker_pos.pose.orientation.z = 0.0
marker_pos.pose.orientation.w = 1.0
marker_pos.color.r = 1.0
marker_pos.color.g = 1.0
marker_pos.color.b = 1.0
marker_pos.color.a = 1.0
marker_pos.text = f"{float(distance / 1000):.2f}\n{hex(anchor.network_id)}"
markerArray.markers.append(marker_pos)
# Publish markers!
self.markers_pub.publish(markerArray)
class Tag:
def __init__(self, anchors):
self.serial = PozyxSerial(self.getSerialport)
self.anchors = anchors
# position calculation algorithm and tracking dimension
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
def setup(self):
# sets up the Pozyx for positioning by calibrating its anchor list
print("")
print("POZYX POSITIONING Version {}".format(version))
print("-------------------------------------------------------")
print("")
print("- System will manually configure tag")
print("")
print("- System will auto start positioning")
print("")
print("-------------------------------------------------------")
print("")
self.setAnchors()
self.printConfig()
print("")
print("-------------------------------------------------------")
print("")
def setAnchors(self):
# adds the manually measured anchors to the Pozyx's device list one for one
status = self.serial.clearDevices(remote_id=None)
for anchor in self.anchors:
status &= self.serial.addDevice(anchor, remote_id=None)
if len(self.anchors) > 4:
status &= self.serial.setSelectionOfAnchors(PozyxConstants.ANCHOR_SELECT_AUTO,
len(self.anchors),
remote_id=None)
@property
def getSerialport(self):
# serialport connection test
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
return None
else:
return serial_port
def getPosition(self):
# performs positioning and exports the results
position = Coordinates()
try:
status = self.serial.doPositioning(position, self.dimension, self.algorithm, remote_id=None)
if status == POZYX_SUCCESS:
# print("POZYX data:", position)
return position
else:
self.printError("positioning")
except:
self.printError("positioning")
return None
def getOrientation(self):
# reads euler angles (yaw, roll, pitch) and exports the results
orientation = EulerAngles()
status = self.serial.getEulerAngles_deg(orientation)
if status == POZYX_SUCCESS:
# print("POZYX data:", orientation)
return orientation
else:
print("Sensor data not found")
return None
@classmethod
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))
@classmethod
def mockedOrientation(cls):
return EulerAngles(random.randint(0, 30), random.randint(0, 30), random.randint(0, 30))
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)
def printError(self, operation):
# Prints Pozyx's errors
error_code = SingleRegister()
if None is None:
self.serial.getErrorCode(error_code)
print("LOCAL ERROR %s, %s" % (operation, self.serial.getErrorMessage(error_code)))
return
status = self.serial.getErrorCode(error_code, None)
if status == POZYX_SUCCESS:
print("ERROR %s on ID %s, %s" %
(operation, "0x%0.4x" % None, self.serial.getErrorMessage(error_code)))
else:
self.serial.getErrorCode(error_code)
print("ERROR %s, couldn't retrieve remote error code, LOCAL ERROR %s" %
(operation, self.serial.getErrorMessage(error_code)))
from pypozyx import PozyxSerial port = '/dev/ttyACM0' # on UNIX systems this will be '/dev/ttyACMX' p = PozyxSerial(port)
class BeaconToGPS(mp_module.MPModule):
IGNORE_FLAG_ALL = (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_ALT
| mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VDOP
| mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY)
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.
}
def setAnchorsManual(self):
''' config anchor'''
status = self.pozyx.clearDevices()
for temp_anchor in self.anchors:
anchor = temp_anchor[1:len(temp_anchor) - 1].split(",")
pozyx_anchor = DeviceCoordinates(
int(anchor[0], 16), 1,
Coordinates(int(anchor[1]), int(anchor[2]), int(anchor[3])))
status &= self.pozyx.addDevice(pozyx_anchor)
self.anchor_list.append(pozyx_anchor)
if len(self.anchors) > 4:
status &= self.pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO,
len(self.anchors))
def print_anchor_config(self):
print("Anchor coordinate config:")
anchor_coordinate = Coordinates()
uwb_setting = UWBSettings()
for i in range(len(self.anchor_list)):
status = self.pozyx.getDeviceCoordinates(
self.anchor_list[i].network_id, anchor_coordinate)
if status == POZYX_SUCCESS:
print("anchor " + hex(self.anchor_list[i].network_id) +
" coordinate is X: " + str(anchor_coordinate.x) +
" mm; Y: " + str(anchor_coordinate.y) + " mm; Z: " +
str(anchor_coordinate.z) + " mm;")
else:
print("get anchor" + hex(self.anchor_list[i].network_id) +
" coordinate config err")
status = self.pozyx.getUWBSettings(uwb_setting,
self.anchor_list[i].network_id)
if status == POZYX_SUCCESS:
print("UWB Setting, channel: " + str(uwb_setting.channel) +
" Bitrate: " + str(uwb_setting.bitrate) + " Prf: " +
str(uwb_setting.prf) + " Plen: " +
str(uwb_setting.plen) + " Gain: " +
str(uwb_setting.gain_db) + " DB")
else:
print("get UWB Setting err")
def setup_pozyx(self):
self.setAnchorsManual()
if self.debug > 0:
self.print_anchor_config()
self.pozyx.doPositioning(self.pos_last, POZYX_3D, 1000,
POZYX_POS_ALG_TRACKING)
self.pos_last_time = time.time()
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 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 mavlink_packet(self, m):
'''handle mavlink packets'''
pass
def send_gps_message(self):
'''send gps message to fc '''
self.data['lat'] = self.current_lat * 1e7
self.data['lon'] = self.current_lon * 1e7
self.data['alt'] = self.tag_pos_ned.z
self.data['vn'] = self.tag_velocity_ned.x
self.data['ve'] = self.tag_velocity_ned.y
self.data['vd'] = self.tag_velocity_ned.z
self.data['speed_accuracy'] = 0.05
self.data['horiz_accuracy'] = 0.1
self.data['vert_accuracy'] = 0.1
self.data['satellites_visible'] = 20
self.data['time_week_ms'] = 0
self.data['time_usec'] = time.time() * 1e6
self.data['gps_id'] = 0
self.data['time_week'] = 0
self.data['fix_type'] = 5
self.master.mav.gps_input_send(
self.data['time_usec'], self.data['gps_id'],
self.data['ignore_flags'], self.data['time_week_ms'],
self.data['time_week'], self.data['fix_type'], self.data['lat'],
self.data['lon'], self.data['alt'], self.data['hdop'],
self.data['vdop'], self.data['vn'], self.data['ve'],
self.data['vd'], self.data['speed_accuracy'],
self.data['horiz_accuracy'], self.data['vert_accuracy'],
self.data['satellites_visible'])
def global_point_from_vector(self):
self.current_lat = (self.reference_lat_rad + self.tag_pos_ned.x /
self.CONSTANTS_RADIUS_OF_EARTH) * self.RAD_TO_DEG
self.current_lon = (self.reference_lon_rad + self.tag_pos_ned.y /
self.target_lon_param) * self.RAD_TO_DEG
if self.debug == 2:
print("Current lat:" + str(self.current_lat) + "; lon:" +
str(self.current_lon))
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
def idle_task(self):
'''get location by uwb'''
if self.pozyx == None:
print("pozyx dev is none")
return
now = time.time()
if (now - self.pos_update_time) > 1 / self.location_update_freq:
self.get_location()
if self.location_update:
self.pos_update_time = now # just location update done,then update time
self.tag_pos_ned = self.convert_to_ned(self.position) #m
self.tag_velocity_ned = self.convert_to_ned(
self.velocity) #m/s
self.tag_velocity_ned.z = -self.tag_velocity_ned.z #ned
self.global_point_from_vector()
self.send_gps_message()
self.location_update = False
if self.debug == 1:
print("update hz:" +
str(1 / (now - self.location_update_time)))
self.location_update_time = now
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()
class Controller(object):
"""description of class"""
def CheckStatus(self, status):
if status == PozyxConstants.STATUS_FAILURE:
raise ConnectionError('Pozyx function failed')
if status == PozyxConstants.STATUS_TIMEOUT:
raise ConnectionError('Pozyx function timed out')
def __init__(self, anchors):
self.anchors = [DeviceCoordinates()]
self.anchors = anchors
self.port = get_first_pozyx_serial_port()
if self.port is None:
self.error = "No Pozyx connected"
return
self.pozyx = PozyxSerial(self.port)
networkId = NetworkID()
status = self.pozyx.getNetworkId(networkId)
self.id = networkId.id
self.CheckStatus(status)
self.ConfigureAnchor(self.id)
def ConfigureAnchor(self, anchorId):
for devCoords in self.anchors:
if devCoords.network_id == anchorId:
position = devCoords.pos
break
if anchorId == self.id:
anchorId = None
status = self.pozyx.setCoordinates(position, anchorId)
self.CheckStatus(status)
def ConfigureTag(self, tagId):
for anchor in self.anchors:
status = self.pozyx.addDevice(anchor, tagId)
self.CheckStatus(status)
status = self.pozyx.configureAnchors(self.anchors,
PozyxConstants.ANCHOR_SELECT_AUTO,
tagId)
self.CheckStatus(status)
mode = SingleRegister(PozyxConstants.GPIO_DIGITAL_INPUT)
pullup = SingleRegister(PozyxConstants.GPIO_PULL_UP)
status = self.pozyx.setConfigGPIO(1, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(2, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(3, mode, pullup, tagId)
self.CheckStatus(status)
status = self.pozyx.setConfigGPIO(4, mode, pullup, tagId)
self.CheckStatus(status)
def ConfigureUWB(self,
id,
txPower=33.0,
channel=1,
bitrate=0,
gain=67,
prf=2,
preamble=40):
if txPower < 0.0 or txPower > 33.0:
raise ValueError("Invalid txPower")
if id == self.id:
id = None
settings = UWBSettings()
settings.bitrate = bitrate
settings.channel = channel
settings.gain_db = gain
settings.plen = preamble
settings.prf = prf
status = self.pozyx.setUWBSettings(settings, id)
self.CheckStatus(status)
status = self.pozyx.setTxPower(txPower, id)
self.CheckStatus(status)
def GetError(self):
code = SingleRegister()
status = self.pozyx.getErrorCode(code)
self.CheckStatus(status)
return self.pozyx.getErrorMessage(code)
def GetCalibrationStatus(self, tagId):
result = SingleRegister()
status = self.pozyx.getCalibrationStatus(result, tagId)
self.CheckStatus(status)
value = dict()
value['magnetic'] = result.value & 3
value['accelerometer'] = (result.value & 12) >> 2
value['gyroscope'] = (result.value & 48) >> 4
value['system'] = (result.value & 192) >> 6
return value
def GetGPIO(self, tagId, pinId):
value = SingleRegister()
status = self.pozyx.getGPIO(pinId, value, tagId)
self.CheckStatus(status)
return value.value == 1
def GetPosition(self, tagId):
target = Coordinates()
status = self.pozyx.doPositioning(
target,
PozyxConstants.DIMENSION_3D,
algorithm=PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
remote_id=tagId)
self.CheckStatus(status)
result = dict()
result['x'] = target.x
result['y'] = target.y
result['z'] = target.z
return target
def GetEulerAngles(self, tagId):
result = EulerAngles()
status = self.pozyx.getEulerAngles_deg(result, tagId)
self.CheckStatus(status)
res = dict()
res['heading'] = result.heading
res['roll'] = result.roll
res['pitch'] = result.pitch
return res
def GetPressure(self, tagId):
data = Pressure()
status = self.pozyx.getPressure_Pa(data, tagId)
self.CheckStatus(status)
return {'pressure': data.value}
def GetTemperature(self, tagId):
data = Temperature()
status = self.pozyx.getTemperature_c(data, tagId)
self.CheckStatus(status)
return {'temperature': data.value}
def DiscoverTags(self):
status = self.pozyx.doDiscovery(PozyxConstants.DISCOVERY_TAGS_ONLY)
self.CheckStatus(status)
count = SingleRegister()
status = self.pozyx.getDeviceListSize(count)
self.CheckStatus(status)
allIds = DeviceList(list_size=count.value)
status = self.pozyx.getDeviceIds(allIds)
self.CheckStatus(status)
tagIds = list()
for id in allIds:
any = False
for tagId in self.anchors:
if tagId.network_id == id:
any = True
if not any:
tagIds.append(id)
return tagIds
self.anchor2_truepose = Pose()
self.anchor2_truepose = states.pose[index]
found +=1
if found == 4:
return True
else:
return False
if __name__ == '__main__':
try:
sim = (sys.argv[1] == "True" or sys.argv[1] == "true")
if not sim:
p = None
ports = get_pozyx_ports()
for i in range(len(ports)):
try:
if ports[i] != os.environ['CORE_PORT']:
p = PozyxSerial(ports[i])
break
except:
pass
if p is None:
quit()
p.printDeviceInfo()
else:
p = None
Pozyx(sim,p)
except rospy.ROSInterruptException:
pass
class IPozyx(object):
"""Continuously calls the Pozyx positioning function and prints its position."""
def __init__(self, anchors):
self.serial_port = get_first_pozyx_serial_port()
print(self.serial_port)
if self.serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
self.pozyx = PozyxSerial(self.serial_port)
#print(self.serial_port)
if (anchors.get('count') == 4):
self.anchors = [
DeviceCoordinates(
0x6110, 1,
Coordinates(
anchors.get('0x6110')[0],
anchors.get('0x6110')[1],
anchors.get('0x6110')[2])),
DeviceCoordinates(
0x6115, 1,
Coordinates(
anchors.get('0x6115')[0],
anchors.get('0x6115')[1],
anchors.get('0x6115')[2])),
DeviceCoordinates(
0x6117, 1,
Coordinates(
anchors.get('0x6117')[0],
anchors.get('0x6117')[1],
anchors.get('0x6117')[2])),
DeviceCoordinates(
0x611e, 1,
Coordinates(
anchors.get('0x611e')[0],
anchors.get('0x611e')[1],
anchors.get('0x611e')[2]))
]
self.algorithm = PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY
self.dimension = PozyxConstants.DIMENSION_3D
self.height = 1000
#self.pub = rospy.Publisher('/mavros/mocap/pose', PoseStamped, queue_size=10)
self.pub = rospy.Publisher('/pose', PoseStamped, queue_size=10)
self.pose = PoseStamped()
self.subZ = rospy.Subscriber('range', Int16, self.rangeCallback)
def setup(self):
self.setAnchorsManual()
#self.printPublishConfigurationResult()
def rangeCallback(self, msg):
if (msg.data == 500):
self.height = 0
elif (msg.data == 30):
self.height = 0
else:
self.height = msg.data #sleep(0.025)
def run(self, height):
"""Performs positioning and displays/exports the results."""
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
return position
else:
pass
#sleep(0.025)
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.header.stamp = rospy.Time.now()
self.pose.header.frame_id = 'map'
self.pose.pose.position.x = position.x
self.pose.pose.position.y = position.y
#self.pose.pose.position.x = 1.5
#self.pose.pose.position.y = 1.5
self.pose.pose.position.z = self.height
self.pose.pose.orientation.w = 1.0
self.pub.publish(self.pose)
else:
pass
#rospy.spin()
def setAnchorsManual(self):
"""Adds the manually measured anchors to the Pozyx's device list one for one."""
#status = self.pozyx.clearDevices(remote_id=self.remote_id)
status = self.pozyx.clearDevices()
for anchor in self.anchors:
#status &= self.pozyx.addDevice(anchor, remote_id=self.remote_id)
status &= self.pozyx.addDevice(anchor)
if len(self.anchors) > 4:
#status &= self.pozyx.setSelectionOfAnchors(PozyxConstants.ANCHOR_SELECT_AUTO, len(self.anchors), remote_id=self.remote_id)
status &= self.pozyx.setSelectionOfAnchors(
PozyxConstants.ANCHOR_SELECT_AUTO, len(self.anchors))
return status
#change the ID's for setting coordinates of devices from config file
anchor1 = anchor1ID if connectedDevice != anchor1ID else None
anchor2 = anchor2ID if connectedDevice != anchor2ID else None
anchor3 = anchor3ID if connectedDevice != anchor3ID else None
anchor4 = anchor4ID if connectedDevice != anchor4ID else None
#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:
#Set the coordinates in the device
pozyx.setCoordinates(Coordinates(anchor1X,anchor1Y,anchor1Z), anchor1)
pozyx.setCoordinates(Coordinates(anchor2X,anchor2Y,anchor2Z), anchor2)
pozyx.setCoordinates(Coordinates(anchor3X,anchor3Y,anchor3Z), anchor3)
pozyx.setCoordinates(Coordinates(anchor4X,anchor4Y,anchor4Z), anchor4)
print("Coordinates set.")
#get the coordinates of the device
pozyx.getCoordinates(coordinatesAnchor1, anchor1)
if check_pypozyx_version:
perform_latest_version_check()
# hardcoded way to assign a serial port of the Pozyx
serial_port = 'COM12'
# the easier way
serial_port = get_first_pozyx_serial_port()
if serial_port is None:
print("No Pozyx connected. Check your USB cable or your driver!")
quit()
remote_id = 0x605D # the network ID of the remote device
remote = False # whether to use the given remote device for ranging
if not remote:
remote_id = None
destination_id = 0x6e66 # network ID of the ranging destination
# distance that separates the amount of LEDs lighting up.
range_step_mm = 1000
# the ranging protocol, other one is PozyxConstants.RANGE_PROTOCOL_PRECISION
ranging_protocol = PozyxConstants.RANGE_PROTOCOL_PRECISION
pozyx = PozyxSerial(serial_port)
r = ReadyToRange(pozyx, destination_id, range_step_mm, ranging_protocol,
remote_id)
r.setup()
while True:
r.loop()
for uwb_settings in all_discovery_uwb_settings():
pozyx.setUWBSettings(uwb_settings)
pozyx.clearDevices()
pozyx.doDiscoveryAll(slots=3, slot_duration=0.1)
device_list = get_device_list(pozyx)
if device_list:
print("Found on {}".format(uwb_settings))
for device_id in device_list:
uwbOnDevice = UWBSettings()
status = pozyx.getUWBSettings(uwbOnDevice, device_id)
if status == PozyxConstants.STATUS_SUCCESS:
print("\t- {}, {}".format(hex(device_id), uwbOnDevice))
else:
print("\t- {}".format(hex(device_id)))
pozyx.setUWBSettings(original_uwb_settings)
if __name__ == '__main__':
from pypozyx import PozyxSerial, get_first_pozyx_serial_port
pozyx = PozyxSerial(get_first_pozyx_serial_port())
uwb = UWBSettings()
pozyx.getUWBSettings(uwb)
print("Local device on {}".format(uwb))
discover_all_devices(pozyx)
