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 heartbeat():
maxdiff = 1
while True:
g.heartn += 1
diff = g.heartn - g.heartn_r
if diff < maxdiff:
if maxdiff > 2:
print("heart %d %d: %d" % (g.heartn, g.heartn_r, maxdiff))
maxdiff = 1
elif diff > maxdiff:
maxdiff = diff
if maxdiff % 50 == 0:
print("heart %d %d: %d" % (g.heartn, g.heartn_r, maxdiff))
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 on_message(mosq, obj, msg):
p = str(msg.payload)
while p[-1] == '\n' or p[-1] == '\t':
p = p[:-1]
# ignore our own position
if g.VIN in msg.topic:
return
#sys.stdout.write(msg.topic + " " + str(msg.qos) + " " + p + "\n")
# "adc","current_value":"7.7142 7.630128199403445"
m = re.search('"adc","current_value":"', p)
if m:
m1 = re.search('"vin":"%s"' % g.VIN, p)
if m1:
# since the second value can be garbage
m = re.search('"adc","current_value":"[0-9.]+ ([0-9.]+)"', p)
if m:
#print("battery %s" % m.group(1))
g.battery = float(m.group(1))
if g.battery < 20:
send_to_ground_control("battery %f" % g.battery)
return
# We still read this, but we don't use 'ultra' - we use 'can_ultra'
m = re.search('"DistPub","current_value":"', p)
if m:
m1 = re.search('"vin":"%s"' % g.VIN, p)
if m1:
# since the second value can be garbage
m = re.search('"DistPub","current_value":"[0-9]+ ([0-9]+)"', p)
if m:
g.ultra = float(m.group(1))
return
def connect_to_ecm():
stopped = False
s = nav_comm.open_socket2()
ecmt0 = time.time()
counter = 0
g.crashacc = None
while True:
if g.crashacc:
#g.crash = False
if not stopped:
counter = 9
#s.send("crash".encode('ascii'))
print("crash: %f" % g.crash)
g.remote_control = True
#stop()
driving.drive(0)
#speak("ouch")
nav_signal.warningblink(True)
stopped = True
s112 = nav_comm.open_socket3()
sstr = ('accident %f %f %f %s\n' %
(g.ppx, g.ppy, g.crashacc, g.VIN)).encode("ascii")
print(sstr)
s112.send(sstr)
print(sstr)
resp = s112.recv(1024)
print(resp)
resp = resp.decode("ascii")
print(resp)
if True:
resp = json.loads(resp)
print(resp)
say = resp['speak']
else:
say = resp
print(say)
speak(say)
s112.close()
if True:
t = time.time()
if t - ecmt0 > 1.0:
#s.send("bar".encode('ascii'))
g.crashacc = 0.0
if g.crash:
g.crashacc = g.crash
nav_tc.send_to_ground_control("message crash %f" %
g.crashacc)
s.send(('{"crash":%d, "x":%f, "y":%f, "crashacc":%f}\n' %
(counter, g.ppx, g.ppy, g.crashacc)).encode("ascii"))
ecmt0 = t
time.sleep(0.05)
if counter != 9:
counter = (counter + 1) % 8
def connect_to_ecm():
stopped = False
s = nav_comm.open_socket2()
ecmt0 = time.time()
counter = 0
g.crashacc = None
while True:
if g.crashacc:
#g.crash = False
if not stopped:
counter = 9
#s.send("crash".encode('ascii'))
print("crash: %f" % g.crash)
g.remote_control = True
#stop()
driving.drive(0)
#speak("ouch")
nav_signal.warningblink(True)
stopped = True
s112 = nav_comm.open_socket3()
sstr = ('accident %f %f %f %s\n' % (
g.ppx, g.ppy, g.crashacc, g.VIN)).encode("ascii")
print(sstr)
s112.send(sstr)
print(sstr)
resp = s112.recv(1024)
print(resp)
resp = resp.decode("ascii")
print(resp)
if True:
resp = json.loads(resp)
print(resp)
say = resp['speak']
else:
say = resp
print(say)
speak(say)
s112.close()
if True:
t = time.time()
if t-ecmt0 > 1.0:
#s.send("bar".encode('ascii'))
g.crashacc = 0.0
if g.crash:
g.crashacc = g.crash
nav_tc.send_to_ground_control(
"message crash %f" % g.crashacc)
s.send(('{"crash":%d, "x":%f, "y":%f, "crashacc":%f}\n' % (
counter, g.ppx, g.ppy, g.crashacc)).encode("ascii"))
ecmt0 = t
time.sleep(0.05)
if counter != 9:
counter = (counter+1)%8
def trip(path, first=0):
g.speakcount = 1
setleds(0, 7)
i = 0
while True:
if g.rc_button:
setleds(1, 6)
stop()
break
j = 0
if first > 0:
path1 = path[first:]
else:
path1 = path
for cmd in path1:
if cmd[0] == 'go':
sp = cmd[1]
sp = int(sp * g.parameter / 100.0)
x = cmd[2]
y = cmd[3]
spdiff = sp - g.outspeed
if spdiff > 20:
# this should be done in a separate thread
drive(sp - spdiff / 2)
time.sleep(0.5)
drive(sp)
else:
drive(sp)
goto_1(x, y)
elif cmd[0] == 'stop':
if len(cmd) > 1:
t = float(cmd[1])
else:
t = 3
g.paused = True
stopx(i, t)
send_to_ground_control("stopat %d" % j)
while g.paused:
time.sleep(1)
elif cmd[0] == 'speak':
speak(cmd[1])
else:
print("unknown path command: %s" % cmd)
j += 1
first = 0
g.speakcount += 1
def trip(path, first=0):
g.speakcount = 1
setleds(0, 7)
i = 0
while True:
if g.rc_button:
setleds(1, 6)
stop()
break
j = 0
if first > 0:
path1 = path[first:]
else:
path1 = path
for cmd in path1:
if cmd[0] == 'go':
sp = cmd[1]
sp = int(sp*g.parameter/100.0)
x = cmd[2]
y = cmd[3]
spdiff = sp-g.outspeed
if spdiff > 20:
# this should be done in a separate thread
drive(sp-spdiff/2)
time.sleep(0.5)
drive(sp)
else:
drive(sp)
goto_1(x, y)
elif cmd[0] == 'stop':
if len(cmd) > 1:
t = float(cmd[1])
else:
t = 3
g.paused = True
stopx(i, t)
send_to_ground_control("stopat %d" % j)
while g.paused:
time.sleep(1)
elif cmd[0] == 'speak':
speak(cmd[1])
else:
print("unknown path command: %s" % cmd)
j += 1
first = 0
g.speakcount += 1
def simulatecar():
dt = 0.1
# acc 0.0 in fact represents infinity
g.simulmaxacc = 0.6
acc = 0.0
while True:
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if abs(g.finspeed - desiredspeed) < 5 or g.simulmaxacc == 0.0:
acc = 0.0
g.finspeed = desiredspeed
elif g.finspeed > desiredspeed:
acc = -g.simulmaxacc
elif g.finspeed < desiredspeed:
acc = g.simulmaxacc
else:
acc = 0.0
if acc != 0.0:
ospeed = g.finspeed
g.finspeed += 100*acc*dt
if ospeed > 0 and g.finspeed < 0:
g.finspeed = 0
g.inspeed = g.finspeed
g.dang = g.steering/100.0 * 1.5 * desiredspeed
g.ang += g.dang*dt
g.ppx += g.finspeed/100*dt*sin(g.ang*pi/180)
g.ppy += g.finspeed/100*dt*cos(g.ang*pi/180)
# make the steering a little flaky
f = random.random()
g.ang += (2*f-1) * 0
time.sleep(dt*g.speedfactor)
send_to_ground_control("dpos %f %f %f %f 0 %f" % (
g.ppx, g.ppy, g.ang, 0, g.finspeed))
nav_mqtt.send_to_mqtt(g.ppx, g.ppy, g.ang)
def simulatecar():
dt = 0.1
# acc 0.0 in fact represents infinity
g.simulmaxacc = 0.6
acc = 0.0
while True:
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if abs(g.finspeed - desiredspeed) < 5 or g.simulmaxacc == 0.0:
acc = 0.0
g.finspeed = desiredspeed
elif g.finspeed > desiredspeed:
acc = -g.simulmaxacc
elif g.finspeed < desiredspeed:
acc = g.simulmaxacc
else:
acc = 0.0
if acc != 0.0:
ospeed = g.finspeed
g.finspeed += 100 * acc * dt
if ospeed > 0 and g.finspeed < 0:
g.finspeed = 0
g.inspeed = g.finspeed
g.dang = g.steering / 100.0 * 1.5 * desiredspeed
g.ang += g.dang * dt
g.ppx += g.finspeed / 100 * dt * sin(g.ang * pi / 180)
g.ppy += g.finspeed / 100 * dt * cos(g.ang * pi / 180)
# make the steering a little flaky
f = random.random()
g.ang += (2 * f - 1) * 0
time.sleep(dt * g.speedfactor)
send_to_ground_control("dpos %f %f %f %f 0 %f" %
(g.ppx, g.ppy, g.ang, 0, g.finspeed))
nav_mqtt.send_to_mqtt(g.ppx, g.ppy, g.ang)
def gopath(path0, dir=-1):
g.last_send = None
print("speedsign = %d" % g.speedsign)
g.speedsign = 1
path = piece2path(path0, dir, 0.25)
lpath = piece2path(path0, dir, 0.15)
rpath = piece2path(path0, dir, 0.35)
lx = None
ly = None
rx = None
ry = None
i1 = -1
for j in range(0, len(path)):
(_, _, i, x, y) = path[j]
if g.remote_control:
print("whole4 finished")
return
i2 = i1
i1 = i
if j == len(path)-1:
i3 = -1
else:
(_, _, i3, _, _) = path[j+1]
send_to_ground_control("between %d %d %d" % (i2, i1, i3))
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)]
goto_1(x, y)
def gopath(path0, dir=-1):
g.last_send = None
print("speedsign = %d" % g.speedsign)
g.speedsign = 1
path = piece2path(path0, dir, 0.25)
lpath = piece2path(path0, dir, 0.15)
rpath = piece2path(path0, dir, 0.35)
lx = None
ly = None
rx = None
ry = None
i1 = -1
for j in range(0, len(path)):
(_, _, i, x, y) = path[j]
if g.remote_control:
print("whole4 finished")
return
i2 = i1
i1 = i
if j == len(path) - 1:
i3 = -1
else:
(_, _, i3, _, _) = path[j + 1]
send_to_ground_control("between %d %d %d" % (i2, i1, i3))
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)]
goto_1(x, y)
def heartbeat():
while True:
g.heartn += 1
send_to_ground_control("heart %.3f %d" % (time.time()-g.t0, g.heartn))
if g.heartn-g.heartn_r > 1:
print("waiting for heart echo %d %d" % (g.heartn, g.heartn_r))
if g.heartn-g.heartn_r > 3:
if g.limitspeed0 == "notset":
print("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":
print("restoring speed limit to %s after network pause" % (str(g.limitspeed0)))
g.limitspeed = g.limitspeed0
g.limitspeed0 = "notset"
time.sleep(5)
def simulatecar():
dt = 0.1
while True:
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
g.finspeed = desiredspeed
g.dang = g.steering/100.0 * 1.5 * desiredspeed
g.ang += g.dang*dt
g.ppx += g.finspeed/100*dt*sin(g.ang*pi/180)
g.ppy += g.finspeed/100*dt*cos(g.ang*pi/180)
# make the steering a little flaky
f = random.random()
g.ang += (2*f-1) * 0
time.sleep(dt)
send_to_ground_control("dpos %f %f %f %f 0 %f" % (
g.ppx, g.ppy, g.ang, 0, g.finspeed))
def heartbeat():
while True:
g.heartn += 1
send_to_ground_control("heart %.3f %d" %
(time.time() - g.t0, g.heartn))
if g.heartn - g.heartn_r > 1:
print("waiting for heart echo %d %d" % (g.heartn, g.heartn_r))
if g.heartn - g.heartn_r > 3:
if g.limitspeed0 == "notset":
print("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":
print("restoring speed limit to %s after network pause" %
(str(g.limitspeed0)))
g.limitspeed = g.limitspeed0
g.limitspeed0 = "notset"
time.sleep(5)
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
tolog0("marker0 %s age %d" % (m, g.age))
m1 = m.split(" ")
if len(m1) != 7:
print("bad marker line")
continue
if m1 == "":
g.age += 1
else:
t = time.time()
doadjust = False
#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 > 0.35
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
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
g.age += 1
else:
accepted = True
g.markercnt += 1
tolog0("marker1 %s %d %f %f" % (m, g.age, 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
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.age = 0
#g.ang = ori
else:
g.age += 1
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 %d %f %f" % (m, g.age, g.ang, ori))
# tolog0("marker2 %d %f %f %d %f %d %f" % (-1, g.px, g.py, int(g.ang), 0.5, g.age, g.ang))
# tolog0("marker3 %d %f %f %d %f %d %f" % (-1, g.ppx, g.ppy, int(g.ang), 0.5, g.age, g.ang))
time.sleep(0.00001)
def wait1():
nav_tc.send_to_ground_control("waitallcars\n")
x = g.queue.get()
g.queue.task_done()
def pos_thread():
while True:
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))
time.sleep(0.1)
def goto_1(x, y):
g.targetx = x
g.targety = y
#TARGETDIST = 0.3
TARGETDIST = 0.15
TARGETDIST = 0.25
missed = False
inc = 0
inc2 = 0
lastdist = None
brake_s = 0.0
while True:
if g.remote_control:
print("remote_control is true")
return
checkpos()
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 < TARGETDIST or dist < brake_s or missed:
if (not g.allangles and abs(adiff) > 90) or dist < 0.3:
if False:
#stop("9")
# drive(-1)
# continue a little so it can pass the target if it wasn't
# there yet
time.sleep(0.5)
# drive(-1)
# time.sleep(0.2)
drive(0)
#print("adiff %f dist %f" % (adiff, dist))
if dist < 0.3:
#print("dist < 0.3")
pass
if abs(adiff) > 90:
print("adiff = %f" % adiff)
return
asgn = sign(adiff)
aval = abs(adiff)
p = 4.0
st = p*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
steer(st)
tolog("gotoa4 steer %f" % (st))
send_to_ground_control("dpos %f %f %f %f 0 %f" % (g.ppx,g.ppy,g.ang,time.time()-g.t0, g.finspeed))
time.sleep(0.1)
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 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 readspeed2():
part = b""
part2 = b""
while True:
data = g.canSocket.recv(1024)
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
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 ]+)", parts)
if m:
#print(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))
part = b""
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
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:
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""
part2 += data[9:]
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 wait1():
nav_tc.send_to_ground_control("waitallcars\n")
x = g.queue.get()
g.queue.task_done()
def whole4aux(dir):
# 'dir' is obsolete - it should always be -1
speed0 = 20
drive(0)
time.sleep(4)
drive(speed0)
g.last_send = None
print("speedsign = %d" % g.speedsign)
g.speedsign = 1
#path0 = rev(piece3b) + [23]
path0 = rev(piece5) + rev(piece1)
print("path0 = %s" % str(path0))
# idea: from the current position, determine which piece we can
# start with
path = piece2path(path0, dir, 0.25)
lpath = piece2path(path0, dir, 0.15)
rpath = piece2path(path0, dir, 0.35)
lx = None
ly = None
rx = None
ry = None
if dir == 1:
path.reverse()
i1 = -1
while True:
i10 = path[-1][2]
# from eight.py:
if interleave == 2:
i2 = path[-3][2]
else:
i2 = path[-2][2]
if (i10, i2) == (23, 26):
nextpiece = randsel(piece2b, rev(piece3a))
#nextpiece = rev(piece3a)
elif (i10, i2) == (6, 13):
nextpiece = piece1
elif (i10, i2) == (36, 30):
nextpiece = randsel(piece2a + [23], piece5 + piece4)
#nextpiece = piece5 + piece4
elif (i10, i2) == (23, 27):
nextpiece = randsel(rev(piece3a), piece2b)
#nextpiece = piece2b
elif (i10, i2) == (5, 10):
nextpiece = rev(piece4)
elif (i10, i2) == (33, 31):
nextpiece = randsel(rev(piece3b) + [23], rev(piece5) + rev(piece1))
#nextpiece = rev(piece5) + rev(piece1)
elif (i10, i2) == (23, 19):
nextpiece = randsel(rev(piece2a), piece3b)
#nextpiece = piece3b
elif (i10, i2) == (23, 16):
nextpiece = randsel(rev(piece2a), piece3b)
#nextpiece = rev(piece2a)
elif (i10, i2) == (4, 12):
#nextpiece = randsel(piece6 + piece1, piece3a + [23])
nextpiece = piece3a + [23]
elif (i10, i2) == (7, 11):
nextpiece = randsel(rev(piece6) + rev(piece4), rev(piece2b) + [23])
#nextpiece = rev(piece2b) + [23]
elif (i10, i2) == (35, 32):
nextpiece = rev(piece1)
elif (i10, i2) == (34, 29):
nextpiece = piece4
else:
drive(0)
return
#print("nextpiece = %s" % str(nextpiece))
for j in range(0, len(path)):
(_, _, i, x, y) = path[j]
if g.remote_control:
print("whole4 finished")
return
i2 = i1
i1 = i
if j == len(path) - 1:
i3 = nextpiece[0]
else:
(_, _, i3, _, _) = path[j + 1]
send_to_ground_control("between %d %d %d" % (i2, i1, i3))
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)]
goto_1(x, y)
# idea: let the connecting node always be a part in both
# pieces; then we get a free check whether they actually go
# together
path = piece2path(nextpiece, dir, 0.25)
lpath = piece2path(nextpiece, dir, 0.15)
rpath = piece2path(nextpiece, dir, 0.35)
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 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 control 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 whole4aux(dir):
# 'dir' is obsolete - it should always be -1
speed0 = 20
drive(0)
time.sleep(4)
drive(speed0)
g.last_send = None
print("speedsign = %d" % g.speedsign)
g.speedsign = 1
#path0 = rev(piece3b) + [23]
path0 = rev(piece5) + rev(piece1)
print("path0 = %s" % str(path0))
# idea: from the current position, determine which piece we can
# start with
path = piece2path(path0, dir, 0.25)
lpath = piece2path(path0, dir, 0.15)
rpath = piece2path(path0, dir, 0.35)
lx = None
ly = None
rx = None
ry = None
if dir == 1:
path.reverse()
i1 = -1
while True:
i10 = path[-1][2]
# from eight.py:
if interleave == 2:
i2 = path[-3][2]
else:
i2 = path[-2][2]
if (i10, i2) == (23, 26):
nextpiece = randsel(piece2b, rev(piece3a))
#nextpiece = rev(piece3a)
elif (i10, i2) == (6, 13):
nextpiece = piece1
elif (i10, i2) == (36, 30):
nextpiece = randsel(piece2a + [23], piece5 + piece4)
#nextpiece = piece5 + piece4
elif (i10, i2) == (23, 27):
nextpiece = randsel(rev(piece3a), piece2b)
#nextpiece = piece2b
elif (i10, i2) == (5, 10):
nextpiece = rev(piece4)
elif (i10, i2) == (33, 31):
nextpiece = randsel(rev(piece3b) + [23],
rev(piece5) + rev(piece1))
#nextpiece = rev(piece5) + rev(piece1)
elif (i10, i2) == (23, 19):
nextpiece = randsel(rev(piece2a), piece3b)
#nextpiece = piece3b
elif (i10, i2) == (23, 16):
nextpiece = randsel(rev(piece2a), piece3b)
#nextpiece = rev(piece2a)
elif (i10, i2) == (4, 12):
#nextpiece = randsel(piece6 + piece1, piece3a + [23])
nextpiece = piece3a + [23]
elif (i10, i2) == (7, 11):
nextpiece = randsel(rev(piece6) + rev(piece4),
rev(piece2b) + [23])
#nextpiece = rev(piece2b) + [23]
elif (i10, i2) == (35, 32):
nextpiece = rev(piece1)
elif (i10, i2) == (34, 29):
nextpiece = piece4
else:
print((i10,i2))
drive(0)
return
#print("nextpiece = %s" % str(nextpiece))
for j in range(0, len(path)):
(_, _, i, x, y) = path[j]
if g.remote_control:
print("whole4 finished")
return
i2 = i1
i1 = i
if j == len(path)-1:
i3 = nextpiece[0]
else:
(_, _, i3, _, _) = path[j+1]
send_to_ground_control("between %d %d %d" % (i2, i1, i3))
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)]
goto_1(x, y)
# idea: let the connecting node always be a part in both
# pieces; then we get a free check whether they actually go
# together
path = piece2path(nextpiece, dir, 0.25)
lpath = piece2path(nextpiece, dir, 0.15)
rpath = piece2path(nextpiece, dir, 0.35)
def pos_thread():
while True:
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))
time.sleep(0.1)
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 = nav_map.piece2path(path0, -0.25)
# experimental, to make it crash more seldom:
#path = nav_map.piece2path(path0, -0.1)
# single lane:
path = nav_map.piece2path(path0, 0)
boxp = False
if boxp:
# the simulation can't make it without bigger margins
lpath = nav_map.piece2path(path0, -0.15)
rpath = nav_map.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 goto_1(x, y):
g.targetx = x
g.targety = y
#TARGETDIST = 0.3
TARGETDIST = 0.15
TARGETDIST = 0.25
missed = False
inc = 0
inc2 = 0
lastdist = None
brake_s = 0.0
while True:
if g.remote_control:
print("remote_control is true")
return
checkpos()
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 < TARGETDIST or dist < brake_s or missed:
if (not g.allangles and abs(adiff) > 90) or dist < 0.3:
if False:
#stop("9")
# drive(-1)
# continue a little so it can pass the target if it wasn't
# there yet
time.sleep(0.5)
# drive(-1)
# time.sleep(0.2)
drive(0)
#print("adiff %f dist %f" % (adiff, dist))
if dist < 0.3:
#print("dist < 0.3")
pass
if abs(adiff) > 90:
print("adiff = %f" % adiff)
return
asgn = sign(adiff)
aval = abs(adiff)
p = 4.0
st = p * 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
steer(st)
tolog("gotoa4 steer %f" % (st))
send_to_ground_control(
"dpos %f %f %f %f 0 %f" %
(g.ppx, g.ppy, g.ang, time.time() - g.t0, g.finspeed))
time.sleep(0.1)
