def findSelfPos(self, event):
mac = get_if_hwaddr(self.interface)
servers = self.client.discover()
while(len(servers) < 3):
rospy.loginfo("Found only " + str(len(servers)) + " servers!")
return
otherServers = [x for x in servers if x.find(self.robotName) == -1]
# This will call the other servers' WiFiTrilatServices. To do this, we enter our own MAC address,
# then take two servers that are NOT ours.
responses = self.client.getDistances(mac, otherServers, time.time(), 50)
#goodServers = [x.srv_name for x in responses if x.distance != -1]
goodResponses = [x for x in responses if x.distance != -1]
for x in responses:
print x.srv_name
print x.distance
goodServers = [x.srv_name for x in goodResponses]
if len(goodServers) < 2:
rospy.loginfo("Not enough servers!")
return
srvToUse = []
for x in goodServers:
srvToUse.append(x)
srvToUse.append(self.robotName)
srvToUse = sorted(srvToUse)
#serverNames = sorted([s[1:s.find('/WiFi')] for s in servers])
index = srvToUse.index(self.robotName)
# Next we need to find the distance between the two other servers
print "Host 0 is " + goodServers[0]
print "Host 1 is " + goodServers[1]
goodResponses.append(self.client.getDistances(self.client.IPtoMAC(self.client.hostToIP(goodServers[0])), ["/" + goodServers[1] + "/WiFiTrilat"])[0])
# We translate the indices, so each server will end up building the same triangle.
indexTrans = [(0, 2, 1), (0, 1, 2), (2, 1, 0)]
myInd = indexTrans[index]
# We take our relative position based on alphabetical order.
try:
[self.x, self.y] = wifiutils.calcFrameOfRef(goodResponses[myInd[0]].distance, goodResponses[myInd[1]].distance, goodResponses[myInd[2]].distance)[index]
except ValueError as e:
rospy.logwarn(str(e))
return
print self.x
print self.y
if self.discoverOnce:
rospy.loginfo("Position resolved; shutting down heartbeat/findpos")
self.discoverTimer.shutdown()
self.heartbeat.shutdown()