def connect_to_ground_control():
while True:
if not g.ground_control:
g.s = open_socket()
if not g.s:
#print("no connection")
pass
else:
print("connection opened")
g.ground_control = g.s
start_new_thread(from_ground_control, ())
send_to_ground_control("info %s" % g.VIN)
time.sleep(5)
def connect_to_ground_control():
while True:
if not g.ground_control:
g.s = open_socket()
if not g.s:
#print("no connection")
pass
else:
print("connection opened")
g.ground_control = g.s
start_new_thread(from_ground_control, ())
now = time.time() - g.t0
send_to_ground_control("info %s %f" % (g.VIN, now))
time.sleep(5)
def gotoaux(x, y, state):
print("gotoaux %f %f %s" % (x, y, state))
driving.drive(0)
if state == "accident":
g.signalling = True
start_new_thread(signal, ())
time.sleep(4)
driving.drive(30)
status = nav2.goto_1(x, y)
if status != 0:
print("goto_1 returned %d for (%f, %f); we are at (%f, %f)" % (
status, x, y, g.ppx, g.ppy))
return False
g.signalling = False
driving.drive(0)
return True
def gotoaux(x, y, state):
print("gotoaux %f %f %s" % (x, y, state))
driving.drive(0)
if state == "accident":
g.signalling = True
start_new_thread(signal, ())
time.sleep(4)
driving.drive(30)
status = nav2.goto_1(x, y)
if status != 0:
print("goto_1 returned %d for (%f, %f); we are at (%f, %f)" %
(status, x, y, g.ppx, g.ppy))
return False
g.signalling = False
driving.drive(0)
return True
def whole4aux(path0):
global thengoal0
qfromplanner = queue.Queue(2)
qtoplanner = queue.Queue(2)
start_new_thread(planner0, (qfromplanner, qtoplanner))
qtoplanner.put(path0)
if g.simulate:
speed0 = 30
else:
speed0 = 20
driving.drive(0)
if not g.simulate:
# can be no sleep at all if we already did drive(0) earlier,
# but must maybe be 4 if we didn't.
pass
#time.sleep(4)
driving.drive(speed0)
g.last_send = None
print("speedsign = %d" % g.speedsign)
g.speedsign = 1
qfromlower = queue.Queue(2)
qtolower = queue.Queue(2)
start_new_thread(executor1, (qtolower, qfromlower))
nextplan = None
thengoal0 = None
while True:
if nextplan == None:
nextplan = qfromplanner.get()
qfromplanner.task_done()
p = nextplan
nextplan = None
thengoal0 = None
if p == 'stop':
out(1, "0 executor got stop")
driving.drive(0)
return
for status in executor0(p, qtolower, qfromlower):
if status == 0:
out(0, "0 executor failed, whole4aux exits")
driving.drive(0)
return
else:
out(1, "0 executor0 reported %d" % (status))
if not qfromplanner.empty():
if nextplan == None:
nextplan = qfromplanner.get()
qfromplanner.task_done()
out(
2, "1 next plan for executor0 is %s" %
(str(nextplan)))
if nextplan != 'stop':
thengoal0 = nextplan[1]
out(1, "0 extending with %s" % str(thengoal0))
# ugly hack, for testing
else:
out(
2,
"0 another new plan for executor0 exists, but we already have one: %s"
% str(nextplan))
def whole4(p=[35, 6]):
start_new_thread(whole4aux, (p, ))
def executor1(qfromhigher, qtohigher):
qfromplanner = queue.Queue(2)
qtoplanner = queue.Queue(2)
start_new_thread(planner1, (qfromplanner, qtoplanner))
while True:
path1 = qfromhigher.get()
qfromhigher.task_done()
out(1, "1 executor got task %s" % (str(path1)))
thengoal = None
qtoplanner.put(('path', path1))
(p, plen) = qfromplanner.get()
qfromplanner.task_done()
contflag = False
while True:
px = [i for (i, _) in p]
# printed by gopath too
#out(1, "1 executor got plan %d %s len %d" % (p[0][1], str(px), plen))
for status in gopath(p, plen):
if status == 0:
out(0, "1 gopath failed; aborting")
qtohigher.put(status)
# I think we should go the top of the outer loop,
# but returning at least aborts for real
return
else:
# out(2, "1 gopath reports %d" % status)
# we should let the planner create this plan in advance,
# shouldn't we?
qtoplanner.put(('next', thengoal))
(p, plen) = qfromplanner.get()
qfromplanner.task_done()
out(
2, "1 executor suggested new plan %s len %d" %
(str(p), plen))
qtohigher.put(2)
ack = qfromhigher.get()
qfromhigher.task_done()
#out(2, "1 higher acked %s" % str(ack))
if ack == 'pass':
pass
else:
(_, thengoal) = ack
if len(p) < 2:
# it seems len(p) is always 0 here
#out(1, "len(p) < 2: %s" % (str(p)))
continue
else:
contflag = True
break
if not contflag:
break
contflag = False
# we will just have put 2, too
qtohigher.put(1)
def gooval(perc0):
start_new_thread(goovalaux, (perc0,))
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 gooval(perc0):
start_new_thread(goovalaux, (perc0, ))
def init():
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()
nav_signal.setleds(0, 7)
start_new_thread(nav_tc.connect_to_ground_control, ())
g.logf = open("navlog", "w")
g.accf = open("acclog", "w")
#g.accf.write("%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))
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 can_ultra\n"
)
g.t0 = time.time()
print("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(nav_imu.readgyro, ())
if not g.simulate:
start_new_thread(driving.senddrive, ())
if not g.simulate:
start_new_thread(keepspeed, ())
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 goto(x, y, state):
start_new_thread(gotoaux, (x, y, state))
def goto(x, y, state):
start_new_thread(gotoaux, (x, y, state))
def speak(str):
p = 50
if g.VIN == "car2":
p = 80
start_new_thread(dospeak, (str, p))
