def platoon(other):
driving.drive(0)
while other not in g.otherpos:
time.sleep(1)
print("a queue appeared for %s" % other)
q = g.otherpos[other]
follow = False
lastx = None
laxty = None
slowsp = 15
fastsp = 25
goingslow = True
tb1 = None
while True:
#print("q length %d" % q.qsize())
(x1, y1) = g.posnow[other]
t0 = time.time()
(x, y) = q.get()
t1 = time.time()
#print("got (%f %f) (%f %f)" % (x, y, x1, y1))
q.task_done()
if y1 > 15.0 and not follow:
follow = True
driving.drive(fastsp)
if not follow:
continue
if t1-t0 > 0.1:
print("waited %f" % (t1-t0))
near = 0.5+0.3
near = 0.6+0.3
d = dist(x1, y1, g.ppx, g.ppy)
if d < near and not goingslow:
print("dist %f, speed %d" % (d, slowsp))
goingslow = True
driving.drive(slowsp)
if d > near and goingslow:
print("dist %f, speed %d" % (d, fastsp))
goingslow = False
driving.drive(fastsp)
if lastx != None:
if dist(x, y, lastx, lasty) < 0.5:
continue
lastx = x
lasty = y
tb0 = time.time()
if tb1 != None and tb0-tb1 > 0.1:
print("waitedb %f" % (tb0-tb1))
status = nav2.goto_1(x, y)
#print((status, g.ppx, g.ppy))
tb1 = time.time()
def platoon(other):
driving.drive(0)
while other not in g.otherpos:
time.sleep(1)
print("a queue appeared for %s" % other)
q = g.otherpos[other]
follow = False
lastx = None
laxty = None
slowsp = 15
fastsp = 25
goingslow = True
tb1 = None
while True:
#print("q length %d" % q.qsize())
(x1, y1) = g.posnow[other]
t0 = time.time()
(x, y) = q.get()
t1 = time.time()
#print("got (%f %f) (%f %f)" % (x, y, x1, y1))
q.task_done()
if y1 > 15.0 and not follow:
follow = True
driving.drive(fastsp)
if not follow:
continue
if t1 - t0 > 0.1:
print("waited %f" % (t1 - t0))
near = 0.5 + 0.3
near = 0.6 + 0.3
d = dist(x1, y1, g.ppx, g.ppy)
if d < near and not goingslow:
print("dist %f, speed %d" % (d, slowsp))
goingslow = True
driving.drive(slowsp)
if d > near and goingslow:
print("dist %f, speed %d" % (d, fastsp))
goingslow = False
driving.drive(fastsp)
if lastx != None:
if dist(x, y, lastx, lasty) < 0.5:
continue
lastx = x
lasty = y
tb0 = time.time()
if tb1 != None and tb0 - tb1 > 0.1:
print("waitedb %f" % (tb0 - tb1))
status = nav2.goto_1(x, y)
#print((status, g.ppx, g.ppy))
tb1 = time.time()
def findpos(x, y, ang, knownnodes=None):
minq = 1000
mindidjmax = 1000
found = None
if x == None or y == None:
return None
if knownnodes != None:
distances1 = [knownnodes]
else:
distances1 = distances
for (i, j) in distances1:
d = distances[(i, j)]
(xi, yi) = nodes[i]
(xj, yj) = nodes[j]
di = dist(xi, yi, x, y)
dj = dist(xj, yj, x, y)
p = (di + dj) / d
didjmax = max(di, dj)
a = atan2(xj - xi, yj - yi) * 180 / pi
da = a - ang
da = da % 360
if da > 180:
da -= 360
da1 = abs(da)
if da1 > 180 - 30:
da1 = 180 - da1
q = didjmax / 0.5 + da1 / 30 + (di + dj)
#print((i, j, q, di, dj, d, (a,ang%360), (xi, yi), (x, y), (xj, yj)))
if ((found == None or minq > q) and
# di < 1.2*d and dj < 1.2*d and
((dj * dj < di * di + d * d and di * di < dj * dj + d * d) or
(dj < 0.5 or di < 0.5)) and (abs(da) < 45 or abs(da) > 180 - 45)):
minq = q
found = (i, j, (i, j, di, dj, d, di + dj, di / (di + dj)))
if not found:
return None
(i, j, p2) = found
(xi, yi) = nodes[i]
(xj, yj) = nodes[j]
a = atan2(xj - xi, yj - yi) * 180 / pi
da = a - ang
da = da % 360
if da > 180:
da -= 360
if abs(da) < 45:
return (i, j, p2)
elif abs(da) > 180 - 45:
return (j, i, p2)
else:
return (i, j, "unknown", da)
def roaddist(x0, y0):
dmin = None
for (x, y) in roadpoints:
d = dist(x, y, x0, y0)
if dmin == None or dmin > d:
dmin = d
return dmin
def roaddist(x0, y0):
dmin = None
for (x, y) in eight.roadpoints:
d = dist(x, y, x0, y0)
if dmin == None or dmin > d:
dmin = d
return dmin
def getdist(x2, y2):
# NEW
x1 = g.ppx
y1 = g.ppy
d = dist(x1, y1, x2, y2)
tolog("we are at (%f, %f), distance to (%f, %f) is %f" % (
x1, y1, x2, y2, d))
return d
def getdist(x2, y2):
# NEW
x1 = g.ppx
y1 = g.ppy
d = dist(x1, y1, x2, y2)
tolog("we are at (%f, %f), distance to (%f, %f) is %f" %
(x1, y1, x2, y2, d))
return d
def tologaux(str0, level):
if g.targetx and g.targety:
d = dist(g.ppx, g.ppy, g.targetx, g.targety)
else:
d = -1
str = "(speed %d time %.3f %f %f %f %f %f %3f) %s" % (
g.finspeed, time.time() - g.t0, g.ppx, g.ppy, g.ang, d, g.battery,
g.can_ultra, str0)
g.logf.write(str + "\n")
if level >= 2:
print(str)
def tolog2(str0, stdout):
stdout = False
if g.targetx and g.targety:
d = dist(g.ppx, g.ppy, g.targetx, g.targety)
else:
d = -1
str = "(speed %d time %.3f %f %f %f %f %f %3f) %s" % (
g.inspeed, time.time() - g.t0, g.ppx, g.ppy, g.ang, d, g.battery,
g.can_ultra, str0)
g.logf.write(str + "\n")
if stdout:
print(str)
def eightinit():
global distances, neighbours, pieces
eightpath(19.2, 15.4, 12.5)
neighbours = dict()
piecelist = [
[6] + piece1 + [35],
[5] + rev(piece4) + [34],
piece2a + [23],
[23] + piece2b,
piece3a + [23],
[23] + piece3b,
[35, 34], # piece5
[5, 6], # piece6
[3, 4]
] # piece7
distances = dict()
pieces = dict()
for piece in piecelist:
piece2pathpoints(piece, 0.35)
dtot = 0
lastn = None
for n in piece:
if lastn != None:
if not n in neighbours:
neighbours[n] = []
neighbours[n] = neighbours[n] + [lastn]
if not lastn in neighbours:
neighbours[lastn] = []
neighbours[lastn] = neighbours[lastn] + [n]
d = dist(nodes[n][0], nodes[n][1], nodes[lastn][0],
nodes[lastn][1])
distances[(n, lastn)] = d
distances[(lastn, n)] = d
dtot += d
lastn = n
pieces[(piece[0], piece[-1])] = (piece[1:-1], dtot)
pieces[(piece[-1], piece[0])] = (rev(piece[1:-1]), dtot)
def eightinit():
global distances, neighbours, pieces
eightpath(19.2,15.4,12.5)
neighbours = dict()
piecelist = [[6] + piece1 + [35],
[5] + rev(piece4) + [34],
piece2a + [23],
[23] + piece2b,
piece3a + [23],
[23] + piece3b,
[35, 34], # piece5
[5, 6], # piece6
[3, 4]] # piece7
distances = dict()
pieces = dict()
for piece in piecelist:
piece2pathpoints(piece, 0.35)
dtot = 0
lastn = None
for n in piece:
if lastn != None:
if not n in neighbours:
neighbours[n] = []
neighbours[n] = neighbours[n] + [lastn]
if not lastn in neighbours:
neighbours[lastn] = []
neighbours[lastn] = neighbours[lastn] + [n]
d = dist(nodes[n][0], nodes[n][1],
nodes[lastn][0], nodes[lastn][1])
distances[(n, lastn)] = d
distances[(lastn, n)] = d
dtot += d
lastn = n
pieces[(piece[0],piece[-1])] = (piece[1:-1], dtot)
pieces[(piece[-1],piece[0])] = (rev(piece[1:-1]), dtot)
def tologaux(str0, level):
if g.targetx and g.targety:
d = dist(g.ppx, g.ppy, g.targetx, g.targety)
else:
d = -1
str = "(speed %d time %.3f %f %f %f %f %f %3f) %s" % (
g.finspeed,
time.time() - g.t0,
g.ppx,
g.ppy,
g.ang,
d,
g.battery,
g.can_ultra,
str0)
g.logf.write(str + "\n")
if level >= 2:
print(str)
def readmarker0():
TOOHIGHSPEED = 2.0
recentmarkers = []
markertime = None
while True:
p = subprocess.Popen("tail -1 /tmp/marker0", stdout=subprocess.PIPE, shell=True);
res = p.communicate()
m = res[0].decode('ascii')
m = m.split('\n')[0]
if m == g.lastmarker0:
tolog0("no new marker0")
continue
g.lastmarker0 = m
if g.ignoremarkers:
continue
m1 = m.split(" ")
if len(m1) != 7:
print("bad marker line")
continue
if m1 == "":
pass
else:
t = time.time()
doadjust = False
#nav_signal.blinkleds()
g.markerno = int(m1[0])
x = float(m1[1])
y = float(m1[2])
quality = float(m1[4])
ori = float(m1[3])
odiff = ori - (g.ang%360)
if odiff > 180:
odiff -= 360
if odiff < -180:
odiff += 360
accepted = False
# if g.angleknown and abs(odiff) > 45.0 and g.markerno != -1:
# tolog("wrong marker %d %f" % (g.markerno, odiff))
# g.markerno = -1
# if markertime == None or t-markertime > 5:
if markertime == None:
#markertime = t
skipmarker = False
else:
skipmarker = True
if ((g.markerno > -1 and quality > g.minquality
and g.markerno not in g.badmarkers
and (g.goodmarkers == None or g.markerno in g.goodmarkers)
and ((x > -0.3 and x < 3.3 and y > 0 and y < 19.7)
or (x > 3.0 and x < 30 and y > 2.3 and y < 5.5)))
and not skipmarker):
close = True
if not g.angleknown:
g.ang = ori
g.ppx = x
g.ppy = y
g.oldpos = dict()
g.angleknown = True
if g.markerno in mp:
mdist = dist(x, y, mp[g.markerno][0], mp[g.markerno][1])
if mdist > g.maxmarkerdist:
continue
it0 = float(m1[5])
it1 = float(m1[6])
now = time.time()
delay1 = it1 - it0
delay2 = now - it1
#tolog0("delay %f delay2 %f" % (delay1, delay2))
# Since the Optipos client runs on the same machine,
# we can simply add the delays
delay = delay1 + delay2
it0_10 = int(it0*10)/10.0
if g.adjust_t and it0 < g.adjust_t and False:
tolog0("POS: picture too old, we already adjusted %f %f" % (
it0, g.adjust_t))
send_to_ground_control("mpos %f %f %f %f 0 %f" % (x,y,g.ang,time.time()-g.t0, g.inspeed))
continue
elif g.oldpos != None and it0_10 in g.oldpos:
(thenx, theny, thenang) = g.oldpos[it0_10]
doadjust = True
tolog0("POS: position then: %f %f" % (thenx, theny))
elif g.oldpos != None:
tolog0("POS: can't use oldpos")
continue
if True:
if g.lastpos != None:
(xl, yl) = g.lastpos
dst = dist(thenx, theny, xl, yl)
tolog0("local speed %f" % (dst/(t-g.lastpost)))
if dst/(t-g.lastpost) > TOOHIGHSPEED:
close = False
dst = dist(g.ppx, g.ppy, x, y)
# even if somewhat correct: it causes us to lose
# position when we reverse
if dst > 2.0 and g.markercnt > 10:
close = False
tolog0("marker dist %f" % dst)
if not close:
msg = "bad marker %d not close" % g.markerno
if msg != g.markermsg:
tolog(msg)
g.markermsg = msg
else:
accepted = True
g.markercnt += 1
tolog0("marker1 %s %f %f" % (m, g.ang, ori))
if doadjust:
doadjust_n = 1
else:
doadjust_n = 0
send_to_ground_control("mpos %f %f %f %f %d %f" % (x,y,g.ang,time.time()-g.t0, doadjust_n, g.inspeed))
#nav_mqtt.send_to_mqtt(x, y)
g.lastpos = (thenx,theny)
g.px = x
g.py = y
if True:
# if g.markercnt % 10 == 1:
# if g.markercnt == 1:
if doadjust:
g.adjust_t = time.time()
tolog0("adjusting pos %f %f -> %f %f" % (g.ppx, g.ppy,
x, y))
if g.markercnt != 1:
g.angdiff = (ori-g.ang)%360
if True:
if doadjust:
tolog0("old pp diff %f %f" % (
g.ppxdiff, g.ppydiff))
ppxdiff1 = x-g.ppx
ppydiff1 = y-g.ppy
ppxdiff1 = x-thenx
ppydiff1 = y-theny
angdiff1 = (ori-thenang)%360
ppxdiff1 /= 2
ppydiff1 /= 2
#angdiff1 /= 2
if True:
g.ppxdiff = ppxdiff1
g.ppydiff = ppydiff1
#print("3 ppydiff := %f" % g.ppydiff)
g.angdiff = angdiff1
adjdist = sqrt(
ppxdiff1*ppxdiff1 +
ppydiff1*ppydiff1)
if g.maxadjdist < adjdist:
g.maxadjdist = adjdist
# print("new maxadjdist %f"
# % g.maxadjdist)
if g.adjdistlimit != None and adjdist > g.adjdistlimit:
g.poserror = True
else:
g.ppx = x
g.ppy = y
#print("1 ppy := %f" % g.ppy)
g.angdiff = g.angdiff % 360
if g.angdiff > 180:
g.angdiff -= 360
else:
g.ppx = x
g.ppy = y
#print("2 ppy := %f" % g.ppy)
#g.vx = sin(g.ang*pi/180)*g.inspeed/100
#g.vy = cos(g.ang*pi/180)*g.inspeed/100
g.lastpost = it0
#g.ang = ori
else:
if g.adjust_t != None:
markerage = time.time() - g.adjust_t
# if markerage > 1.0:
# print("marker age %f" % markerage)
if False:
print("marker good=%s %d = (%f,%f) (%f, %f) %f %f" % (
str(accepted), g.markerno, x, y,
g.ppx, g.ppy, g.finspeed, g.inspeed))
if accepted:
recentmarkers = [str(g.markerno)] + recentmarkers
else:
recentmarkers = ['x'] + recentmarkers
if len(recentmarkers) > 10:
recentmarkers = recentmarkers[0:10]
send_to_ground_control("markers %s" % " ".join(recentmarkers))
if not accepted:
send_to_ground_control("badmarker %f %f" % (x,y))
tolog0("marker5 %s %f %f" % (m, g.ang, ori))
time.sleep(0.00001)
def readmarker0():
TOOHIGHSPEED = 2.0
recentmarkers = []
markertime = None
goodmarkertime = None
markerage = None
while True:
p = subprocess.Popen("tail -1 /tmp/marker0", stdout=subprocess.PIPE, shell=True);
res = p.communicate()
m = res[0].decode('ascii')
m = m.split('\n')[0]
if m == g.lastmarker0:
tolog0("no new marker0")
continue
g.lastmarker0 = m
if g.ignoremarkers:
continue
m1 = m.split(" ")
if len(m1) != 7:
print("bad marker line")
continue
if m1 == "":
pass
else:
t = time.time()
doadjust = False
#nav_signal.blinkleds()
g.markerno = int(m1[0])
x = float(m1[1])
y = float(m1[2])
quality = float(m1[4])
ori = float(m1[3])
odiff = ori - (g.ang%360)
if odiff > 180:
odiff -= 360
if odiff < -180:
odiff += 360
accepted = False
# if g.angleknown and abs(odiff) > 45.0 and g.markerno != -1:
# tolog("wrong marker %d %f" % (g.markerno, odiff))
# g.markerno = -1
# if markertime == None or t-markertime > 5:
if markertime == None:
#markertime = t
skipmarker = False
else:
skipmarker = True
x += g.shiftx
minq = g.minquality
# g.ang here below should be thenang, I think
badmarker = False
for (badm, bada) in g.badmarkers:
if g.markerno == badm:
if bada == 'all':
bada = g.ang
angm = (g.ang - bada)%360
if angm > 180:
angm -= 360
if abs(angm) < 25:
badmarker = True
break
if g.goodmarkers != None:
goodmarker = False
for (goodm, gooda, goodq) in g.goodmarkers:
if g.markerno == goodm:
if gooda == 'all':
gooda = g.ang
angm = (g.ang - gooda)%360
minq = goodq
if angm > 180:
angm -= 360
if abs(angm) < 25:
goodmarker = True
break
else:
goodmarker = True
if (g.markerno > -1
and goodmarker
and ((x > -0.3 and x < 3.3 and y > 0 and y < 19.7)
or (x > 3.0 and x < 30 and y > 2.3 and y < 5.5))):
tolog0("marker0 %s %f" % (m, quality))
# complain somewhere if good and bad overlap
if ((pbool(g.markerno > -1) and pbool(quality > minq)
#and abs(g.steering) < 30
#and (x < 1.0 or x > 2.0)
and pbool(goodmarkertime == None or
t-goodmarkertime > g.markertimesep)
and pbool(not badmarker)
and pbool(goodmarker)
and ((x > -0.3 and x < 3.3 and y > 0 and y < 19.7)
or (x > 3.0 and x < 30 and y > 2.3 and y < 5.5)))
and not skipmarker):
close = True
if not g.angleknown:
g.ang = ori
g.ppx = x
g.ppy = y
g.oldpos = dict()
g.angleknown = True
mdist = -1
if g.markerno in mp:
mdist = dist(x, y, mp[g.markerno][0], mp[g.markerno][1])
if mdist > g.maxmarkerdist:
#print("dist = %f" % mdist)
continue
it0 = float(m1[5])
it1 = float(m1[6])
now = time.time()
delay1 = it1 - it0
delay2 = now - it1
#tolog0("delay %f delay2 %f" % (delay1, delay2))
# Since the Optipos client runs on the same machine,
# we can simply add the delays
delay = delay1 + delay2
it0_10 = int(it0*10)/10.0
if g.adjust_t and it0 < g.adjust_t and False:
tolog0("POS: picture too old, we already adjusted %f %f" % (
it0, g.adjust_t))
send_to_ground_control("mpos %f %f %f %f 0 %f" % (x,y,g.ang,time.time()-g.t0, g.inspeed))
continue
elif g.oldpos != None and it0_10 in g.oldpos:
(thenx, theny, thenang) = g.oldpos[it0_10]
doadjust = True
tolog0("POS: position then: %f %f %f" % (thenx, theny,
thenang))
elif g.oldpos != None:
tolog0("POS: can't use oldpos")
continue
if True:
if g.lastpos != None:
(xl, yl) = g.lastpos
dst = dist(thenx, theny, xl, yl)
tolog0("local speed %f" % (dst/(t-g.lastpost)))
if dst/(t-g.lastpost) > TOOHIGHSPEED:
close = False
dst = dist(g.ppx, g.ppy, x, y)
# even if somewhat correct: it causes us to lose
# position when we reverse
if dst > 2.0 and g.markercnt > 10:
close = False
tolog0("marker dist %f" % dst)
if not close:
msg = "bad marker %d not close" % g.markerno
if msg != g.markermsg:
tolog(msg)
g.markermsg = msg
else:
accepted = True
goodmarkertime = t
g.markercnt += 1
tolog0("marker1 %s %f %f %f %f %f %f" % (m, g.ang, ori, thenx, theny, quality, mdist))
if doadjust:
doadjust_n = 1
else:
doadjust_n = 0
send_to_ground_control("mpos %f %f %f %f %d %f" % (x,y,g.ang,time.time()-g.t0, doadjust_n, g.inspeed))
nav_mqtt.send_to_mqtt(x, y, ori)
g.lastpos = (thenx,theny)
g.px = x
g.py = y
if True:
# if g.markercnt % 10 == 1:
# if g.markercnt == 1:
if doadjust:
g.adjust_t = time.time()
tolog0("adjusting pos %f %f -> %f %f" % (g.ppx, g.ppy,
x, y))
if g.markercnt != 1:
g.angdiff = (ori-g.ang)%360
if True:
if doadjust:
tolog0("old pp diff %f %f" % (
g.ppxdiff, g.ppydiff))
ppxdiff1 = x-g.ppx
ppydiff1 = y-g.ppy
ppxdiff1 = x-thenx
ppydiff1 = y-theny
angdiff1 = (ori-thenang)%360
# Unclear to me whether we should halve or not: after a long time, we should
# treat the marker as the truth, but when markers come soon after one
# another, one is not more likely than the other to be right, so we go to the
# middle point.
#ppxdiff1 /= 2
#ppydiff1 /= 2
#angdiff1 /= 2
if True:
g.ppxdiff = ppxdiff1
g.ppydiff = ppydiff1
#print("3 ppydiff := %f" % g.ppydiff)
g.angdiff = angdiff1
adjdist = sqrt(
ppxdiff1*ppxdiff1 +
ppydiff1*ppydiff1)
if g.maxadjdist < adjdist:
g.maxadjdist = adjdist
# print("new maxadjdist %f"
# % g.maxadjdist)
if g.adjdistlimit != None and adjdist > g.adjdistlimit:
g.poserror = True
else:
g.ppx = x
g.ppy = y
#print("1 ppy := %f" % g.ppy)
g.angdiff = g.angdiff % 360
if g.angdiff > 180:
g.angdiff -= 360
else:
g.ppx = x
g.ppy = y
#print("2 ppy := %f" % g.ppy)
#g.vx = sin(g.ang*pi/180)*g.inspeed/100
#g.vy = cos(g.ang*pi/180)*g.inspeed/100
g.lastpost = it0
#g.ang = ori
else:
if g.adjust_t != None:
markerage = time.time() - g.adjust_t
if markerage > 10:
tolog("marker age %f" % markerage)
if False:
print("marker good=%s %d = (%f,%f) (%f, %f) %f %f" % (
str(accepted), g.markerno, x, y,
g.ppx, g.ppy, g.finspeed, g.inspeed))
if accepted:
recentmarkers = [str(g.markerno)] + recentmarkers
else:
recentmarkers = ['x'] + recentmarkers
if len(recentmarkers) > 10:
recentmarkers = recentmarkers[0:10]
send_to_ground_control("markers %s" % " ".join(recentmarkers))
if not accepted:
send_to_ground_control("badmarker %f %f" % (x,y))
tolog0("marker5 %s %f %f" % (m, g.ang, ori))
time.sleep(0.00001)
def readmarker0():
TOOHIGHSPEED = 2.0
recentmarkers = []
markertime = None
goodmarkertime = None
markerage = None
while True:
p = subprocess.Popen("tail -1 /tmp/marker0",
stdout=subprocess.PIPE,
shell=True)
res = p.communicate()
m = res[0].decode('ascii')
m = m.split('\n')[0]
if m == g.lastmarker0:
tolog0("no new marker0")
continue
g.lastmarker0 = m
if g.ignoremarkers:
continue
m1 = m.split(" ")
if len(m1) != 7:
print("bad marker line")
continue
if m1 == "":
pass
else:
t = time.time()
doadjust = False
#nav_signal.blinkleds()
g.markerno = int(m1[0])
x = float(m1[1])
y = float(m1[2])
quality = float(m1[4])
ori = float(m1[3])
odiff = ori - (g.ang % 360)
if odiff > 180:
odiff -= 360
if odiff < -180:
odiff += 360
accepted = False
# if g.angleknown and abs(odiff) > 45.0 and g.markerno != -1:
# tolog("wrong marker %d %f" % (g.markerno, odiff))
# g.markerno = -1
# if markertime == None or t-markertime > 5:
if markertime == None:
#markertime = t
skipmarker = False
else:
skipmarker = True
x += g.shiftx
minq = g.minquality
# g.ang here below should be thenang, I think
badmarker = False
for (badm, bada) in g.badmarkers:
if g.markerno == badm:
if bada == 'all':
bada = g.ang
angm = (g.ang - bada) % 360
if angm > 180:
angm -= 360
if abs(angm) < 25:
badmarker = True
break
if g.goodmarkers != None:
goodmarker = False
for (goodm, gooda, goodq) in g.goodmarkers:
if g.markerno == goodm:
if gooda == 'all':
gooda = g.ang
angm = (g.ang - gooda) % 360
minq = goodq
if angm > 180:
angm -= 360
if abs(angm) < 25:
goodmarker = True
break
else:
goodmarker = True
if (g.markerno > -1 and goodmarker
and ((x > -0.3 and x < 3.3 and y > 0 and y < 19.7) or
(x > 3.0 and x < 30 and y > 2.3 and y < 5.5))):
tolog0("marker0 %s %f" % (m, quality))
# complain somewhere if good and bad overlap
if ((
pbool(g.markerno > -1) and pbool(quality > minq)
#and abs(g.steering) < 30
#and (x < 1.0 or x > 2.0)
and pbool(goodmarkertime == None
or t - goodmarkertime > g.markertimesep) and
pbool(not badmarker) and pbool(goodmarker) and
((x > -0.3 and x < 3.3 and y > 0 and y < 19.7) or
(x > 3.0 and x < 30 and y > 2.3 and y < 5.5)))
and not skipmarker):
close = True
if not g.angleknown:
g.ang = ori
g.ppx = x
g.ppy = y
g.oldpos = dict()
g.angleknown = True
mdist = -1
if g.markerno in mp:
mdist = dist(x, y, mp[g.markerno][0], mp[g.markerno][1])
if mdist > g.maxmarkerdist:
#print("dist = %f" % mdist)
continue
it0 = float(m1[5])
it1 = float(m1[6])
now = time.time()
delay1 = it1 - it0
delay2 = now - it1
#tolog0("delay %f delay2 %f" % (delay1, delay2))
# Since the Optipos client runs on the same machine,
# we can simply add the delays
delay = delay1 + delay2
it0_10 = int(it0 * 10) / 10.0
if g.adjust_t and it0 < g.adjust_t and False:
tolog0("POS: picture too old, we already adjusted %f %f" %
(it0, g.adjust_t))
send_to_ground_control(
"mpos %f %f %f %f 0 %f" %
(x, y, g.ang, time.time() - g.t0, g.inspeed))
continue
elif g.oldpos != None and it0_10 in g.oldpos:
(thenx, theny, thenang) = g.oldpos[it0_10]
doadjust = True
tolog0("POS: position then: %f %f %f" %
(thenx, theny, thenang))
elif g.oldpos != None:
tolog0("POS: can't use oldpos")
continue
if True:
if g.lastpos != None:
(xl, yl) = g.lastpos
dst = dist(thenx, theny, xl, yl)
tolog0("local speed %f" % (dst / (t - g.lastpost)))
if dst / (t - g.lastpost) > TOOHIGHSPEED:
close = False
dst = dist(g.ppx, g.ppy, x, y)
# even if somewhat correct: it causes us to lose
# position when we reverse
if dst > 2.0 and g.markercnt > 10:
close = False
tolog0("marker dist %f" % dst)
if not close:
msg = "bad marker %d not close" % g.markerno
if msg != g.markermsg:
tolog(msg)
g.markermsg = msg
else:
accepted = True
goodmarkertime = t
g.markercnt += 1
tolog0("marker1 %s %f %f %f %f %f %f" %
(m, g.ang, ori, thenx, theny, quality, mdist))
if doadjust:
doadjust_n = 1
else:
doadjust_n = 0
send_to_ground_control("mpos %f %f %f %f %d %f" %
(x, y, g.ang, time.time() - g.t0,
doadjust_n, g.inspeed))
nav_mqtt.send_to_mqtt(x, y, ori)
g.lastpos = (thenx, theny)
g.px = x
g.py = y
if True:
# if g.markercnt % 10 == 1:
# if g.markercnt == 1:
if doadjust:
g.adjust_t = time.time()
tolog0("adjusting pos %f %f -> %f %f" %
(g.ppx, g.ppy, x, y))
if g.markercnt != 1:
g.angdiff = (ori - g.ang) % 360
if True:
if doadjust:
tolog0("old pp diff %f %f" %
(g.ppxdiff, g.ppydiff))
ppxdiff1 = x - g.ppx
ppydiff1 = y - g.ppy
ppxdiff1 = x - thenx
ppydiff1 = y - theny
angdiff1 = (ori - thenang) % 360
# Unclear to me whether we should halve or not: after a long time, we should
# treat the marker as the truth, but when markers come soon after one
# another, one is not more likely than the other to be right, so we go to the
# middle point.
#ppxdiff1 /= 2
#ppydiff1 /= 2
#angdiff1 /= 2
if True:
g.ppxdiff = ppxdiff1
g.ppydiff = ppydiff1
#print("3 ppydiff := %f" % g.ppydiff)
g.angdiff = angdiff1
adjdist = sqrt(ppxdiff1 * ppxdiff1 +
ppydiff1 * ppydiff1)
if g.maxadjdist < adjdist:
g.maxadjdist = adjdist
# print("new maxadjdist %f"
# % g.maxadjdist)
if g.adjdistlimit != None and adjdist > g.adjdistlimit:
g.poserror = True
else:
g.ppx = x
g.ppy = y
#print("1 ppy := %f" % g.ppy)
g.angdiff = g.angdiff % 360
if g.angdiff > 180:
g.angdiff -= 360
else:
g.ppx = x
g.ppy = y
#print("2 ppy := %f" % g.ppy)
#g.vx = sin(g.ang*pi/180)*g.inspeed/100
#g.vy = cos(g.ang*pi/180)*g.inspeed/100
g.lastpost = it0
#g.ang = ori
else:
if g.adjust_t != None:
markerage = time.time() - g.adjust_t
if markerage > 10:
tolog("marker age %f" % markerage)
if False:
print("marker good=%s %d = (%f,%f) (%f, %f) %f %f" %
(str(accepted), g.markerno, x, y, g.ppx, g.ppy,
g.finspeed, g.inspeed))
if accepted:
recentmarkers = [str(g.markerno)] + recentmarkers
else:
recentmarkers = ['x'] + recentmarkers
if len(recentmarkers) > 10:
recentmarkers = recentmarkers[0:10]
send_to_ground_control("markers %s" % " ".join(recentmarkers))
if not accepted:
send_to_ground_control("badmarker %f %f" % (x, y))
tolog0("marker5 %s %f %f" % (m, g.ang, ori))
time.sleep(0.00001)
def findpos(x, y, ang, knownnodes = None):
minq = 1000
mindidjmax = 1000
found = None
if x == None or y == None:
return None
if knownnodes != None and knownnodes != []:
distances1 = knownnodes
#print("knownnodes = %s" % str(knownnodes))
else:
distances1 = eight.distances
for (i, j) in distances1:
d = eight.distances[(i, j)]
(xi, yi) = eight.nodes[i]
(xj, yj) = eight.nodes[j]
di = dist(xi, yi, x, y)
dj = dist(xj, yj, x, y)
p = (di+dj)/d
didjmax = max(di,dj)
a = atan2(xj-xi, yj-yi)*180/pi
da = a-ang
da = da%360
if da > 180:
da -= 360
da1 = abs(da)
if da1 > 180-30:
da1 = 180-da1
q = didjmax/0.5 + da1/30 + (di+dj)
#print((i, j, q, di, dj, d, (a,ang%360), (xi, yi), (x, y), (xj, yj)))
if ((found == None or minq > q) and
# di < 1.2*d and dj < 1.2*d and
(
(dj*dj < di*di+d*d and di*di < dj*dj+d*d) or
(dj < 0.5 or di < 0.5)
) and
(abs(da) < 45 or abs(da) > 180-45)):
minq = q
found = (i, j, (i, j, di, dj, d, di+dj, di/(di+dj)))
if not found:
return None
(i, j, p2) = found
(xi, yi) = eight.nodes[i]
(xj, yj) = eight.nodes[j]
a = atan2(xj-xi, yj-yi)*180/pi
da = a-ang
da = da%360
if da > 180:
da -= 360
if abs(da) < 45:
return (i, j, p2)
elif abs(da) > 180-45:
return (j, i, p2)
else:
return (i, j, "unknown", da)