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 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 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 False:
print(
"outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f"
% (g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed))
if g.outspeed == sp and sp != 0:
# pass
continue
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:
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
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):
#dir = float(l[5*i+2])
dist = float(l[5 * i + 3])
#x = float(l[5*i+4])
#y = float(l[5*i+5])
othercar = float(l[5 * i + 6])
if closest == None or closest > dist:
closest = dist
if closest:
#print("closest car in front1: dir %f dist %f" % (
# dir, closest))
# 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:
print("car in front")
tolog("car in front")
g.limitspeed = 100 * closest / 0.85 / 4
if g.limitspeed < 11:
#print("setting limitspeed to 0")
g.limitspeed = 0
if g.outspeedcm != None and g.outspeedcm != 0:
warningblink(True)
else:
#print("reduced limitspeed")
pass
#print("closest car in front2: dir %f dist %f limitspeed %f" % (
#dir, closest, g.limitspeed))
lastreportclosest = True
else:
g.limitspeed = None
if lastreportclosest:
#print("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 %f" %
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] == "free":
s = int(l[1])
g.section_status[s] = "free"
elif l[0] == "occupied":
s = int(l[1])
g.section_status[s] = "occupied"
elif l[0] == "cargoto":
x = float(l[2])
y = float(l[3])
goto(x, y, l[4])
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 from_ground_control():
lastreportclosest = False
prevl = None
while True:
if g.ground_control:
for data in linesplit(g.ground_control):
#print(data)
l = data.split(" ")
#print(l)
#print(data)
g.tctime = time.time()
if l[0] == "go":
x = float(l[1])
y = float(l[2])
print(("goto is not implemented", x, y))
elif l[0] == "path":
path = ast.literal_eval(data[5:])
print("Received path from traffic control:")
print(path)
# currently, we don't do anything with this path
elif l[0] == "continue":
g.paused = False
elif l[0] == "carpos":
carname = l[1]
x = float(l[2])
y = float(l[3])
g.posnow[carname] = (x, y)
if carname not in g.otherpos:
g.otherpos[carname] = queue.Queue(100000)
g.otherpos[carname].put((x, y))
elif l[0] == "carsinfront":
if l != prevl:
pass
#print("traffic %s" % str(l[1:]))
prevl = l
n = int(l[1])
closest = None
for i in range(0, n):
relation = l[6 * i + 2]
dist = float(l[6 * i + 3])
#x = float(l[6*i+4])
#y = float(l[6*i+5])
othercar = l[6 * i + 6]
onlyif = float(l[6 * i + 7])
if closest == None or closest > dist:
closest = dist
if closest != None:
if (onlyif != 0 and g.nextdecisionpoint != 0
and onlyif != g.nextdecisionpoint):
tolog("onlyif %d %d" %
(onlyif, g.nextdecisionpoint))
continue
# a car length
closest = closest - 0.5
closest0 = closest
# some more safety:
closest = closest - 0.5
if closest < 0:
closest = 0
# 4 is our safety margin and should make for
# a smoother ride
if g.limitspeed == None:
pass
#print("car in front")
tolog("car in front")
#g.limitspeed = 100*closest/0.85/4
g.limitspeed = 100 * closest / 0.85
if g.limitspeed < 11:
#print("setting limitspeed to 0")
g.limitspeed = 0
if g.outspeedcm != None and g.outspeedcm != 0:
nav_signal.warningblink(True)
else:
#print("reduced limitspeed")
pass
if g.limitspeed < g.outspeedcm:
print1(
g, "%s %s %.2f m speed %.1f -> %.1f (%.1f)" %
(relation, othercar, closest0, g.finspeed,
g.limitspeed, g.outspeedcm))
if "crash" in relation and g.simulate:
g.crash = True
g.simulmaxacc = 0.0
ff = tolog
if g.finspeed != 0 and g.finspeed >= g.limitspeed:
if relation == "givewayto":
ff = tolog2
ff = tolog
ff("closest car in front2: %s dist %f limitspeed %f" %
(relation, closest, g.limitspeed))
lastreportclosest = True
else:
if not g.crash:
g.limitspeed = None
if lastreportclosest:
tolog0("no close cars")
pass
lastreportclosest = False
if g.outspeedcm:
# neither 0 nor None
if g.limitspeed == 0:
send_to_ground_control(
"message stopping for obstacle")
elif g.limitspeed != None and g.limitspeed < g.outspeedcm:
send_to_ground_control(
"message slowing for obstacle %.1f" %
g.limitspeed)
else:
send_to_ground_control("message ")
else:
send_to_ground_control("message ")
elif l[0] == "parameter":
g.parameter = int(l[1])
print("parameter %d" % g.parameter)
# control be used so we don't have to stop if the next
# section is free
elif l[0] == "waitallcarsdone":
g.queue.put(1)
elif l[0] == "cargoto":
x = float(l[2])
y = float(l[3])
nav2.goto(x, y, l[4])
elif l[0] == "sync":
flag = int(l[1])
if flag == 1:
tctime = float(l[2])
print("syncing to %f" % (tctime))
tolog("sync1")
g.t0 = time.time() - tctime
tolog("sync2")
elif l[0] == "heartecho":
t1 = float(l[1])
t2 = float(l[2])
g.heartn_r = int(l[3])
#print("heartecho %.3f %.3f %.3f %d" % (time.time() - g.t0, t1, t2, g.heartn_r))
else:
print("unknown control command %s" % data)
time.sleep(1)
def 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):
#dir = float(l[5*i+2])
dist = float(l[5*i+3])
#x = float(l[5*i+4])
#y = float(l[5*i+5])
othercar = float(l[5*i+6])
if closest == None or closest > dist:
closest = dist
if closest:
#print("closest car in front1: dir %f dist %f" % (
# dir, closest))
# 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:
print("car in front")
tolog("car in front")
g.limitspeed = 100*closest/0.85/4
if g.limitspeed < 11:
#print("setting limitspeed to 0")
g.limitspeed = 0
if g.outspeedcm != None and g.outspeedcm != 0:
warningblink(True)
else:
#print("reduced limitspeed")
pass
#print("closest car in front2: dir %f dist %f limitspeed %f" % (
#dir, closest, g.limitspeed))
lastreportclosest = True
else:
g.limitspeed = None
if lastreportclosest:
#print("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 %f" % 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] == "free":
s = int(l[1])
g.section_status[s] = "free"
elif l[0] == "occupied":
s = int(l[1])
g.section_status[s] = "occupied"
elif l[0] == "cargoto":
x = float(l[2])
y = float(l[3])
goto(x, y, l[4])
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 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)