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
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 from_ground_control():
lastreportclosest = False
prevl = None
while True:
if g.ground_control:
for data in linesplit(g.ground_control):
#print(data)
l = data.split(" ")
#print(l)
#print(data)
g.tctime = time.time()
if l[0] == "go":
x = float(l[1])
y = float(l[2])
print(("goto is not implemented", x, y))
elif l[0] == "path":
path = ast.literal_eval(data[5:])
print("Received path from traffic control:")
print(path)
# currently, we don't do anything with this path
elif l[0] == "continue":
g.paused = False
elif l[0] == "carpos":
carname = l[1]
x = float(l[2])
y = float(l[3])
g.posnow[carname] = (x, y)
if carname not in g.otherpos:
g.otherpos[carname] = queue.Queue(100000)
g.otherpos[carname].put((x, y))
elif l[0] == "carsinfront":
if l != prevl:
pass
#print("traffic %s" % str(l[1:]))
prevl = l
n = int(l[1])
closest = None
for i in range(0, n):
relation = l[6 * i + 2]
dist = float(l[6 * i + 3])
#x = float(l[6*i+4])
#y = float(l[6*i+5])
othercar = l[6 * i + 6]
onlyif = float(l[6 * i + 7])
if closest == None or closest > dist:
closest = dist
if closest != None:
if (onlyif != 0 and g.nextdecisionpoint != 0
and onlyif != g.nextdecisionpoint):
tolog("onlyif %d %d" %
(onlyif, g.nextdecisionpoint))
continue
# a car length
closest = closest - 0.5
closest0 = closest
# some more safety:
closest = closest - 0.5
if closest < 0:
closest = 0
# 4 is our safety margin and should make for
# a smoother ride
if g.limitspeed == None:
pass
#print("car in front")
tolog("car in front")
#g.limitspeed = 100*closest/0.85/4
g.limitspeed = 100 * closest / 0.85
if g.limitspeed < 11:
#print("setting limitspeed to 0")
g.limitspeed = 0
if g.outspeedcm != None and g.outspeedcm != 0:
nav_signal.warningblink(True)
else:
#print("reduced limitspeed")
pass
if g.limitspeed < g.outspeedcm:
print1(
g, "%s %s %.2f m speed %.1f -> %.1f (%.1f)" %
(relation, othercar, closest0, g.finspeed,
g.limitspeed, g.outspeedcm))
if "crash" in relation and g.simulate:
g.crash = True
g.simulmaxacc = 0.0
ff = tolog
if g.finspeed != 0 and g.finspeed >= g.limitspeed:
if relation == "givewayto":
ff = tolog2
ff = tolog
ff("closest car in front2: %s dist %f limitspeed %f" %
(relation, closest, g.limitspeed))
lastreportclosest = True
else:
if not g.crash:
g.limitspeed = None
if lastreportclosest:
tolog0("no close cars")
pass
lastreportclosest = False
if g.outspeedcm:
# neither 0 nor None
if g.limitspeed == 0:
send_to_ground_control(
"message stopping for obstacle")
elif g.limitspeed != None and g.limitspeed < g.outspeedcm:
send_to_ground_control(
"message slowing for obstacle %.1f" %
g.limitspeed)
else:
send_to_ground_control("message ")
else:
send_to_ground_control("message ")
elif l[0] == "parameter":
g.parameter = int(l[1])
print("parameter %d" % g.parameter)
# control be used so we don't have to stop if the next
# section is free
elif l[0] == "waitallcarsdone":
g.queue.put(1)
elif l[0] == "cargoto":
x = float(l[2])
y = float(l[3])
nav2.goto(x, y, l[4])
elif l[0] == "sync":
flag = int(l[1])
if flag == 1:
tctime = float(l[2])
print("syncing to %f" % (tctime))
tolog("sync1")
g.t0 = time.time() - tctime
tolog("sync2")
elif l[0] == "heartecho":
t1 = float(l[1])
t2 = float(l[2])
g.heartn_r = int(l[3])
#print("heartecho %.3f %.3f %.3f %d" % (time.time() - g.t0, t1, t2, g.heartn_r))
else:
print("unknown control command %s" % data)
time.sleep(1)
def readgyro0():
#gscale = 32768.0/250
gscale = 32768.0 / 1000
ascale = 1670.0
x = 0.0
y = 0.0
x0 = 0.0
y0 = 0.0
z0 = 0.0
rx = 0.0
ry = 0.0
count = 0
countt = time.time()
try:
tlast = time.time()
t1 = time.time()
while True:
count += 1
if count % 1000 == 0:
newt = time.time()
#print("readgyro0 1000 times = %f s" % (newt-countt))
countt = newt
w = g.bus.read_i2c_block_data(imuaddress, 0x47, 2)
high = w[0]
low = w[1]
r = make_word(high, low)
r -= g.rbias
if True:
high = g.bus.read_byte_data(imuaddress, 0x45)
low = g.bus.read_byte_data(imuaddress, 0x46)
ry = make_word(high, low)
ry -= g.rybias
w = g.bus.read_i2c_block_data(imuaddress, 0x43, 2)
high = w[0]
low = w[1]
rx = make_word(high, low)
rx -= g.rxbias
if False:
if rx > 120 and g.finspeed != 0 and g.dstatus != 2:
inhibitdodrive()
g.dstatus = 2
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
246, 0)
os.system(cmd)
# dodrive(0, 0)
print("stopped")
drive(0)
# make the steering and the angle go in the same direction
# now positive is clockwise
r = -r
t2 = time.time()
dt = t2 - t1
if dt > 0.5:
# pretend we crashed
g.crash = 10.0
nav_log.tolog("readgyro0 paused for %f s" % dt)
t1 = t2
angvel = r / gscale
g.dang = angvel * dt
anghalf = g.ang + g.dang / 2
g.ang += g.dang
if True:
w = g.bus.read_i2c_block_data(imuaddress, 0x3b, 6)
x = make_word(w[0], w[1])
x -= g.xbias
y = make_word(w[2], w[3])
y -= g.ybias
z = make_word(w[4], w[5])
z -= g.zbias
x /= ascale
y /= ascale
z /= ascale
g.acc = sqrt(x * x + y * y + z * z)
if g.acc > g.crashlimit and g.detectcrashes:
nav_log.tolog0("crash")
g.crash = g.acc
x0 = -x
y0 = -y
z0 = z
# the signs here assume that x goes to the right and y forward
x = x0 * cos(pi / 180 * anghalf) - y0 * sin(pi / 180 * anghalf)
y = x0 * sin(pi / 180 * anghalf) + y0 * cos(pi / 180 * anghalf)
g.vx += x * dt
g.vy += y * dt
g.vz += z * dt
g.px += g.vx * dt
g.py += g.vy * dt
g.pz += g.vz * dt
g.realspeed = (g.finspeed + g.fleftspeed) / 2
# ad hoc constant, which made pleftspeed close to fleftspeed
corrleft = 1 + angvel / 320.0
# p for pretend
pleftspeed = g.finspeed * corrleft
if True:
usedspeed = pleftspeed
else:
usedspeed = g.realspeed
vvx = usedspeed / 100.0 * sin(pi / 180 * g.ang)
vvy = usedspeed / 100.0 * cos(pi / 180 * g.ang)
ppxi = vvx * dt
ppyi = vvy * dt
if True:
ds = sqrt(ppxi * ppxi + ppyi * ppyi)
g.totals += ds
g.ppx += ppxi
g.ppy += ppyi
if g.oldpos != None:
t2_10 = int(t2 * 10) / 10.0
g.oldpos[t2_10] = (g.ppx, g.ppy, g.ang)
# don't put too many things in this thread
if False:
w = g.bus.read_i2c_block_data(imuaddress, 0x4c, 6)
mx0 = make_word(w[1], w[0])
my0 = make_word(w[3], w[2])
mz0 = make_word(w[5], w[4])
mx = float((mx0 - g.mxmin)) / (g.mxmax - g.mxmin) * 2 - 1
my = float((my0 - g.mymin)) / (g.mymax - g.mymin) * 2 - 1
mz = mz0
quot = (mx + my) / sqrt(2)
if quot > 1.0:
quot = 1.0
if quot < -1.0:
quot = -1.0
mang = (asin(quot)) * 180 / pi + 45
if mx < my:
mang = 270 - mang
mang = mang % 360
mang = -mang
mang = mang % 360
if True:
dtup = (x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx,
g.ppy, g.ang % 360, angvel, g.can_steer, g.can_speed,
g.inspeed, g.outspeed, g.odometer, z0, r, rx, ry,
g.acc, g.finspeed, g.fodometer, t2 - g.t0, pleftspeed,
g.leftspeed, g.fleftspeed, g.realspeed, g.can_ultra,
g.droppedlog)
fstr = "%f" + " %f" * (len(dtup) - 1) + "\n"
if False:
g.accf.write(fstr % dtup)
else:
if g.qlen > 0.9 * g.accfqsize:
g.droppedlog += 1
else:
if True:
g.accfq.put(fstr % dtup, False)
g.droppedlog = 0
g.qlen += 1
else:
try:
g.accfq.put(fstr % dtup, False)
g.droppedlog = 0
g.qlen += 1
except Exception as a:
print("queue exception")
if (t2 - tlast > 0.1):
nav_log.tolog0("")
tlast = t2
j = g.angdiff / g.angdifffactor
g.ang += j
g.angdiff -= j
#print("pp diff %f %f" % (g.ppxdiff, g.ppydiff))
j = g.ppxdiff / g.xydifffactor
if abs(j) > 0.01:
pass
#print("ppx %f->%f j %f xdiff %f" % (g.ppx, g.ppx+g.ppxdiff,
g.ppx += j
g.ppxdiff -= j
j = g.ppydiff / g.xydifffactor
g.ppy += j
g.ppydiff -= j
if False:
if ((abs(g.ang % 360 - 270) < 1.0 and g.ppy > 18.0)
or (abs(g.ang % 360 - 90) < 1.0 and g.ppy < 14.0)):
nav_log.tolog("ang %f ppx %f ultra %f" %
(g.ang % 360, g.ppx, g.can_ultra))
time.sleep(0.00001)
except Exception as e:
nav_log.tolog("exception in readgyro: " + str(e))
print("exception in readgyro: " + str(e))
def readgyro0():
#gscale = 32768.0/250
gscale = 32768.0 / 1000
ascale = 1670.0
x = 0.0
y = 0.0
x0 = 0.0
y0 = 0.0
z0 = 0.0
rx = 0.0
ry = 0.0
acc = 0.0
try:
tlast = time.time()
t1 = time.time()
while True:
w = g.bus.read_i2c_block_data(imuaddress, 0x47, 2)
high = w[0]
low = w[1]
r = make_word(high, low)
r -= g.rbias
if True:
high = g.bus.read_byte_data(imuaddress, 0x45)
low = g.bus.read_byte_data(imuaddress, 0x46)
ry = make_word(high, low)
ry -= g.rybias
w = g.bus.read_i2c_block_data(imuaddress, 0x43, 2)
high = w[0]
low = w[1]
rx = make_word(high, low)
rx -= g.rxbias
if False:
if rx > 120 and g.finspeed != 0 and g.dstatus != 2:
inhibitdodrive()
g.dstatus = 2
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
246, 0)
os.system(cmd)
# dodrive(0, 0)
print("stopped")
drive(0)
# make the steering and the angle go in the same direction
# now positive is clockwise
r = -r
t2 = time.time()
dt = t2 - t1
t1 = t2
angvel = r / gscale
g.dang = angvel * dt
g.ang += g.dang
if True:
w = g.bus.read_i2c_block_data(imuaddress, 0x3b, 6)
x = make_word(w[0], w[1])
x -= g.xbias
y = make_word(w[2], w[3])
y -= g.ybias
z = make_word(w[4], w[5])
z -= g.zbias
x /= ascale
y /= ascale
z /= ascale
acc = sqrt(x * x + y * y + z * z)
if acc > 9.0 and g.detectcrashes:
nav_log.tolog0("crash")
g.crash = acc
x0 = -x
y0 = -y
z0 = z
# the signs here assume that x goes to the right and y forward
x = x0 * cos(pi / 180 * g.ang) - y0 * sin(pi / 180 * g.ang)
y = x0 * sin(pi / 180 * g.ang) + y0 * cos(pi / 180 * g.ang)
g.vx += x * dt
g.vy += y * dt
g.vz += z * dt
g.px += g.vx * dt
g.py += g.vy * dt
g.pz += g.vz * dt
corr = 1.0
vvx = g.inspeed * corr / 100.0 * sin(pi / 180 * g.ang)
vvy = g.inspeed * corr / 100.0 * cos(pi / 180 * g.ang)
ppxi = vvx * dt
ppyi = vvy * dt
if True:
ds = sqrt(ppxi * ppxi + ppyi * ppyi)
g.totals += ds
g.ppx += ppxi
g.ppy += ppyi
if g.oldpos != None:
t2_10 = int(t2 * 10) / 10.0
g.oldpos[t2_10] = (g.ppx, g.ppy, g.ang)
# don't put too many things in this thread
if False:
w = g.bus.read_i2c_block_data(imuaddress, 0x4c, 6)
mx0 = make_word(w[1], w[0])
my0 = make_word(w[3], w[2])
mz0 = make_word(w[5], w[4])
mx = float((mx0 - g.mxmin)) / (g.mxmax - g.mxmin) * 2 - 1
my = float((my0 - g.mymin)) / (g.mymax - g.mymin) * 2 - 1
mz = mz0
quot = (mx + my) / sqrt(2)
if quot > 1.0:
quot = 1.0
if quot < -1.0:
quot = -1.0
mang = (asin(quot)) * 180 / pi + 45
if mx < my:
mang = 270 - mang
mang = mang % 360
mang = -mang
mang = mang % 360
if True:
g.accf.write(
"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n"
% (x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx,
g.ppy, g.ang % 360, angvel, g.can_steer, g.can_speed,
g.inspeed, g.outspeed, g.odometer, z0, r, rx, ry, acc,
g.finspeed, g.fodometer, t2 - g.t0, g.can_ultra))
if (t2 - tlast > 0.1):
nav_log.tolog0("")
tlast = t2
j = g.angdiff / 1000
g.ang += j
g.angdiff -= j
#print("pp diff %f %f" % (g.ppxdiff, g.ppydiff))
j = g.ppxdiff / 100
g.ppx += j
g.ppxdiff -= j
j = g.ppydiff / 100
g.ppy += j
g.ppydiff -= j
time.sleep(0.00001)
except Exception as e:
nav_log.tolog("exception in readgyro: " + str(e))
print("exception in readgyro: " + str(e))
def readgyro0():
#gscale = 32768.0/250
gscale = 32768.0/1000
ascale = 1670.0
x = 0.0
y = 0.0
x0 = 0.0
y0 = 0.0
z0 = 0.0
rx = 0.0
ry = 0.0
count = 0
countt = time.time()
g.pauseimu = 0.0
try:
tlast = time.time()
t1 = time.time()
while True:
count += 1
if count%1000 == 0:
newt = time.time()
#print("readgyro0 1000 times = %f s" % (newt-countt))
countt = newt
# if g.pauseimu == 0.0:
# g.pauseimu = 1.5
if g.pauseimu > 0.0:
time.sleep(g.pauseimu)
g.pauseimu = 0.0
w = g.bus.read_i2c_block_data(imuaddress, 0x47, 2)
high = w[0]
low = w[1]
r = make_word(high, low)
r -= g.rbias
if True:
high = g.bus.read_byte_data(imuaddress, 0x45)
low = g.bus.read_byte_data(imuaddress, 0x46)
ry = make_word(high, low)
ry -= g.rybias
w = g.bus.read_i2c_block_data(imuaddress, 0x43, 2)
high = w[0]
low = w[1]
rx = make_word(high, low)
rx -= g.rxbias
if False:
if rx > 120 and g.finspeed != 0 and g.dstatus != 2:
inhibitdodrive()
g.dstatus = 2
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
246, 0)
os.system(cmd)
# dodrive(0, 0)
print("stopped")
drive(0)
# make the steering and the angle go in the same direction
# now positive is clockwise
r = -r
t2 = time.time()
dt = t2-t1
if dt > g.dtlimit:
print("%f dt %f" % (t2-g.t0, dt))
if dt > 0.5:
#faulthandler.dump_traceback()
nav_log.tolog("readgyro0 paused for %f s" % dt)
if dt > 1.5:
# we ought to stop and wait
pass
t1 = t2
angvel = r/gscale
g.dang = angvel*dt
anghalf = g.ang + g.dang/2
g.ang += g.dang
if True:
w = g.bus.read_i2c_block_data(imuaddress, 0x3b, 6)
x = make_word(w[0], w[1])
x -= g.xbias
y = make_word(w[2], w[3])
y -= g.ybias
z = make_word(w[4], w[5])
z -= g.zbias
x /= ascale
y /= ascale
z /= ascale
g.acc = sqrt(x*x+y*y+z*z)
if g.acc > g.crashlimit and g.detectcrashes:
nav_log.tolog0("crash")
g.crash = g.acc
x0 = -x
y0 = -y
z0 = z
# the signs here assume that x goes to the right and y forward
x = x0*cos(pi/180*anghalf) - y0*sin(pi/180*anghalf)
y = x0*sin(pi/180*anghalf) + y0*cos(pi/180*anghalf)
g.vx += x*dt
g.vy += y*dt
g.vz += z*dt
g.px += g.vx*dt
g.py += g.vy*dt
g.pz += g.vz*dt
g.realspeed = (g.finspeed + g.fleftspeed)/2
# ad hoc constant, which made pleftspeed close to fleftspeed
corrleft = 1+angvel/320.0
# p for pretend
pleftspeed = g.finspeed*corrleft
if True:
usedspeed = pleftspeed
else:
usedspeed = g.realspeed
vvx = usedspeed/100.0*sin(pi/180*g.ang)
vvy = usedspeed/100.0*cos(pi/180*g.ang)
ppxi = vvx*dt
ppyi = vvy*dt
if True:
ds = sqrt(ppxi*ppxi+ppyi*ppyi)
g.totals += ds
g.ppx += ppxi
g.ppy += ppyi
if g.oldpos != None:
t2_10 = int(t2*10)/10.0
g.oldpos[t2_10] = (g.ppx, g.ppy, g.ang)
# don't put too many things in this thread
if False:
w = g.bus.read_i2c_block_data(imuaddress, 0x4c, 6)
mx0 = make_word(w[1], w[0])
my0 = make_word(w[3], w[2])
mz0 = make_word(w[5], w[4])
mx = float((mx0-g.mxmin))/(g.mxmax-g.mxmin)*2 - 1
my = float((my0-g.mymin))/(g.mymax-g.mymin)*2 - 1
mz = mz0
quot = (mx+my)/sqrt(2)
if quot > 1.0:
quot = 1.0
if quot < -1.0:
quot = -1.0
mang = (asin(quot))*180/pi+45
if mx < my:
mang = 270-mang
mang = mang%360
mang = -mang
mang = mang%360
if True:
dtup = (x, y, g.vx, g.vy, g.px,
g.py, x0, y0, vvx, vvy,
g.ppx, g.ppy, g.ang%360, angvel, g.can_steer,
g.can_speed, g.inspeed, g.outspeed, g.odometer, z0,
r, rx, ry, g.acc, g.finspeed,
g.fodometer, t2-g.t0, pleftspeed, g.leftspeed, g.fleftspeed,
g.realspeed, g.can_ultra, g.droppedlog)
fstr = "%f" + " %f"*(len(dtup)-1) + "\n"
if False:
g.accf.write(fstr % dtup)
else:
if g.qlen> 0.9*g.accfqsize:
g.droppedlog += 1
else:
if True:
g.accfq.put(fstr % dtup, False)
g.droppedlog = 0
g.qlen += 1
else:
try:
g.accfq.put(fstr % dtup, False)
g.droppedlog = 0
g.qlen += 1
except Exception as a:
print("queue exception")
if (t2-tlast > 0.1):
nav_log.tolog0("")
tlast = t2
j = g.angdiff/g.angdifffactor
g.ang += j
g.angdiff -= j
#print("pp diff %f %f" % (g.ppxdiff, g.ppydiff))
j = g.ppxdiff/g.xydifffactor
if abs(j) > 0.01:
pass
#print("ppx %f->%f j %f xdiff %f" % (g.ppx, g.ppx+g.ppxdiff,
g.ppx += j
g.ppxdiff -= j
j = g.ppydiff/g.xydifffactor
g.ppy += j
g.ppydiff -= j
if False:
if ((abs(g.ang%360 - 270) < 1.0 and
g.ppy > 18.0) or
(abs(g.ang%360 - 90) < 1.0 and
g.ppy < 14.0)):
nav_log.tolog("ang %f ppx %f ultra %f" % (
g.ang%360, g.ppx, g.can_ultra))
time.sleep(0.00001)
except Exception as e:
nav_log.tolog("exception in readgyro: " + str(e))
print("exception in readgyro: " + str(e))
def readgyro0():
#gscale = 32768.0/250
gscale = 32768.0/1000
ascale = 1670.0
x = 0.0
y = 0.0
x0 = 0.0
y0 = 0.0
z0 = 0.0
rx = 0.0
ry = 0.0
acc = 0.0
try:
tlast = time.time()
t1 = time.time()
while True:
w = g.bus.read_i2c_block_data(g.imuaddress, 0x47, 2)
high = w[0]
low = w[1]
r = make_word(high, low)
r -= g.rbias
if True:
high = g.bus.read_byte_data(g.imuaddress, 0x45)
low = g.bus.read_byte_data(g.imuaddress, 0x46)
ry = make_word(high, low)
ry -= g.rybias
w = g.bus.read_i2c_block_data(g.imuaddress, 0x43, 2)
high = w[0]
low = w[1]
rx = make_word(high, low)
rx -= g.rxbias
if False:
if rx > 120 and g.finspeed != 0 and g.dstatus != 2:
inhibitdodrive()
g.dstatus = 2
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
246, 0)
os.system(cmd)
# dodrive(0, 0)
print("stopped")
drive(0)
# make the steering and the angle go in the same direction
# now positive is clockwise
r = -r
t2 = time.time()
dt = t2-t1
t1 = t2
angvel = r/gscale
g.dang = angvel*dt
g.ang += g.dang
if True:
w = g.bus.read_i2c_block_data(g.imuaddress, 0x3b, 6)
x = make_word(w[0], w[1])
x -= g.xbias
y = make_word(w[2], w[3])
y -= g.ybias
z = make_word(w[4], w[5])
z -= g.zbias
x /= ascale
y /= ascale
z /= ascale
acc = sqrt(x*x+y*y+z*z)
if acc > 9.0 and g.detectcrashes:
g.crash = acc
x0 = -x
y0 = -y
z0 = z
# the signs here assume that x goes to the right and y forward
x = x0*cos(pi/180*g.ang) - y0*sin(pi/180*g.ang)
y = x0*sin(pi/180*g.ang) + y0*cos(pi/180*g.ang)
g.vx += x*dt
g.vy += y*dt
g.vz += z*dt
g.px += g.vx*dt
g.py += g.vy*dt
g.pz += g.vz*dt
corr = 1.0
vvx = g.inspeed*corr/100.0*sin(pi/180*g.ang)
vvy = g.inspeed*corr/100.0*cos(pi/180*g.ang)
ppxi = vvx*dt
ppyi = vvy*dt
if True:
ds = sqrt(ppxi*ppxi+ppyi*ppyi)
g.totals += ds
g.ppx += ppxi
g.ppy += ppyi
if g.oldpos != None:
t2_10 = int(t2*10)/10.0
g.oldpos[t2_10] = (g.ppx, g.ppy, g.ang)
# don't put too many things in this thread
if False:
w = g.bus.read_i2c_block_data(g.imuaddress, 0x4c, 6)
mx0 = make_word(w[1], w[0])
my0 = make_word(w[3], w[2])
mz0 = make_word(w[5], w[4])
mx = float((mx0-g.mxmin))/(g.mxmax-g.mxmin)*2 - 1
my = float((my0-g.mymin))/(g.mymax-g.mymin)*2 - 1
mz = mz0
quot = (mx+my)/sqrt(2)
if quot > 1.0:
quot = 1.0
if quot < -1.0:
quot = -1.0
mang = (asin(quot))*180/pi+45
if mx < my:
mang = 270-mang
mang = mang%360
mang = -mang
mang = mang%360
if True:
g.accf.write("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n" % (
x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx, g.ppy, g.ang,
angvel, g.can_steer, g.can_speed, g.inspeed, g.outspeed, g.odometer,
z0, r, rx, ry, acc, g.finspeed, g.fodometer, t2-g.t0, g.can_ultra))
if (t2-tlast > 0.1):
nav_log.tolog0("")
tlast = t2
j = g.angdiff/1000
g.ang += j
g.angdiff -= j
#print("pp diff %f %f" % (g.ppxdiff, g.ppydiff))
j = g.ppxdiff/100
g.ppx += j
g.ppxdiff -= j
j = g.ppydiff/100
g.ppy += j
g.ppydiff -= j
time.sleep(0.00001)
except Exception as e:
nav_log.tolog("exception in readgyro: " + str(e))
print("exception in readgyro: " + str(e))
def from_ground_control():
lastreportclosest = False
prevl = None
while True:
if g.ground_control:
for data in linesplit(g.ground_control):
#print(data)
l = data.split(" ")
#print(l)
#print(data)
g.tctime = time.time()
if l[0] == "go":
x = float(l[1])
y = float(l[2])
print(("goto is not implemented", x, y))
elif l[0] == "path":
path = ast.literal_eval(data[5:])
print("Received path from traffic control:")
print(path)
# currently, we don't do anything with this path
elif l[0] == "continue":
g.paused = False
elif l[0] == "carpos":
carname = l[1]
x = float(l[2])
y = float(l[3])
g.posnow[carname] = (x, y)
if carname not in g.otherpos:
g.otherpos[carname] = queue.Queue(100000)
g.otherpos[carname].put((x, y))
elif l[0] == "carsinfront":
if l != prevl:
pass
#print("traffic %s" % str(l[1:]))
prevl = l
n = int(l[1])
closest = None
for i in range(0, n):
relation = l[6*i+2]
dist = float(l[6*i+3])
#x = float(l[6*i+4])
#y = float(l[6*i+5])
othercar = l[6*i+6]
onlyif = float(l[6*i+7])
if closest == None or closest > dist:
closest = dist
if closest != None:
if (onlyif != 0
and g.nextdecisionpoint != 0
and onlyif != g.nextdecisionpoint):
tolog("onlyif %d %d" % (
onlyif, g.nextdecisionpoint))
continue
# a car length
closest = closest - 0.5
closest0 = closest
# some more safety:
closest = closest - 0.5
if closest < 0:
closest = 0
# 4 is our safety margin and should make for
# a smoother ride
if g.limitspeed == None:
pass
#print("car in front")
tolog("car in front")
#g.limitspeed = 100*closest/0.85/4
g.limitspeed = 100*closest/0.85
if g.limitspeed < 11:
#print("setting limitspeed to 0")
g.limitspeed = 0
if g.outspeedcm != None and g.outspeedcm != 0:
nav_signal.warningblink(True)
else:
#print("reduced limitspeed")
pass
if g.limitspeed < g.outspeedcm:
print1(g, "%s %s %.2f m speed %.1f -> %.1f (%.1f)" % (
relation,
othercar, closest0,
g.finspeed,
g.limitspeed,
g.outspeedcm))
if "crash" in relation and g.simulate:
g.crash = True
g.simulmaxacc = 0.0
ff = tolog
if g.finspeed != 0 and g.finspeed >= g.limitspeed:
if relation == "givewayto":
ff = tolog2
ff = tolog
ff("closest car in front2: %s dist %f limitspeed %f" % (
relation, closest, g.limitspeed))
lastreportclosest = True
else:
if not g.crash:
g.limitspeed = None
if lastreportclosest:
tolog0("no close cars")
pass
lastreportclosest = False
if g.outspeedcm:
# neither 0 nor None
if g.limitspeed == 0:
send_to_ground_control("message stopping for obstacle")
elif g.limitspeed != None and g.limitspeed < g.outspeedcm:
send_to_ground_control("message slowing for obstacle %.1f" % g.limitspeed)
else:
send_to_ground_control("message ")
else:
send_to_ground_control("message ")
elif l[0] == "parameter":
g.parameter = int(l[1])
print("parameter %d" % g.parameter)
# can be used so we don't have to stop if the next
# section is free
elif l[0] == "waitallcarsdone":
g.queue.put(1)
elif l[0] == "cargoto":
x = float(l[2])
y = float(l[3])
nav2.goto(x, y, l[4])
elif l[0] == "sync":
flag = int(l[1])
if flag == 1:
tctime = float(l[2])
print("syncing to %f" % (tctime))
tolog("sync1")
g.t0 = time.time() - tctime
tolog("sync2")
elif l[0] == "heartecho":
t1 = float(l[1])
t2 = float(l[2])
g.heartn_r = int(l[3])
#print("heartecho %.3f %.3f %.3f %d" % (time.time() - g.t0, t1, t2, g.heartn_r))
else:
print("unknown control command %s" % data)
time.sleep(1)
