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 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)