def main():
tab = Tab()
history = History()
tab.setUrl("http://tutorialspoint.com")
history.add(tab.saveUrl())
tab.setUrl("https://www.python.org/doc/")
history.add(tab.saveUrl())
tab.setUrl("http://www.trome.com.pe")
print("Current web url: " + tab.getUrl())
print("Back button is clicked.. ")
tab.restoreFromSavedUrl(history.get(1))
print("Saved url: " + tab.getUrl())
print("Back button again.. ")
tab.restoreFromSavedUrl(history.get(0))
print("Saved url: " + tab.getUrl())
def testPredicateAll(self):
historyBuffer=['load smth',
'export aa=cc',
'history clear']
expected=['load smth','export aa=cc']
readLine = ReadLine()
readLine.getLineBuffer = MagicMock(return_value=historyBuffer)
history = History( readLine )
result = history.get( history.passAllPredicate )
self.assertEquals(expected, result)
class Interpreter(Cmd):
HEADER = '\033[95m'
OKBLUE = '\033[94m'
OKGREEN = '\033[92m'
BRYELLOW = '\033[93m'
YELLOW = '\033[33m'
FAIL = '\033[91m'
ENDC = '\033[0m'
INTRO = BRYELLOW + \
''' __
<(\033[91mo\033[93m )___
( ._> /
`---'
''' + ENDC
def __init__(self):
Cmd.__init__(self)
if sys.platform.startswith("win"):
self.__disableColors()
self.__home = os.path.expanduser("~")
self.__appDir = os.path.dirname(__file__)
self.__historyFile = os.path.normpath(self.__home + "/.pkt_synth")
self.__packetGenerator = None
self.__packetSender = None
self.__context = {}
self.__packet_context_difference = set([])
self.__sender_context_difference = set([])
self.__listPacketGenerators = self.__findInheritedClasses('AbstractPacket')
self.__listPacketSenders = self.__findInheritedClasses('AbstractSender')
self.prompt = self.OKGREEN + ">> " + self.ENDC
self.__valueParser = VariableValueParser(self.__context)
readlineImpl = ReadLineImpl(self.__historyFile)
self.__history = History( readlineImpl )
self.__history.load()
def __disableColors(self):
self.HEADER = ''
self.OKBLUE = ''
self.OKGREEN = ''
self.BRYELLOW = ''
self.YELLOW = ''
self.FAIL = ''
self.ENDC = ''
self.INTRO =\
''' __
<(o )___
( ._> /
`---'
'''
def __findInheritedClasses(self, baseClass):
fileExtensions = ('.py')
definedModules = []
allClasses = {}
for fileInCurDir in os.listdir(self.__appDir):
if fileInCurDir.endswith(fileExtensions):
filename = fileInCurDir.split('.')[0]
fp, pathname, description = imp.find_module(filename)
module = imp.load_module(filename, fp, pathname, description)
for name, value in inspect.getmembers(module):
if inspect.isclass(value):
if str(name).find(baseClass) == -1:
allClasses[name] = value
for className in allClasses:
if str(inspect.getmro(allClasses[className])).find(baseClass + ".") != -1:
definedModules.append(className)
return definedModules
def saveHistory(self):
self.__history.save()
def cmdloop(self):
intro = self.INTRO
while(True):
try:
Cmd.cmdloop(self, intro)
break
except KeyboardInterrupt:
self.stdout.write("\n")
break
except Exception:
import traceback
traceback.print_exc()
self.saveHistory()
def do_quit(self, arg):
return True
def do_EOF(self, arg):
return True
def print_possible_load_list(self):
print "Generators:"
self.print_generators()
print "Senders:"
self.print_senders()
def print_generators(self):
for module in self.__listPacketGenerators:
print "\t" + module
def print_senders(self):
for module in self.__listPacketSenders:
print "\t" + module
def print_env(self, arg):
for var in self.__context.keys():
print self.BRYELLOW + var + self.ENDC + "=" + repr(self.__context[var])
def do_export(self, arg):
if '' == arg:
return self.print_env(arg)
eqPos = arg.find("=")
if( -1 == eqPos):
print "Syntax error. Please use as set var=value"
else:
self.__context[arg[0:eqPos].strip()] = self.parseVariableValue(arg[eqPos+1:])
def do_send(self, arg):
if '' == arg:
arg = "1"
packet = self.__packetGenerator.generatePacket();
for x in range( 0, int(arg)):
self.__packetSender.sendPacket(packet)
print "Sent[" + str(x) + "]:" + repr(packet);
def do_load(self, arg):
return self.exceptSafeInvoker(partial(self.load_impl,arg))
def removeItems(self, dict, keys ):
for key in keys:
dict.pop(key)
def load_impl(self, arg):
if arg == '':
self.print_possible_load_list()
return
if( str(arg) in self.__listPacketGenerators):
self.removeItems(self.__context, self.__packet_context_difference)
contextCopy = self.__context.copy()
self.loadGenerator(arg)
self.__packet_context_difference = set(self.__context.keys()) - set(contextCopy.keys())
return
if( arg in self.__listPacketSenders):
self.removeItems(self.__context, self.__sender_context_difference)
contextCopy = self.__context.copy()
self.loadSender(arg)
self.__sender_context_difference = set(self.__context.keys()) - set(contextCopy.keys())
return
print arg + " is not loaded, no such module "
def loadGenerator(self, arg):
fp = None
try:
fp, pathname, description = imp.find_module(arg)
module = imp.load_module(arg, fp, pathname, description)
packetGenerator = getattr(module, arg)
self.__packetGenerator = packetGenerator(self.__context)
print arg + " loaded"
except:
import traceback
traceback.print_exc()
print arg + " is not loaded, no such module "
if fp:
fp.close()
def loadSender(self, arg):
fp = None
try:
fp, pathname, description = imp.find_module(arg)
module = imp.load_module(arg, fp, pathname, description)
packetSender = getattr(module, arg)
self.__packetSender = packetSender(self.__context)
print arg + " loaded "
except:
print arg + " is not loaded, no such module "
if fp:
fp.close()
def do_help(self, arg):
print "\t load [sender,generator] - load available packet sender or available packet type generator"
print "\t quit - exit application"
print "\t send [number_of_packets_to_send] - generate and send packet"
print "\t export [variable = value | {lambda} | {$variable} ] - setting value to a context variable"
print "\t clobber - reset everything, besides history"
print "\t history [[script] | [last <number>] | [clear]]"
def do_clobber(self, arg):
return self.exceptSafeInvoker(partial(self.clobber_impl,arg))
def clobber_impl(self, arg):
self.__packetGenerator = None
self.__packetSender = None
self.__context = {}
def do_history(self, arg):
return self.exceptSafeInvoker(partial(self.history_impl,arg))
def printHistory(self, history):
for item in history:
print item
def history_impl(self, arg):
if arg.startswith("clear"):
self.__history.clear();
return
if '' == arg:
self.printHistory(self.__history.get(self.__history.passAllPredicate))
return
if arg.startswith('script'):
self.printHistory(self.__history.get(self.__history.passScript))
return
if arg.startswith('last'):
linesNumber = int(arg.replace('last', '').strip(" "))
self.printHistory(self.__history.getLastN_lines(linesNumber))
return
print "history syntax error, please type help for more information"
def completedefault_impl(self, predicate, all_targets, *ignored):
if predicate(ignored[1]):
if ignored[0] == '':
return all_targets
else:
matched = []
for target in all_targets:
if target.lower().startswith(ignored[0].lower()):
matched.append(target)
return matched
return None
def load_cmd_predicate(self, cmd):
if cmd.startswith('load'):
return True
return False
def export_cmd_predicate(self, cmd):
isVariableName_LHS = (cmd.startswith('export') and cmd.find('=') == -1)
rhsVariablePattern = re.compile(r'=\{\$')
if isVariableName_LHS or (re.search(rhsVariablePattern, cmd ) is not None):
return True
return False
def history_cmd_predicate(self, cmd):
return cmd.startswith("history")
def completedefault(self, *ignored):
completation_variants =\
self.completedefault_impl( self.load_cmd_predicate,
self.__listPacketGenerators + self.__listPacketSenders,
*ignored )
if completation_variants is not None:
return completation_variants
completation_variants =\
self.completedefault_impl( self.export_cmd_predicate,
self.__context.keys(),
*ignored )
if completation_variants is not None:
return completation_variants
completation_variants =\
self.completedefault_impl( self.history_cmd_predicate,
['last', 'script', 'clear'],
*ignored )
if completation_variants is not None:
return completation_variants
return []
def exceptSafeInvoker(self, functor):
try:
return functor()
except IOError as e:
print "I/O error({0}): {1}".format(e.errno, e.strerror)
except ValueError:
print "Could not convert data to an integer."
except KeyboardInterrupt:
return True
except:
import traceback
traceback.print_exc()
return None
def parseVariableValue(self, value):
return self.__valueParser.parseVariableValue(value)
class VideoReader:
# max len of buffer # TODO : add argument
bufferMax = 25
# step by step mode # TODO : add argument
stepByStep = True
# where to insert legend in output image (to be overloaded by sub classes)
legendLine = 1
font = cv2.FONT_HERSHEY_SIMPLEX
def getNextFrame(self):
ret, frame = self.cap.read()
if not ret:
return None
return { "input": frame }
def showInfo(self):
print "Frame {0}".format(self.history.index())
# return (hours, minutes, seconds, hundredth) from frame number and frame rate
def frameToTimestamp(self, frameNum):
c = frameNum * 100 / self.frameRate
return self.toTimestamp(c)
# return (hours, minutes, seconds, hundredth) from start time and "now" time
def getTimestamp(self, start, now):
c = int((now - start)*100)
return self.toTimestamp(c)
def toTimestamp(self, c):
s = int(math.floor(c / 100))
c = c % 100
m = int(math.floor(s / 60))
s = s % 60
h = int(math.floor(m / 60))
m = m % 60
return (h, m, s, c)
def __init__(self,
_input, # input capture
_maxBuffer = 25, # fifo max size
_firstFrameCallback = None, # callback for first read frame
_inputFrameCallback = None, # callback for next frames before changing anything
_outputFrameCallback = None, # callback for next frames just before displaying it
_infoCallback = None, # callback to get informations to display in legend
_detailsCallback = None, # callback to get detailled informations to display when 'i' pressed
_popCallback = None, # callback when a frame is popped from history
_stepByStep = True, # play/pause mode at startup
_showInput = True, # show input in a window
_showOutput = True, # show output in a window
_skip = 0, # frames to skip at startup
_frameRate = None # give framerate
):
self.inputFile = _input
self.bufferMax = _maxBuffer
self.inputFrameCallback = _inputFrameCallback
self.outputFrameCallback= _outputFrameCallback
self.infoCallback = _infoCallback
self.detailsCallback = _detailsCallback
self.popCallback = _popCallback
self.stepByStep = _stepByStep
self.showInput = _showInput
if (self.showInput):
cv2.namedWindow("raw")
self.showOutput = _showOutput
if (self.showOutput):
cv2.namedWindow("output")
self.cap = cv2.VideoCapture(self.inputFile)
# skip first frames if asked for
if _skip != 0 :
print "Skipping", _skip, "frames ..."
for i in range( 1 , _skip ) :
if not self.cap.grab():
exit(1)
# read first frame, deduce size
ret, frame = self.cap.read()
if not ret:
exit(1)
self.history = History(_next = self.getNextFrame, _first = { "input": frame },
_maxSize = _maxBuffer, _initialIndex = _skip,
_removeCallback = self.innerPopCallback)
step = self.history.get()
self.width = step['input'].shape[1]
self.height = step['input'].shape[0]
self.nbPixel = self.width * self.height
if type(self.inputFile) is int:
self.frameRate = None
self.start = time()
if _frameRate is None:
raise Exception("capture from cam must specify framerate")
elif _frameRate == 'auto':
# read some frame to evaluate framerate
for f in range(1,50):
ret, frame = self.cap.read()
if not ret:
print "End of capture during framerate calculation ..."
return None
self.frameRate = 50 / (time() - self.start)
else:
self.frameRate = _frameRate
print "force cam to frame rate", _frameRate
if not self.cap.set(cv2.cv.CV_CAP_PROP_FPS, _frameRate):
print "Failed to force framerate"
# TODO : look a v4l compilation options to enable fps set ?
# self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 320)
# self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 200)
else:
self.frameRate = self.cap.get(cv2.cv.CV_CAP_PROP_FPS)
if self.frameRate <= 0:
if _frameRate is None:
print "Can't get frame rate ({0}), defaults to 25".format(self.frameRate)
self.frameRate = 25
else:
self.frameRate = _frameRate
self.fcc = self.cap.get(cv2.cv.CV_CAP_PROP_FOURCC)
if self.fcc <= 0:
print "Can't get fcc, defaults to XVID"
self.fcc = cv2.cv.CV_FOURCC(*'XVID')
if self.showInput:
cv2.imshow("raw", step['input'])
frame = step['input'].copy()
if _firstFrameCallback is not None:
_firstFrameCallback(self, frame)
step['output'] = frame
legend = "input '{3}' {0}x{1}@{2}".format(self.width, self.height, self.frameRate, self.inputFile)
print legend
cv2.putText(step['output'], legend, (5, self.legendLine*20), self.font, 0.5, (255,255,255))
if self.showOutput:
cv2.imshow("output", step['output'])
def run(self):
try:
if not self.showOutput and not self.showInput:
import keys
self.keys = keys.single_keypress()
# read frames
while(True):
isNew = False
if self.showOutput or self.showInput:
# force redraw + handle keys
if self.stepByStep:
kc = translateKey(cv2.waitKey(0))
else:
kc = translateKey(cv2.waitKey(1))
else:
# no window -> use stdin
if self.stepByStep:
kc = self.keys.read(True)
else:
kc = self.keys.read()
if kc == 'i':
print "Frame {0}".format(self.history.index())
if self.detailsCallback is not None:
print " ", self.detailsCallback(self, index, step)
continue
elif kc == 'q':
break
elif kc == 's' or kc == 'p':
self.stepByStep = not self.stepByStep
continue
# left key => go backward
elif kc == 'LEFT':
step = self.history.backward()
if step is None:
continue
else:
step, isNew = self.history.forward()
if step is None:
break;
if self.showInput:
cv2.imshow("raw", step['input'])
if isNew:
step['output'] = step['input'].copy()
self.frameNumber = self.history.index()
if self.inputFrameCallback is not None:
self.inputFrameCallback(self, self.frameNumber, step)
if type(self.inputFile) is int:
legend = "{0:02}:{1:02}:{2:02}.{3:02}".format(*self.getTimestamp(self.start, time()))
else:
legend = "{0:02}:{1:02}:{2:02}.{3:02}".format(*self.frameToTimestamp(self.frameNumber))
if self.infoCallback is not None:
legend = legend + ": " + self.infoCallback(self, self.frameNumber, step)
cv2.putText(step['output'], legend, (5, self.legendLine*20), self.font, 0.5, (255,255,255))
if self.outputFrameCallback is not None:
self.outputFrameCallback(self, self.frameNumber, step)
if self.showOutput:
cv2.imshow("output", step['output'])
# We're at end of video input => force output of last frames in history
if len(self.history.buffer) != 0 and self.popCallback is not None:
print "popping queue {1} frames from {0}".format(self.history.iter0, len(self.history.buffer))
while self.history.pop() is not None:
# pop() will call its callback -> nothing to do in loop
pass
finally:
if not self.showOutput and not self.showInput:
self.keys.stop()
def innerPopCallback(self, stepNumber, step):
if self.popCallback is not None:
self.popCallback(self, stepNumber, step)
class VideoReader:
# max len of buffer # TODO : add argument
bufferMax = 25
# step by step mode # TODO : add argument
stepByStep = True
# where to insert legend in output image (to be overloaded by sub classes)
legendLine = 1
font = cv2.FONT_HERSHEY_SIMPLEX
def getNextFrame(self):
if self.softwareFrameRate:
# have to wait to next frame tick
shouldBeNum = self.frameNumber + 1
shouldBeAt = float(shouldBeNum) / self.frameRate + self.start
now = time()
if now > shouldBeAt:
lost = int(math.floor((now - shouldBeAt) * self.frameRate));
shouldBeNum += lost + 1
shouldBeAt = shouldBeNum / self.frameRate + self.start
else:
lost = 0
if shouldBeAt != now:
sleep(shouldBeAt - now)
ret, frame = self.cap.read()
if self.softwareFrameRate and lost > 0:
print shouldBeNum, ": skipped", lost, "frames (", now, "/", shouldBeAt, ")"
if not ret:
return None
return { "input": frame, "frameNumber": shouldBeNum }
def showInfo(self):
print "Frame {0}".format(self.history.index())
# return (hours, minutes, seconds, hundredth) from frame number and frame rate
def frameToTimestamp(self, frameNum):
c = frameNum * 100 / self.frameRate
return self.toTimestamp(c)
# return (hours, minutes, seconds, hundredth) from start time and "now" time
def getTimestamp(self, start, now):
c = int((now - start)*100)
return self.toTimestamp(c)
def toTimestamp(self, c):
s = int(math.floor(c / 100))
c = c % 100
m = int(math.floor(s / 60))
s = s % 60
h = int(math.floor(m / 60))
m = m % 60
return (h, m, s, c)
def __init__(self,
_input, # input capture
_maxBuffer = 25, # fifo max size
_firstFrameCallback = None, # callback for first read frame
_inputFrameCallback = None, # callback for next frames before changing anything
_outputFrameCallback = None, # callback for next frames just before displaying it
_infoCallback = None, # callback to get informations to display in legend
_detailsCallback = None, # callback to get detailled informations to display when 'i' pressed
_popCallback = None, # callback when a frame is popped from history
_endCallback = None, # callback before exiting run()
_stepByStep = True, # play/pause mode at startup
_showInput = True, # show input in a window
_showOutput = True, # show output in a window
_skip = 0, # frames to skip at startup
_frameRate = None # give framerate
):
self.inputFile = _input
self.bufferMax = _maxBuffer
self.inputFrameCallback = _inputFrameCallback
self.outputFrameCallback= _outputFrameCallback
self.infoCallback = _infoCallback
self.detailsCallback = _detailsCallback
self.popCallback = _popCallback
self.endCallback = _endCallback
self.stepByStep = _stepByStep
self.showInput = _showInput
if (self.showInput):
cv2.namedWindow("raw")
self.showOutput = _showOutput
if (self.showOutput):
cv2.namedWindow("output")
self.cap = cv2.VideoCapture(self.inputFile)
# skip first frames if asked for
if _skip != 0 :
print "Skipping", _skip, "frames ..."
for i in range( 1 , _skip ) :
if not self.cap.grab():
exit(1)
# read first frame, deduce size
ret, frame = self.cap.read()
if not ret:
exit(1)
self.frameNumber = 0
self.history = History(_next = self.getNextFrame, _first = { "input": frame, "frameNumber": 0 },
_maxSize = _maxBuffer, _initialIndex = _skip,
_removeCallback = self.innerPopCallback)
step = self.history.get()
self.width = step['input'].shape[1]
self.height = step['input'].shape[0]
self.nbPixel = self.width * self.height
if type(self.inputFile) is int:
self.frameRate = None
self.start = time()
if _frameRate is None:
raise Exception("capture from cam must specify framerate")
elif _frameRate == 'auto':
# read some frame to evaluate framerate
for f in range(1,50):
ret, frame = self.cap.read()
if not ret:
print "End of capture during framerate calculation ..."
return None
# force to 10% less, and rond to integer
self.frameRate = int( 50 / (time() - self.start) * 0.9 )
print "cam frame rate evaluated to", self.frameRate
else:
self.frameRate = _frameRate
print "Force cam to frame rate", self.frameRate
if not self.cap.set(cv2.cv.CV_CAP_PROP_FPS, self.frameRate):
print "Failed to force cam to framerate, will try by software"
self.softwareFrameRate = True
else:
self.softwareFrameRate = False
# TODO : look a v4l compilation options to enable fps set ?
# self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 320)
# self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 200)
else:
self.frameRate = self.cap.get(cv2.cv.CV_CAP_PROP_FPS)
if self.frameRate <= 0:
if _frameRate is None:
print "Can't get frame rate ({0}), defaults to 25".format(self.frameRate)
self.frameRate = 25
else:
self.frameRate = _frameRate
self.fcc = self.cap.get(cv2.cv.CV_CAP_PROP_FOURCC)
if self.fcc <= 0:
print "Can't get fcc, defaults to XVID"
self.fcc = cv2.cv.CV_FOURCC(*'XVID')
if self.showInput:
cv2.imshow("raw", step['input'])
frame = step['input'].copy()
if _firstFrameCallback is not None:
_firstFrameCallback(self, frame)
step['output'] = frame
legend = "input '{3}' {0}x{1}@{2}".format(self.width, self.height, self.frameRate, self.inputFile)
print legend
cv2.putText(step['output'], legend, (5, self.legendLine*20), self.font, 0.5, (255,255,255))
if self.showOutput:
cv2.imshow("output", step['output'])
def run(self):
try:
if not self.showOutput and not self.showInput:
import keys
self.keys = keys.single_keypress()
# read frames
while(True):
isNew = False
if self.showOutput or self.showInput:
# force redraw + handle keys
if self.stepByStep:
kc = translateKey(cv2.waitKey(0))
else:
kc = translateKey(cv2.waitKey(1))
else:
# no window -> use stdin
if self.stepByStep:
kc = self.keys.read(True)
else:
kc = self.keys.read()
if kc == 'i':
print "Frame {0}".format(self.history.index())
if self.detailsCallback is not None:
print " ", self.detailsCallback(self, index, step)
continue
elif kc == 'q':
break
elif kc == 's' or kc == 'p':
self.stepByStep = not self.stepByStep
continue
# left key => go backward
elif kc == 'LEFT':
step = self.history.backward()
if step is None:
continue
else:
step, isNew = self.history.forward()
if step is None:
break;
if self.showInput:
cv2.imshow("raw", step['input'])
if isNew:
step['output'] = step['input'].copy()
self.frameNumber = step['frameNumber']
if self.inputFrameCallback is not None:
self.inputFrameCallback(self, self.frameNumber, step)
if type(self.inputFile) is int:
legend = "{0:02}:{1:02}:{2:02}.{3:02}".format(*self.getTimestamp(self.start, time()))
else:
legend = "{0:02}:{1:02}:{2:02}.{3:02}".format(*self.frameToTimestamp(self.frameNumber))
if self.infoCallback is not None:
legend = legend + ": " + self.infoCallback(self, self.frameNumber, step)
cv2.putText(step['output'], legend, (5, self.legendLine*20), self.font, 0.5, (255,255,255))
if self.outputFrameCallback is not None:
self.outputFrameCallback(self, self.frameNumber, step)
if self.showOutput:
cv2.imshow("output", step['output'])
# We're at end of video input => force output of last frames in history
if len(self.history.buffer) != 0 and self.popCallback is not None:
print "popping queue {1} frames from {0}".format(self.history.iter0, len(self.history.buffer))
while self.history.pop() is not None:
# pop() will call its callback -> nothing to do in loop
pass
finally:
if self.endCallback is not None:
self.endCallback(self, self.frameNumber)
if not self.showOutput and not self.showInput:
self.keys.stop()
def innerPopCallback(self, stepNumber, step):
if self.popCallback is not None:
self.popCallback(self, stepNumber, step)
# modififiedDate
# name
# path
# type
# size
baseList = []
baseB = []
baseFormat = []
for i in range(0, 6):
baseList.append({
"id":
i,
"name":
"FILdE_" + str(i),
"path":
"PlayToAir",
"type":
"cmf",
"size":
2923654568212,
"modififiedDate":
datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S")
})
historial.set(baseList)
historial.get()
historial.printStatus()
historial.save()
class MapTest:
use_interpolation = True
car_point_radius = 14
car_point_range = range(-car_point_radius, car_point_radius + 1)
car_heading_point_radius = int(car_point_radius / 3)
car_heading_point_range = range(-car_heading_point_radius, car_heading_point_radius + 1)
def __init__(self, width=800, height=800):
"""
Constructor
"""
CurrentData.register_method_as_observer(self.on_data_change)
if width > 0:
self.width = width # x_max
if height > 0:
self.height = height # y_max
self.grid = np.zeros((self.width, self.height), np.uint8)
self.lidar_counter = 0
self.ppm_header = bytes('P5 {} {} 255 '.format(self.width, self.height), 'ascii')
self.ppm_array = np.zeros((self.width, self.height), np.uint8)
self.pos_euler_history = History(10)
self.last_lidar_set = []
self.last_lidar_set_ts = 0
self.wait_for_next_sensor_data = False
self.euler_offset = 0
self.gui = None
def gui_init(self, gui):
"""initializes GUI"""
self.gui = gui
def get_global_coords_from_lidar_scan(self, car_pos, car_heading, lidar_scan_angle, lidar_scan_dist):
"""
Calculates the global coordinates of a detected lidar point using car position, rotation and
the angle and distance of the lidar point
"""
global_angle_rad = math.radians(car_heading + lidar_scan_angle + self.euler_offset)
global_x = int(round(lidar_scan_dist/10 * math.sin(global_angle_rad) + car_pos[0]/10))
global_y = int(round(-lidar_scan_dist/10 * math.cos(global_angle_rad) + car_pos[1]/10))
return global_x, global_y
def on_data_change(self, changed_data_str):
"""
Is registered as an observer method of CurrentDatas __on_data_change method (see _init_). Is called whenever
CurrentData receives new sensor/lidar data, receiving a data string "lidar"/"sensor" corresponding to the type
of data received. Determines what is to be done when such data is received, i.e. adding lidar data to
map/adding sensor data to history.
"""
if changed_data_str == "lidar":
self.last_lidar_set = CurrentData.get_value_from_tag_from_lidar("pcl")
self.last_lidar_set_ts = CurrentData.get_value_from_tag_from_lidar("timestamp")
if MapTest.use_interpolation:
self.wait_for_next_sensor_data = True
else:
self.add_last_lidar_set_to_map()
elif changed_data_str == "sensor":
pos = CurrentData.get_value_from_tag_from_sensor("position")[:-1]
euler = 360 - CurrentData.get_value_from_tag_from_sensor("euler")[0]
timestamp = CurrentData.get_value_from_tag_from_sensor("timestamp")
self.pos_euler_history.append([timestamp, pos, euler])
if self.wait_for_next_sensor_data and MapTest.use_interpolation:
self.add_last_lidar_set_to_map()
self.wait_for_next_sensor_data = False
def calculate_euler_offset(self):
"""
Calculates offset for the car rotation to arrive at the cars "global orientation".
To be used when car is in "default position"
"""
self.euler_offset = CurrentData.get_value_from_tag_from_sensor("euler")[0] - 90
def get_interpolated_pos_and_euler(self, timestamp):
"""
returns the approximated car position and rotation for a specified timestamp using linear interpolation.
Only used if use_interpolation == True. (see self.add_last_lidar_set_to_map)
"""
pos_euler_history_len = len(self.pos_euler_history)
if pos_euler_history_len > 0:
first_entry = self.pos_euler_history.get(0)
last_entry = self.pos_euler_history.get(pos_euler_history_len - 1)
if first_entry[0] <= timestamp <= last_entry[0]:
for i in range(pos_euler_history_len):
cur = self.pos_euler_history.get(i)
if timestamp < cur[0]:
if 0 < i <= pos_euler_history_len-1:
prev = self.pos_euler_history.get(i-1)
lin_interpol_factor = (timestamp-prev[0]) / (cur[0]-prev[0])
interpol_pos = []
for j, val in enumerate(prev[1]):
interpol_pos.append(val + lin_interpol_factor * (cur[1][j]-val))
interpol_euler = prev[2]
normal_dist = cur[2] - prev[2]
distance_over_360 = min(360 - prev[2] + cur[2], 360 - cur[2] + prev[2])
if distance_over_360 < normal_dist:
if prev[2] < cur[2]:
distance_over_360 = -distance_over_360
euler_offset = lin_interpol_factor * distance_over_360
else:
euler_offset = lin_interpol_factor * normal_dist
interpol_euler = prev[2] + euler_offset
if interpol_euler < 0:
interpol_euler = 360 + interpol_euler
elif interpol_euler > 360:
interpol_euler = interpol_euler % 360
return interpol_pos, interpol_euler
elif i == 0:
return cur[1], cur[2]
else:
if i == pos_euler_history_len:
return cur[1], cur[2]
elif timestamp < first_entry[0]:
return first_entry[1], first_entry[2]
else:
return last_entry[1], last_entry[2]
def add_last_lidar_set_to_map(self):
"""adds the last collected lidar set (after converting it into global coordinates) to the map."""
self.lidar_counter += 1
for i, scan in enumerate(self.last_lidar_set):
if 0 <= scan[1] <= 120 or 240 <= scan[1] <= 360:
if MapTest.use_interpolation:
current_ts = self.last_lidar_set_ts - 100 + int(round((i / len(self.last_lidar_set) * 100)))
car_pos, car_heading = self.get_interpolated_pos_and_euler(current_ts)
else:
last_history_element = self.pos_euler_history.get(len(self.pos_euler_history)-1)
car_pos = last_history_element[1]
car_heading = last_history_element[2]
global_x, global_y = MapTest.get_global_coords_from_lidar_scan(self, car_pos, car_heading, scan[1], scan[2])
self.add_point_to_map(global_x, global_y)
if self.lidar_counter >= 30:
self.lidar_counter = 0
self.reset_poor_map_data()
def add_point_to_map(self, x, y):
"""adds a point (described by x and y coordinates) to the map by increasing the integer values of the
corresponding element in the grid as well as its immediate surroundings"""
# print("{} {}".format(x, y))
for i in range(-2, 3):
for j in range(-2, 3):
if 0 <= x + i < self.width and 0 <= y + j < self.height:
dist = math.hypot(i, j)
if dist >= 2:
self.grid[x + i][y + j] += 1
elif 0 < dist < 2:
self.grid[x + i][y + j] += 2
else:
self.grid[x + i][y + j] += 3
def reset_map(self):
"""Resets the map to a blank (np.zeros) grid.
To be used when euler/rotation is reset or when map data is faulty."""
self.grid = np.zeros((self.width, self.height), np.uint8)
def reset_poor_map_data(self):
"""removes points from map which don't pass a minimum threshold of times measured"""
self.grid[self.grid < 2] = 0
def _get_map_as_ppm(self):
"""Generates a 2D-grid representing a visually intuitive 8-bit rendition of the map (background = white,
obstacles = black).
Adds car position and rotation to the new grid. Used in self.get_map_as_photo_img"""
self.ppm_array = np.copy(self.grid)
car_pos = CurrentData.get_value_from_tag_from_sensor("position")
car_heading = CurrentData.get_value_from_tag_from_sensor("euler")
self.ppm_array[self.ppm_array < 1] = 255
self.ppm_array[self.ppm_array < 255] = 0
if car_pos is not None and car_heading is not None:
car_x = int(round(car_pos[0]/10))
car_y = int(round(car_pos[1]/10))
car_heading[0] = 360 - car_heading[0]
for i in MapTest.car_point_range:
for j in MapTest.car_point_range:
if 0 <= car_x + i < self.width and 0 <= car_y + j < self.height:
dist = math.hypot(i, j)
if dist <= MapTest.car_point_radius:
grey_val = 50 + (dist / MapTest.car_point_radius) * 50
self.ppm_array[car_x + i][car_y + j] = grey_val
heading_x = int(car_x + MapTest.car_point_radius * math.sin(math.radians(self.euler_offset + car_heading[0])))
heading_y = int(car_y + MapTest.car_point_radius * -math.cos(math.radians(self.euler_offset + car_heading[0])))
for i in MapTest.car_heading_point_range:
for j in MapTest.car_heading_point_range:
if 0 <= heading_x + i < self.width and 0 <= heading_y + j < self.height:
dist = math.hypot(i, j)
if dist <= MapTest.car_heading_point_radius:
self.ppm_array[heading_x + i][heading_y + j] = 255
self.ppm_array = np.fliplr(self.ppm_array)
return self.ppm_header + b' ' + self.ppm_array.tobytes()
def get_map_as_photo_img(self):
"""generates an image representing the map and returns the image"""
img = PhotoImage(width=self.width, height=self.height, data=self._get_map_as_ppm(), format='PPM')
return img
