def checkbox1(x, y, tup, leftp):
(lxprev, lyprev, lx, ly) = tup
dlx = lx-lxprev
dly = ly-lyprev
a = atan2(dlx, dly)
lx1 = lxprev + dlx*cos(a) - dly*sin(a)
ly1 = lyprev + dlx*sin(a) + dly*cos(a)
dx = x-lxprev
dy = y-lyprev
x1 = lxprev + dx*cos(a) - dy*sin(a)
y1 = lyprev + dx*sin(a) + dy*cos(a)
if y1 >= lyprev and y1 <= ly1:
if leftp:
if lxprev > x1:
print("%f %f [%f %f]" % (x, y, lxprev, x1))
nav_signal.speak("ee")
return False
else:
# when leftp==False, we have really rxprev etc.
if lxprev < x1:
print("%f %f [%f %f]" % (x, y, x1, lxprev))
nav_signal.speak("oo")
return False
return True
def checkbox1(x, y, tup, leftp):
(lxprev, lyprev, lx, ly) = tup
dlx = lx - lxprev
dly = ly - lyprev
a = atan2(dlx, dly)
lx1 = lxprev + dlx * cos(a) - dly * sin(a)
ly1 = lyprev + dlx * sin(a) + dly * cos(a)
dx = x - lxprev
dy = y - lyprev
x1 = lxprev + dx * cos(a) - dy * sin(a)
y1 = lyprev + dx * sin(a) + dy * cos(a)
if y1 >= lyprev and y1 <= ly1:
if leftp:
if lxprev > x1:
print("%f %f [%f %f]" % (x, y, lxprev, x1))
nav_signal.speak("ee")
return False
else:
# when leftp==False, we have really rxprev etc.
if lxprev < x1:
print("%f %f [%f %f]" % (x, y, x1, lxprev))
nav_signal.speak("oo")
return False
return True
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()
drive(0)
#speak("ouch")
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
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()
drive(0)
#speak("ouch")
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
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 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 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)
