currentmarkpos = None
global markedpoint
markedpoint = None
areaobjs = []
pathlist = []
global currentpath
obstacles = dict()
sys.path.append("car-control2")
import eight
eight.eightinit()
def draw_way(offset, w, **kargs):
path = [(i, eight.nodes[i]) for i in eight.ways[w]]
p = eight.makepath(offset, path)
if offset != 0:
for (i, x, y) in p:
print "%f %d %f %f" % (offset, i, x, y)
draw_path(p, **kargs)
# If the first point is repeated as the last point, we consider the
# path closed, otherwise not.
def draw_path(p, **kargs):
first = True
for cmd in p:
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 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, ())
currentcarcircle = None
currentmark = None
currentmarkpos = None
global markedpoint
markedpoint = None
areaobjs = []
pathlist = []
global currentpath
obstacles = dict()
eight.eightinit()
def draw_way(offset, w, **kargs):
path = [(i, eight.nodes[i]) for i in eight.ways[w]]
p = nav_map.makepath(offset, path)
if offset != 0:
for (i, x, y) in p:
print "%f %d %f %f" % (offset, i, x, y)
draw_path(p, **kargs)
# If the first point is repeated as the last point, we consider the
# path closed, otherwise not.
def draw_path(p, **kargs):
first = True
for cmd in p:
