def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.plt = Plot()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
self.cars.append(Car(x=100, y=100, theta=-45,
speed=0)) # representation for actual car
self.cars.append(Car(x=100, y=100, theta=-45 + 15,
speed=0)) # representation for ins estimate
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization single pass
self.cars.append(
Car(x=100, y=100, theta=-45, speed=0)
) # representation for ins estimate xy optimization multi pass
self.cars.append(
Car(x=100, y=100, theta=-45,
speed=0)) # representation for ins estimate theta optimization
for car in self.cars:
car.color = Draw.WHITE
self.cars[2].color = Draw.YELLOW
self.cars[3].color = Draw.RED
self.cars[4].color = Draw.GREEN
self.action = None
self.dragdrop = DragAndDropController(self.events)
self.controller = self.dragdrop
# self.cars[0].camview = CamView(self.cars[0],self.background.arr)
# self.cars[0].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "estimate"
self.cars[2].name = "opt"
self.cars[3].name = "opt*"
self.cars[4].name = "theta"
self.cars[0].controller = self.controller
self.cars[1].controller = self.controller
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[2].controller = OptimizeNearestEdgeXYSinglePass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[3].controller = OptimizeNearestEdgeXYMultiPass(
optimize=self.optimize,
labels=self.labels,
label_positions=self.label_positions,
camview=self.cars[0].camview,
estimate_car=self.cars[1])
self.cars[4].controller = OptimizeThetaParable(
optimize=self.optimize,
distances=self.distances,
camview=self.cars[0].camview,
estimate_car=self.cars[3])
# self.cars[4].controller = OptimizeNearestEdgeTheta(
# optimize = self.optimize,
# labels = self.labels,
# label_positions = self.label_positions,
# hilbert = self.hilbert,
# camview = self.cars[0].camview,
# estimate_car = self.cars[1])
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.done = False
self.cursor = (0, 0)
self.register_events()
self.spin()
def __init__(self):
super(App, self).__init__()
# load background
self.background = Background(filename="background.png")
# get array copy of background image
self.background.arr = pygame.surfarray.array3d(self.background.img)
# create bw from image
_, self.background.arr_bw = cv2.threshold(self.background.arr[:, :, 0],
128, 1, cv2.THRESH_BINARY)
# print self.background.arr_bw.shape, self.background.arr_bw.dtype
# self.background.arr_dist = cv2.distanceTransform(self.background.arr_bw, cv.CV_DIST_L1, 3)
# get nearest (zero) pixel labels with corresponding distances
self.background.arr_dist, self.labels = cv2.distanceTransformWithLabels(
self.background.arr_bw,
cv.CV_DIST_L1,
3,
labelType=cv2.DIST_LABEL_PIXEL)
self.distances = self.background.arr_dist
### get x,y coordinates for each label
# get positions of zero points
zero_points = Utils.zero_points(self.background.arr_bw)
# get labels for zero points
zero_labels = self.labels[zero_points[:, 0], zero_points[:, 1]]
# create dictionary mapping labels to zero point positions
self.label_positions = dict(
zip(zero_labels, zip(zero_points[:, 0], zero_points[:, 1])))
# create hilbert curve lookup table
self.hilbert = Hilbert.hilbert_lookup(*self.background.arr.shape[:2])
# provide a rgb variant of dist for display
self.background.arr_dist_rgb = self.background.arr.copy()
self.background.arr_dist_rgb[:, :, 0] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 1] = self.background.arr_dist
self.background.arr_dist_rgb[:, :, 2] = self.background.arr_dist
# print a.shape
self.setup_pygame()
self.events = Events()
self.lane = Lane(self.events)
self.lane.load("parkour.sv")
# self.lane.add_support_point(100,100)
# self.lane.add_support_point(200,100)
# self.lane.add_support_point(200,200)
# self.lane.add_support_point(100,200)
self.optimize = Optimize(self.lane)
self.cars = []
# for k in range(1):
# self.cars.append(Car(x=150+k*5,y=100,theta=np.random.randint(0,360),speed=np.random.randint(45,180)))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 1.5 * 90))
self.cars.append(Car(x=50, y=250, theta=90, speed=1 * 90)) # [1] human
self.cars.append(Car(x=50, y=250, theta=90,
speed=1 * 90)) # [2] ghost of ins estimating [0]
self.action = None
self.human = HumanController()
self.heuristic = Heuristic(self.lane)
Node.heuristic = self.heuristic
self.onestep = OneStepLookaheadController(self.cars, self.lane,
self.heuristic)
self.nstep = NStepLookaheadController(self.cars, self.lane,
self.heuristic, 2)
self.bestfirst = BestFirstController(self.cars, self.lane,
self.heuristic)
self.controller = self.bestfirst
self.cars[0].camview = CamView(self.cars[0], self.background.arr)
self.cars[0].camview.register_events(self.events)
self.cars[0].controller = self.controller
self.cars[0].collision = False
self.cars[0].imu = IMU(self.cars[0])
self.cars[0].ins = INS(self.cars[0].imu.calibration_noise)
self.cars[0].ins.update_pose(self.cars[0].x,
self.cars[0].y,
self.cars[0].theta / Utils.d2r,
gain=1)
self.insghost = INSGhostController(self.cars[0].ins)
self.cars[1].controller = self.human
self.cars[2].controller = self.insghost
self.cars[2].collision = False
self.cars[2].size *= 1.25
self.cars[2].camview = CamView(self.cars[2],
self.background.arr_dist_rgb,
width=275,
height=275,
offset=(0, 75),
angle_offset=-25)
self.cars[2].camview.register_events(self.events)
self.cars[0].name = "actual"
self.cars[1].name = "human"
self.cars[2].name = "estimate"
# this causes the controller of cars[0] to use the information from cars[0].ghost but act on cars[0]
# self.cars[0].ghost = self.cars[2]
# self.window = Window(self.screen, self.events, 300, 200, "caption")
self.grid = Grid(50, 50, *self.size)
self.last_distance_grid_update = time() - 10
self.update_distance_grid()
self.done = False
for car in self.cars:
# save original speed
if not hasattr(car, "speed_on"):
car.speed_on = car.speed
# toggle speed
car.speed = car.speed_on - car.speed
# car.pause = not car.pause
self.plot_window_size = 100
self.xyt_corr_ring_buffer = RingBuffer(self.plot_window_size,
channels=3)
self.xyt_corr_plot = RingBufferPlot(self.xyt_corr_ring_buffer)
# self.normal_test_p_value_plot = RingBufferPlot(RingBuffer(self.plot_window_size,channels=self.xyt_corr_ring_buffer.channels))
self.std_plot = RingBufferPlot(
RingBuffer(self.plot_window_size,
channels=self.xyt_corr_ring_buffer.channels))
self.velocity_carthesian_history_plot = RingBufferPlot(
self.cars[0].ins.velocity_carthesian_history)
# self.hist_plot = HistogramPlot(10)
self.register_events()
self.spin()
