def drive(sp):
if True:
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
g.outspeedcm = sp*2
#print("outspeedcm = %d" % g.outspeedcm)
else:
# do this in readspeed2 instead
# maybe, but then steer will zero the speed
g.outspeed = sp
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp < 0:
sp += 256
st = g.steering
if st < 0:
st += 256
tolog("motor %d steer %d" % (sp, g.steering))
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
sp, st)
#print (sp, g.steering, cmd)
os.system(cmd)
def readmarker():
while True:
tolog("starting readmarker")
try:
readmarker0()
except Exception as e:
tolog("readmarker exception %s" % str(e))
print("readmarker exception %s" % str(e))
def stop(txt = ""):
g.steering = 0
g.outspeed = 0.0
g.speedtime = None
tolog("(%s) motor %d steer %d" % (txt, g.outspeed, g.steering))
dodrive(0, 0)
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 heartbeat2():
while True:
if g.tctime != None:
tdiff = time.time() - g.tctime
print(tdiff)
if tdiff > 0.2:
if g.limitspeed0 == "notset":
tolog("setting speed to 0 during network pause")
g.limitspeed0 = g.limitspeed
g.limitspeed = 0.0
else:
if g.limitspeed0 != "notset":
tolog("restoring speed limit to %s after network pause" % (str(g.limitspeed0)))
g.limitspeed = g.limitspeed0
g.limitspeed0 = "notset"
time.sleep(0.05)
def heartbeat():
maxdiff = 1
while True:
g.heartn += 1
diff = g.heartn - g.heartn_r
if diff < maxdiff:
if maxdiff >= g.heartwarn:
print("heart %d %d: %d (%d)" % (g.heartn, g.heartn_r, maxdiff,
time.time()))
maxdiff = 1
elif diff > maxdiff:
maxdiff = diff
if maxdiff % 50 == 0:
print("heart %d %d: %d (%d)" % (g.heartn, g.heartn_r, maxdiff,
time.time()))
nav_tc.send_to_ground_control(
"heart %.3f %d" % (time.time()-g.t0, g.heartn))
if g.heartn-g.heartn_r > 1:
tolog("waiting for heart echo %d %d" % (g.heartn, g.heartn_r))
if g.heartn-g.heartn_r > 3:
if g.limitspeed0 == "notset":
tolog("setting speed to 0 during network pause")
g.limitspeed0 = g.limitspeed
g.limitspeed = 0.0
if g.heartn-g.heartn_r < 2:
if g.limitspeed0 != "notset":
tolog("restoring speed limit to %s after network pause" % (str(g.limitspeed0)))
g.limitspeed = g.limitspeed0
g.limitspeed0 = "notset"
time.sleep(0.1)
def goto_1(x, y):
g.targetx = x
g.targety = y
missed = False
inc = 0
inc2 = 0
lastdist = None
brake_s = 0.0
tolog("goto_1 %f %f" % (x, y))
while True:
if g.remote_control:
print("remote_control is true")
return 2
if not checkpos():
if False:
print("checkpos returned False")
return 2
if g.poserror:
if False:
print("positioning system error")
tolog("poserr 1")
g.limitspeed = 0
time.sleep(10)
tolog("poserr 2")
g.limitspeed = None
print("continuing")
g.poserror = False
tolog("poserr 3")
return 2
if g.obstacle:
print("obstacle")
return 2
dist = getdist(x, y)
if g.inspeed != 0:
# Assume we are going in the direction of the target.
# At low speeds, braking time is about 1.5 s.
brake_s = 1.5 * abs(g.inspeed)/100
# say that braking distance is 1 dm at higher speed, when
# braking electrically
if g.inspeed > 0:
brake_s = 0.4
else:
brake_s = 0.6
# we should only use brake_s when we are going to stop
brake_s = 0.0
# 'lastdist' is never non-None now, nor 'missed'
if lastdist != None:
if dist < lastdist - 0.01:
inc = -1
lastdist = dist
elif dist > lastdist + 0.01:
if inc == -1:
missed = True
tolog("missed target")
inc = 1
lastdist = dist
tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y))
a = atan2(y-g.ppy, x-g.ppx)
adeg = 180/pi*a
adeg = 90-adeg
adiff = g.ang - adeg
adiff = adiff%360
tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff))
if g.speedsign < 0:
adiff += 180
if adiff > 180:
adiff -= 360
adiff = -adiff
# now, positive means the target is to the right of us
tolog("gotoa3 adiff %f" % (adiff))
#print(adiff)
# if dist < g.targetdist or dist < brake_s or missed:
if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist:
if False:
#stop("9")
# driving.drive(-1)
# continue a little so it can pass the target if it wasn't
# there yet
time.sleep(0.5)
# driving.drive(-1)
# time.sleep(0.2)
driving.drive(0)
#print("adiff %f dist %f" % (adiff, dist))
if dist < g.targetdist:
#print("dist < %f" % g.targetdist)
pass
if abs(adiff) > 90:
print("adiff = %f; leaving (%f,%f) behind" % (adiff,x,y))
return 1
return 0
asgn = sign(adiff)
aval = abs(adiff)
st = g.anglefactor*aval
if st > 100:
st = 100
st = asgn*g.speedsign*st
if False:
st_d = st - g.steering
if st_d > 10:
st = g.steering + 10
elif st_d < -10:
st = g.steering - 10
driving.steer(st)
tolog("gotoa4 steer %f" % (st))
if not g.simulate:
nav_tc.send_to_ground_control("dpos %f %f %f %f 0 %f" % (
g.ppx,g.ppy,g.ang,time.time()-g.t0, g.finspeed))
tt0 = time.time()
d = nav_map.roaddist(g.ppx, g.ppy)
if d > g.slightlyoffroad:
# print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy))
if d > g.maxoffroad:
print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy))
return 2
period = 0.1*g.speedfactor
tt1 = time.time()
dtt = tt1-tt0
dtt1 = period - dtt
if dtt1 > 0:
time.sleep(period)
def goto_1(x, y):
g.targetx = x
g.targety = y
missed = False
inc = 0
inc2 = 0
lastdist = None
brake_s = 0.0
while True:
if g.remote_control:
print("remote_control is true")
return 2
if not checkpos():
print("checkpos returned False")
return 2
if g.poserror:
if False:
print("positioning system error")
tolog("poserr 1")
g.limitspeed = 0
time.sleep(10)
tolog("poserr 2")
g.limitspeed = None
print("continuing")
g.poserror = False
tolog("poserr 3")
return 2
if g.obstacle:
print("obstacle")
return 2
dist = getdist(x, y)
if g.inspeed != 0:
# Assume we are going in the direction of the target.
# At low speeds, braking time is about 1.5 s.
brake_s = 1.5 * abs(g.inspeed) / 100
# say that braking distance is 1 dm at higher speed, when
# braking electrically
if g.inspeed > 0:
brake_s = 0.4
else:
brake_s = 0.6
# we should only use brake_s when we are going to stop
brake_s = 0.0
# 'lastdist' is never non-None now, nor 'missed'
if lastdist != None:
if dist < lastdist - 0.01:
inc = -1
lastdist = dist
elif dist > lastdist + 0.01:
if inc == -1:
missed = True
tolog("missed target")
inc = 1
lastdist = dist
tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y))
a = atan2(y - g.ppy, x - g.ppx)
adeg = 180 / pi * a
adeg = 90 - adeg
adiff = g.ang - adeg
adiff = adiff % 360
tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff))
if g.speedsign < 0:
adiff += 180
if adiff > 180:
adiff -= 360
adiff = -adiff
# now, positive means the target is to the right of us
tolog("gotoa3 adiff %f" % (adiff))
#print(adiff)
# if dist < g.targetdist or dist < brake_s or missed:
if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist:
if False:
#stop("9")
# driving.drive(-1)
# continue a little so it can pass the target if it wasn't
# there yet
time.sleep(0.5)
# driving.drive(-1)
# time.sleep(0.2)
driving.drive(0)
#print("adiff %f dist %f" % (adiff, dist))
if dist < g.targetdist:
#print("dist < %f" % g.targetdist)
pass
if abs(adiff) > 90:
print("adiff = %f; leaving (%f,%f) behind" % (adiff, x, y))
return 1
return 0
asgn = sign(adiff)
aval = abs(adiff)
st = g.anglefactor * aval
if st > 100:
st = 100
st = asgn * g.speedsign * st
if False:
st_d = st - g.steering
if st_d > 10:
st = g.steering + 10
elif st_d < -10:
st = g.steering - 10
driving.steer(st)
tolog("gotoa4 steer %f" % (st))
if not g.simulate:
nav_tc.send_to_ground_control(
"dpos %f %f %f %f 0 %f" %
(g.ppx, g.ppy, g.ang, time.time() - g.t0, g.finspeed))
tt0 = time.time()
d = eight.roaddist(g.ppx, g.ppy)
if d > g.slightlyoffroad:
# print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy))
if d > g.maxoffroad:
print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy))
return 2
tt1 = time.time()
dtt = tt1 - tt0
dtt1 = 0.1 - dtt
if dtt1 > 0:
time.sleep(0.1)
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 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 readspeed2():
part = b""
part2 = b""
while True:
try:
# 64 was 1024
data = g.canSocket.recv(64)
if (data[0], data[1]) == (100,4) and data[4] == 2:
# length of packet is 2
print((data[8], data[9]))
g.rc_button = True
time.sleep(0.00001)
elif (data[0], data[1]) == (100,4):
if data[8] == 16:
parts = str(part)
m = re.search("speed x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)", parts)
if m:
#print((time.time(),parts))
oinspeed = g.inspeed
g.inspeed = g.speedsign * int(m.group(1))
alpha = 0.8
g.inspeed_avg = (1-alpha)*g.inspeed + alpha*oinspeed
if (g.inspeed == 0 and g.speedtime != None and
time.time() - g.speedtime > 7.0):
nav_signal.speak("obstacle")
send_to_ground_control("obstacle")
g.obstacle = True
g.odometer = int(m.group(2))
if g.odometer != g.lastodometer:
send_to_ground_control("odometer %d" % (g.odometer))
g.lastodometer = g.odometer
#print("rsp-odo %d %d" % (g.inspeed, g.odometer))
g.finspeed = int(m.group(3))
g.finspeed *= g.speedsign
g.fodometer = int(m.group(4))
#print("fsp-odo %d %d" % (g.finspeed, g.fodometer))
g.leftspeed = int(m.group(5))
g.fleftspeed = int(m.group(6))
part = b""
time.sleep(0.00001)
part += data[9:]
elif (data[0], data[1]) == (1,1):
sp = data[8]
st = data[9]
if False:
if g.last_send != None and (sp, st) != g.last_send:
tolog("remote control")
g.remote_control = True
if sp > 128:
sp -= 256
g.can_speed = sp
if not g.braking:
if sp < 0:
g.speedsign = -1
elif sp > 0:
g.speedsign = 1
if st > 128:
st -= 256
tolog("CAN %d %d" % (sp, st))
g.can_steer = st
time.sleep(0.00001)
elif (data[0], data[1]) == (108,4):
# Reading DistPub this way is not a good idea, since those
# messages come often and slow down the other threads (or the
# whole process?).
# DistPub
# note that non-ASCII will appear as text \x07 in 'parts'
if data[8] == 16:
if len(part2) > 18:
part2x = part2[19:]
part2s = part2x.decode('ascii')
l = part2[18]
part2s2 = part2s[0:l]
m = re.search("^([0-9]+) ([0-9]+) $", part2s2)
if m:
#print((part2s2, len(part2), part2))
#print(part2s2)
cnt = int(m.group(1))
d = int(m.group(2))
#print((cnt,d))
g.can_ultra = d/100.0
# not used:
can_ultra_count = cnt
part2 = b""
time.sleep(0.00001)
part2 += data[9:]
except Exception as a:
print(a)
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 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 readgyro():
while True:
nav_log.tolog("starting readgyro")
readgyro0()
imuinit()
def gopath(path00, plen):
global lastwaypoint
g.last_send = None
path0 = [i for (i, _) in path00]
#out(2, "path00 = %s" % (str(path00)))
outstr = "1 gopath: %d %s" % (path00[0][1], str(path0[0:plen]))
if path00[plen:] != []:
outstr += " + %s" % (str(path0[plen:]))
out(1, outstr)
#out(2, "1 speedsign = %d" % g.speedsign)
g.speedsign = 1
# two lanes:
#path = eight.piece2path(path0, -0.25)
# experimental, to make it crash more seldom:
#path = eight.piece2path(path0, -0.1)
# single lane:
path = eight.piece2path(path0, 0)
boxp = False
if boxp:
# the simulation can't make it without bigger margins
lpath = eight.piece2path(path0, -0.15)
rpath = eight.piece2path(path0, -0.35)
lx = None
ly = None
rx = None
ry = None
i1 = -1
# for j in range(0, len(path)):
for j in range(0, plen):
(i, x, y) = path[j]
# pretend this is level 2:
out(1, "2 goto %s = %s" % (str(i), str(path00[j])))
tolog("nav1 goto_1 %d %f %f" % (i, x, y))
if i == lastwaypoint:
continue
#pass
if lastwaypoint != None:
nav_tc.send_to_ground_control("between %d %d" % (lastwaypoint, i))
lastwaypoint = i
if g.remote_control:
print("1 remote control/crash")
yield 0
if False:
i2 = i1
i1 = i
if j == len(path) - 1:
i3 = -1
else:
(i3, _, _) = path[j + 1]
nav_tc.send_to_ground_control("between %d %d %d" % (i2, i1, i3))
if boxp:
lxprev = lx
rxprev = rx
lyprev = ly
ryprev = ry
(_, lx, ly) = lpath[j]
(_, rx, ry) = rpath[j]
if lxprev != None:
if False:
print(
"keep between (%f,%f) - (%f,%f) and (%f,%f) - (%f,%f)"
% (lxprev, lyprev, lx, ly, rxprev, ryprev, rx, ry))
g.currentbox = [(lxprev, lyprev, lx, ly),
(rxprev, ryprev, rx, ry)]
if True:
status = nav2.goto_1(x, y)
else:
time.sleep(1)
status = 0
if status != 0:
out(
2, "1 goto_1 returned %d for node %d; we are at (%f, %f)" %
(status, i, g.ppx, g.ppy))
if status == 2:
#driving.drive(-20)
#time.sleep(2)
driving.drive(0)
yield 0
yield 1
return
def init():
random.seed(g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
if not g.simulate:
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eight.eightinit()
g.t0 = time.time()
nav_signal.setleds(0, 7)
suffix = ""
if g.simulate:
suffix = "-" + g.VIN
g.logf = open("navlog" + suffix, "w")
g.accf = open("acclog" + suffix, "w", 1024)
if not g.standalone:
start_new_thread(nav_tc.connect_to_ground_control, ())
#g.accf.write("%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))
g.accf.write("x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t pleftspeed leftspeed fleftspeed realspeed can_ultra droppedlog\n")
g.accfqsize = 1000
g.accfq = queue.Queue(g.accfqsize)
start_new_thread(nav_log.logthread, (g.accfq,))
g.qlen = 0
tolog("t0 = %f" % g.t0)
tolog("init")
if not g.simulate:
nav_imu.calibrate_imu()
if not g.simulate:
start_new_thread(wm.readmarker, ())
# if not g.simulate:
start_new_thread(nav_mqtt.handle_mqtt, ())
if not g.simulate:
start_new_thread(wm.readspeed2, ())
if not g.simulate:
start_new_thread_really(nav_imu.readgyro, ())
if not g.simulate:
start_new_thread(driving.senddrive, ())
if not g.simulate:
start_new_thread(keepspeed, ())
if not g.standalone:
start_new_thread(heartbeat, ())
if not g.simulate:
start_new_thread(connect_to_ecm, ())
if not g.simulate:
start_new_thread(pos_thread, ())
if g.simulate:
g.steering = 0
g.finspeed = 0
start_new_thread(wm.simulatecar, ())
def readgyro():
while True:
nav_log.tolog("starting readgyro")
readgyro0()
imuinit()
def from_ground_control():
lastreportclosest = False
while True:
if g.ground_control:
for data in linesplit(g.ground_control):
#print(data)
l = data.split(" ")
#print(l)
#print(data)
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] == "carsinfront":
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 = float(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
# 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 "crash" in relation:
g.crash = True
g.simulmaxacc = 0.0
ff = tolog
if g.finspeed != 0 and g.finspeed >= g.limitspeed:
if relation == "givewayto":
ff = tolog2
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)
def keepspeed():
outspeedi = 0
# 0 to 9
speeds = [0, 7, 11, 15, 19, 23, 27, 37, 41, 45, 49,
# 93 to 100 haven't been run yet
53, 57, 73, 77, 81, 85, 89, 93, 97, 100]
while True:
time.sleep(0.1)
if g.outspeedcm == None:
continue
spi = outspeedi
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if g.user_pause:
desiredspeed = 0
if desiredspeed > g.finspeed:
if spi < len(speeds)-1:
spi += 1
elif desiredspeed < g.finspeed:
if spi > 0:
spi -= 1
desiredspeed_sign = sign(desiredspeed)
desiredspeed_abs = abs(desiredspeed)
if True:
# bypass the control
spi = int(desiredspeed_abs/10)
if spi > len(speeds)-1:
spi = len(speeds)-1
sleeptime = 1
else:
sleeptime = 3
sp = speeds[spi]
outspeedi = spi
# spi/outspeedi don't remember the sign
sp *= desiredspeed_sign
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if g.outspeed == sp and sp != 0:
# pass
continue
if False:
print("outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f" % (
g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed))
g.outspeed = sp
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp != 0:
nav_signal.warningblink(False)
# if sp < 0:
# sp += 256
st = g.steering
# if st < 0:
# st += 256
tolog("motor %d steer %d" % (sp, st))
driving.dodrive(sp, st)
time.sleep(sleeptime)
def init():
random.seed(g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
if not g.simulate:
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eight.eightinit()
g.t0 = time.time()
nav_signal.setleds(0, 7)
suffix = ""
if g.simulate:
suffix = "-" + g.VIN
g.logf = open("navlog" + suffix, "w")
g.accf = open("acclog" + suffix, "w", 1024)
if not g.standalone:
start_new_thread(nav_tc.connect_to_ground_control, ())
#g.accf.write("%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))
g.accf.write("x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t pleftspeed leftspeed fleftspeed realspeed can_ultra droppedlog\n")
g.accfqsize = 1000
g.accfq = queue.Queue(g.accfqsize)
start_new_thread(nav_log.logthread, (g.accfq,))
g.qlen = 0
tolog("t0 = %f" % g.t0)
tolog("init")
if not g.simulate:
nav_imu.calibrate_imu()
if not g.simulate:
start_new_thread(wm.readmarker, ())
# if not g.simulate:
start_new_thread(nav_mqtt.handle_mqtt, ())
if not g.simulate:
start_new_thread(wm.readspeed2, ())
if not g.simulate:
start_new_thread_really(nav_imu.readgyro, ())
if not g.simulate:
start_new_thread(driving.senddrive, ())
if not g.simulate:
start_new_thread(keepspeed, ())
if not g.standalone:
start_new_thread(heartbeat, ())
if not g.simulate:
start_new_thread(connect_to_ecm, ())
if g.simulate:
g.steering = 0
g.finspeed = 0
start_new_thread(wm.simulatecar, ())
def readspeed2():
part = b""
part2 = b""
while True:
try:
# 64 was 1024
data = g.canSocket.recv(64)
if (data[0], data[1]) == (100, 4) and data[4] == 2:
# length of packet is 2
print((data[8], data[9]))
g.rc_button = True
time.sleep(0.00001)
elif (data[0], data[1]) == (100, 4):
if data[8] == 16:
parts = str(part)
m = re.search(
"speed x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)x([0-9 ]+)",
parts)
if m:
#print((time.time(),parts))
oinspeed = g.inspeed
g.inspeed = g.speedsign * int(m.group(1))
alpha = 0.8
g.inspeed_avg = (1 -
alpha) * g.inspeed + alpha * oinspeed
if (g.inspeed == 0 and g.speedtime != None
and time.time() - g.speedtime > 7.0):
nav_signal.speak("obstacle")
send_to_ground_control("obstacle")
g.obstacle = True
g.odometer = int(m.group(2))
if g.odometer != g.lastodometer:
send_to_ground_control("odometer %d" %
(g.odometer))
g.lastodometer = g.odometer
#print("rsp-odo %d %d" % (g.inspeed, g.odometer))
g.finspeed = int(m.group(3))
g.finspeed *= g.speedsign
g.fodometer = int(m.group(4))
#print("fsp-odo %d %d" % (g.finspeed, g.fodometer))
g.leftspeed = int(m.group(5))
g.fleftspeed = int(m.group(6))
part = b""
time.sleep(0.00001)
part += data[9:]
elif (data[0], data[1]) == (1, 1):
sp = data[8]
st = data[9]
if False:
if g.last_send != None and (sp, st) != g.last_send:
tolog("remote control")
g.remote_control = True
if sp > 128:
sp -= 256
g.can_speed = sp
if not g.braking:
if sp < 0:
g.speedsign = -1
elif sp > 0:
g.speedsign = 1
if st > 128:
st -= 256
tolog("CAN %d %d" % (sp, st))
g.can_steer = st
time.sleep(0.00001)
elif (data[0], data[1]) == (108, 4):
# Reading DistPub this way is not a good idea, since those
# messages come often and slow down the other threads (or the
# whole process?).
# DistPub
# note that non-ASCII will appear as text \x07 in 'parts'
if data[8] == 16:
if len(part2) > 18:
part2x = part2[19:]
part2s = part2x.decode('ascii')
l = part2[18]
part2s2 = part2s[0:l]
m = re.search("^([0-9]+) ([0-9]+) $", part2s2)
if m:
#print((part2s2, len(part2), part2))
#print(part2s2)
cnt = int(m.group(1))
d = int(m.group(2))
#print((cnt,d))
g.can_ultra = d / 100.0
# not used:
can_ultra_count = cnt
part2 = b""
time.sleep(0.00001)
part2 += data[9:]
except Exception as a:
print(a)
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
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 keepspeed():
outspeedi = 0
# 0 to 9
speeds = [0, 7, 11, 15, 19, 23, 27, 37, 41, 45, 49,
# 93 to 100 haven't been run yet
53, 57, 73, 77, 81, 85, 89, 93, 97, 100]
while True:
time.sleep(0.1)
if g.outspeedcm == None:
continue
spi = outspeedi
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if g.user_pause:
desiredspeed = 0
if desiredspeed > g.finspeed:
if spi < len(speeds)-1:
spi += 1
elif desiredspeed < g.finspeed:
if spi > 0:
spi -= 1
desiredspeed_sign = sign(desiredspeed)
desiredspeed_abs = abs(desiredspeed)
if True:
# bypass the control
spi = int(desiredspeed_abs/10)
if spi > len(speeds)-1:
spi = len(speeds)-1
sleeptime = 1
else:
sleeptime = 3
sp = speeds[spi]
outspeedi = spi
# spi/outspeedi don't remember the sign
sp *= desiredspeed_sign
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if g.outspeed == sp and sp != 0:
# pass
continue
if False:
print("outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f" % (
g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed))
g.outspeed = sp
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp != 0:
nav_signal.warningblink(False)
# if sp < 0:
# sp += 256
st = g.steering
# if st < 0:
# st += 256
tolog("motor %d steer %d" % (sp, st))
driving.dodrive(sp, st)
time.sleep(sleeptime)
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)
