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)