def getCurrentPosition(self):
position = Coordinates()
totalPosition = Coordinates()
measurement = 0
numberOfMeasurements = 20
while (measurement < numberOfMeasurements):
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
totalPosition.x = totalPosition.x + position.x
totalPosition.y = totalPosition.y + position.y
totalPosition.z = totalPosition.z + position.z
measurement = measurement + 1
else:
"""self.printPublishErrorCode("positioning")"""
totalPosition.x = totalPosition.x / numberOfMeasurements
totalPosition.y = totalPosition.y / numberOfMeasurements
totalPosition.z = totalPosition.z / numberOfMeasurements
return totalPosition
def getTargetData(self, currentPosition, targetPosition):
positionError = Coordinates()
positionError.x = targetPosition.x - currentPosition.x
positionError.y = targetPosition.y - currentPosition.y
targetDistance = math.sqrt(math.pow(positionError.x, 2) + math.pow(positionError.y, 2))
targetAngle = 0
if positionError.x > 0:
if positionError.y > 0:
targetAngle = 90 + math.degrees(math.atan(positionError.x / positionError.y))
else:
if positionError.y == 0:
targetAngle = 0
else:
targetAngle = 270 + math.degrees(math.atan(positionError.x / positionError.y))
else:
if positionError.y > 0:
targetAngle = 90 + math.degrees(math.atan(positionError.x / positionError.y))
else:
if positionError.y == 0:
targetAngle = 180
else:
targetAngle = 270 + math.degrees(math.atan(positionError.x / positionError.y))
return [targetDistance, targetAngle]
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 getCurrentPosition(self):
position = Coordinates()
totalPosition = Coordinates()
status = 0
while not status:
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=self.remote_id)
self.totalPositionX = self.totalPositionX - self.measurementsX.popleft() + position.x
self.totalPositionY = self.totalPositionY - self.measurementsY.popleft() + position.y
self.totalPositionZ = self.totalPositionZ - self.measurementsZ.popleft() + position.z
self.measurementsX.append(position.x)
self.measurementsY.append(position.y)
self.measurementsZ.append(position.z)
totalPosition.x = self.totalPositionX / self.numberOfMeasurements
totalPosition.y = self.totalPositionY / self.numberOfMeasurements
totalPosition.z = self.totalPositionZ / self.numberOfMeasurements
return totalPosition
def loop(self):
"""Performs positioning and prints the results."""
for i, tag in enumerate(self.tags):
position = Coordinates()
angles = EulerAngles()
status = self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm, remote_id=tag)
if status == POZYX_SUCCESS:
#BUFFERx.pop(0)
#BUFFERx.append(int(position.x))
#BUFFERy.pop(0)
#BUFFERy.append(int(position.y))
#tempy = AverageMinMax(BUFFERy)
# tempx = AverageMinMax(BUFFERx)
# BUFFERx[1] = BUFFERx[0]
# BUFFERx[0] = position.x
# BUFFERy[1] = BUFFERy[0]
# BUFFERy[0] = position.y
# tempx = Exponentialfilter(BUFFERx)
# tempy = Exponentialfilter(BUFFERy)
theta= -2*pi/3
position.x = cos(theta)*position.x - sin(theta)*position.y
position.y = sin(theta)*position.x + cos(theta)*position.y
uwb_pos = Twist()
uwb_pos.linear.x = int(position.x) / 1000.
uwb_pos.linear.y = int(position.y) / 1000.
uwb_pos.linear.z = int(position.z) / 1000.
self.printPublishPosition(position, tag)
self.uwb_target_pub.publish(uwb_pos)
return False
else:
self.printPublishErrorCode("positioning", tag)
return True
def 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