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)))
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
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
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)
if exportFile is "y": f.write(publishString + "\n")
if exportMQTT is "y": client.publish(topic, publishString)
if exportAPI is "y":
pos = str(positionTag).split(" ")
xPos = pos[1][:-1]
yPos = pos[3][:-1]
zPos = pos[5]
requests.put(apiUrl + "/" + apiTag + "/" + xPos + "/" + yPos +
"/" + zPos)
#Exception exit
except:
if exportFile is "y": f.close()
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.
if status == POZYX_SUCCESS:
# print the container. Note how a SingleRegister will print as a hex string by default.
print(who_am_i) # will print '0x43'
# and repeat
# initialize the data container
acceleration = Acceleration()
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)))
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 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
