def __init__(self, map_name='campus'): self.map = MapLoader.load(path='resources/campus.json') self.pt = PositionTracker(osmap=self.map) self.pf = PositionFetcher(self.pt) self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint) self.vis = Visualizer(map_name=map_name, pt=self.pt, add_wp=self.add_waypoint, osmap=self.map) self.waypoints_lock = Lock() self.waypoints_changed = True self.waypoints: Optional[List[Waypoint]] = None self.set_waypoints([])
def predict_position(self): if self.pt.prediction is None or self.pt.rotation_prediction is None: return v, s, _ = info() dt = PositionTracker.time() - self.pt.last_prediction # Predict rotation curr_r = self.pt.rotation_prediction dr = (s * dt * (v / CarInfo.max_v * 4)) curr_r = (curr_r + dr) % 360.0 # Predict position curr = self.pt.prediction curr_rad = math.radians(curr_r) ds = v * dt dx = ds * math.cos(curr_rad) dy = ds * math.sin(curr_rad) y = curr[1] + dy / 6_378_000 * 180 / math.pi x = curr[0] + (dx / 6_378_000 * 180 / math.pi) / math.cos( math.radians(curr[1])) self.pt.update_prediction((x, y), curr_r)
def _r_base(self): if self.printing: print("Constructing r-base:") r_base = [] special_cell_sizes = [] special_lookup = dict() height = 0 while height < self.size -1: if self.printing: print("\n{}".format(self.left[-1])) _,_,func = self.refinement_family.extend(self.left, None) r_base.append(func) if func is not None: if self.printing: print(func._info) special_cell_sizes.append(0) else: if self.printing: print("Special level:") cell, cell_size, point = self._split_left_cell() special_cell_sizes.append(cell_size) special_lookup[height] = (cell, point) height += 1 if self.printing: print("\n{}".format(self.left[-1])) self._cur_position = PositionTracker(special_cell_sizes) self._special_lookup = special_lookup if self.printing: print("\nFinished R-base construction.") _temp_levels = sorted([level for level in special_lookup]) print("Special levels: {}".format(_temp_levels)) special_cells = [special_lookup[level][0] for level in _temp_levels] special_vals = [special_lookup[level][1] for level in _temp_levels] print("Basis of group: {}".format(special_vals)) print("Ordering from rbase: {}".format(self.left.fix())) return r_base
def main(): exit_keys = (pygame.K_ESCAPE, ) background_colour = (255, 255, 255) width, height = (1400, 750) screen = pygame.display.set_mode((width, height)) pygame.font.init() font = pygame.font.SysFont('Helvetica', 16) pygame.display.set_caption('CarSim') screen.fill(background_colour) pygame.display.flip() running = True waypoints = [] car = Car(width/2, height/2) dt = 0 last = time.time_ns() pt = PositionTracker() pp = PathPlanner(car, pt) while running: for event in pygame.event.get(): if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key in exit_keys): running = False break if event.type == pygame.MOUSEBUTTONDOWN: x, y = pygame.mouse.get_pos() waypoints.append(Waypoint(x, y)) if event.type == pygame.KEYDOWN and event.key == pygame.K_r: if car.cruise_control_on: car.disable_cruise_control() else: car.enable_cruise_control(80) if event.type == pygame.KEYDOWN and event.key == pygame.K_q: if car.front_wheel.has_target and car.front_wheel.target_rotation < 0: car.unset_steer_target() else: car.steer_target(20, Direction.LEFT) if event.type == pygame.KEYDOWN and event.key == pygame.K_e: if car.front_wheel.has_target and car.front_wheel.target_rotation > 0: car.unset_steer_target() else: car.steer_target(20, Direction.RIGHT) pressed = pygame.key.get_pressed() if pressed[pygame.K_w]: car.accelerate() if pressed[pygame.K_a]: car.steer_left(dt) if pressed[pygame.K_d]: car.steer_right(dt) if pressed[pygame.K_s]: car.apply_brake(dt) if pressed[pygame.K_SPACE]: car.full_speed() car.update(dt) if car.velocity: pt.add(car.position) pp.plan(waypoints) screen.fill(background_colour) for wp in waypoints: pygame.draw.circle(screen, Waypoint.sec_color, (wp.x, wp.y), wp.tolerance) pygame.draw.circle(screen, Waypoint.color, (wp.x, wp.y), wp.radius) # pygame.draw.rect(screen, (255, 0, 0), car.rect) screen.blit(car.image, car.rect) # pygame.draw.line(screen, car.sensor.color, car.sensor.center, car.sensor.get_end(), 2) speed = font.render(f'v={round(car.velocity)} kmh', True, (0, 0, 0)) screen.blit(speed, (width-speed.get_width()-5, 5)) cc = font.render(f'CC {("off", "on")[car.cruise_control_on]}', True, (0, 0, 0)) screen.blit(cc, (width-cc.get_width()-5, 10 + speed.get_height())) wheel = font.render(f'Wheel: {round(car.front_wheel.rotation)}°', True, (0, 0, 0)) screen.blit(wheel, (width - wheel.get_width() - 5, 15 + speed.get_height()*2)) target = font.render(f'Target: {round(car.front_wheel.target_rotation)}°', True, (0, 0, 0)) screen.blit(target, (width - target.get_width() - 5, 20 + speed.get_height() * 3)) rot = font.render(f'Car rotation: {None if pt.rotation is None else round(pt.rotation * 100) / 100}°', True, (0, 0, 0)) screen.blit(rot, (width - rot.get_width() - 5, 25 + speed.get_height() * 4)) des = font.render(f'Desired rotation: {None if pp.desired_heading is None else round(pp.desired_heading * 100) / 100}°', True, (0, 0, 0)) screen.blit(des, (width - des.get_width() - 5, 30 + speed.get_height() * 5)) pygame.display.flip() current = time.time_ns() dt = (current - last) / 10 ** 9 last = current
class CarController: def __init__(self, map_name='campus'): self.map = MapLoader.load(path='resources/campus.json') self.pt = PositionTracker(osmap=self.map) self.pf = PositionFetcher(self.pt) self.planner = PathPlanner(self.pt, on_wp_reached=self.remove_waypoint) self.vis = Visualizer(map_name=map_name, pt=self.pt, add_wp=self.add_waypoint, osmap=self.map) self.waypoints_lock = Lock() self.waypoints_changed = True self.waypoints: Optional[List[Waypoint]] = None self.set_waypoints([]) def get_waypoints(self) -> List[Waypoint]: with self.waypoints_lock: return self.waypoints[:] def on_waypoint_change(self) -> None: self.waypoints_changed = True self.vis.set_waypoints(self.waypoints[:]) def remove_waypoint(self, waypoint_id: int) -> None: with self.waypoints_lock: self.waypoints = list( filter(lambda wp: wp.id != waypoint_id, self.waypoints)) self.on_waypoint_change() def update_position(self) -> None: if not self.pf.fetch(): self.planner.predict_position() def add_waypoint(self, waypoint: Waypoint) -> None: with self.waypoints_lock: orig_pos = Position(lat=waypoint.lat, lon=waypoint.lon) adjusted_position, path = self.map.closest_point(orig_pos) waypoint.path = path waypoint.lat = adjusted_position.lat waypoint.lon = adjusted_position.lon self.waypoints.append(waypoint) self.on_waypoint_change() def set_waypoints(self, waypoints: List[Waypoint]) -> None: with self.waypoints_lock: self.waypoints = waypoints self.on_waypoint_change() def init_position(self): for i in range(1, 0, -1): self.update_position() self.vis.update() print(i) time.sleep(1.0) def follow_waypoints(self) -> None: self.set_waypoints([ # Waypoint(18.1622607, 49.8358360), # Waypoint(18.1620518, 49.8356722) # Waypoint(18.1622303, 49.8356781), # Waypoint(18.1624181, 49.8356175), # Waypoint(49.8355103, 18.1625603), # Waypoint(49.8354083, 18.1626969), # Waypoint(49.8353356, 18.1627667), # Waypoint(49.8351281, 18.1626489) ]) self.pt.disable_rotation() copy = None while True: self.update_position() self.vis.update() if self.waypoints_changed: with self.waypoints_lock: copy = self.waypoints[:] self.waypoints_changed = False self.planner.plan(copy) self.pt.enable_rotation() time.sleep(0.1) def follow_qr(self): while True: self.planner.plan_from_camera() self.update_position() time.sleep(0.2) self.vis.update()
elif event.type == pygame.KEYDOWN and event.key == pygame.K_a: self.draw_adjusted = not self.draw_adjusted elif event.type == pygame.KEYDOWN and event.key == pygame.K_s: self.draw_exact = not self.draw_exact elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1: x, y = pygame.mouse.get_pos() x, y = self.cart_to_coords(x, y) self.add_waypoint(Waypoint(x, y)) def update(self): self.update_from_pt() self.poll_events() if __name__ == '__main__': pt = PositionTracker(osmap=None) vis = Visualizer('garage', pt, add_wp=lambda wp: None) files = [ 'geodoma', 'geohrbitov', 'geolouky', 'geoolsina', 'geoolsinaauto', 'geoolsinazahradni', 'garage', 'waypoint1', 'waypoint2', 'waypointstart' ] files = [f'resources/{name}.csv' for name in files] begin_x = vis.map.left_top[0] begin_y = vis.map.left_top[1] w = vis.map.right_bottom[0] - vis.map.left_top[0] h = vis.map.left_top[1] - vis.map.right_bottom[1] _waypoints = [ Waypoint(begin_x + w * 0.7, begin_y - h * 0.6),
def initialise_search_tree(self): self._cur_height = 0 self._cur_position = PositionTracker(self._special_level_sizes)
class PartitionBacktrackWorkhorse(): def __init__(self, group_property, refinement_family, size): self.size = size self.refinement_family = refinement_family self.group_property = group_property self.left = PartitionStack([0]*size,[-1]*size) self.right = PartitionStack([0]*size,[-1]*size) self._cur_height = 0 self._cur_position = None self._special_lookup = None self._left_split_index = None self.printing = False self.multi_backtrack = True self.double_coset_check = False def find_partition_backtrack_subgroup(self): #Assume the refinement families are symmetric. generators = [] r_base = self._r_base() count = 0 leaf_count = 0 if self.printing: print("-----------------------------------") print("Traversing tree:") while self._cur_height > -1: count += 1 if self.printing: a = self._cur_position b = self.right[self._cur_height] c = self.left[self._cur_height] print("\n{}: {}\n{} -> {}".format(self._cur_height,a,c,b)) #Alternatice 1: premature rule out. if self.alt1(): if self.printing: print("Alternative 1: partition violation") #print("alt_1") self.backtrack(self._cur_height - 1) #Alternative 2: discrete check. elif self.right.discrete(): leaf_count += 1 #print("alt_2") perm = Permutation.read_partitions(self.left[self._cur_height], self.right[self._cur_height]) if self.double_coset_check and self.printing: G=PermGroup(generators) Dco = G*perm*G perm_key = ordering.ordering_to_perm_key(self.left.fix()) if Dco.mid_minimal(key = perm_key): print("Actually minimal in own double coset: {}".format(Dco._list_elements(Perm_key))) added = False if not perm.trivial() and self.group_property.check(perm): generators.append(perm) added = True #self.backtrack() if self.printing: print("Alternative 2 found permutation: {} ({})".format(perm, added)) if self.multi_backtrack and added: self.backtrack(self._cur_position.min_changed_index()) else: self.backtrack() #Alternative 3: not a special level elif r_base[self._cur_height] is not None: #print("alt_3") r_base[self._cur_height](None, self.right) if self.printing: print("Alternative 3 applying function: {}".format(r_base[self._cur_height]._info)) self._cur_height += 1 #Alternative 4: is a special level else: if self.printing: print("Alternative 4 special level") #print("alt_4") self.extend_right() if self.printing: print("\nFinished tree traversal.") print("Visited {} vertices ({} of which were terminal).".format(count, leaf_count)) return generators def _r_base(self): if self.printing: print("Constructing r-base:") r_base = [] special_cell_sizes = [] special_lookup = dict() height = 0 while height < self.size -1: if self.printing: print("\n{}".format(self.left[-1])) _,_,func = self.refinement_family.extend(self.left, None) r_base.append(func) if func is not None: if self.printing: print(func._info) special_cell_sizes.append(0) else: if self.printing: print("Special level:") cell, cell_size, point = self._split_left_cell() special_cell_sizes.append(cell_size) special_lookup[height] = (cell, point) height += 1 if self.printing: print("\n{}".format(self.left[-1])) self._cur_position = PositionTracker(special_cell_sizes) self._special_lookup = special_lookup if self.printing: print("\nFinished R-base construction.") _temp_levels = sorted([level for level in special_lookup]) print("Special levels: {}".format(_temp_levels)) special_cells = [special_lookup[level][0] for level in _temp_levels] special_vals = [special_lookup[level][1] for level in _temp_levels] print("Basis of group: {}".format(special_vals)) print("Ordering from rbase: {}".format(self.left.fix())) return r_base def _split_left_cell(self): #Overwrite this with a clever function that queries the refinements for #clues as to which cell and element to split. top = self.left[-1] if self._left_split_index is None or len(top[self._left_split_index]) < 2: _, self._left_split_index = min([(len(cell),index) for index, cell in enumerate(top) if len(cell) > 1]) cell = top[self._left_split_index] point = cell[0] cell_size = len(cell) #Turn checks off. self.left.extend(self._left_split_index, [point]) return self._left_split_index, cell_size, point def find_partition_backtrack_coset(self): #Assume the refinement families are LR-symmetric. r_base = self._r_base() count = 0 while self._cur_height > -1: count += 1 #a = self._cur_position #b = self.right[self._cur_height] #c = self.left[self._cur_height] #if self.size == 13: #print("\n{}:{}\n{} -> {}".format(self._cur_height,a,c,b)) #Alternatice 1: premature rule out. if self.alt1(): #print("alt_1") self.backtrack(self._cur_height - 1) #Alternative 2: discrete check. elif self.right.discrete(): #print("alt_2") perm = Permutation.read_partitions(self.left[self._cur_height], self.right[self._cur_height]) if self.group_property.check(perm): return perm #self.backtrack() self.backtrack(self._cur_position.min_changed_index()) #Alternative 3: not a special level elif r_base[self._cur_height] is not None: #print("alt_3") r_base[self._cur_height](None, self.right) self._cur_height += 1 #Alternative 4: is a special level else: #print("alt_4") self.extend_right() print(count) return generators def backtrack(self, new_height = None): if new_height is None: new_height = self._cur_height self._cur_height = self._cur_position.increment(new_height) if self._cur_height > -1: while len(self.right) > self._cur_height + 1: self.right.pop() self.extend_right() def alt1(self): #Size violation if len(self.right) != self._cur_height + 1: return True elif len(self.right[-1][-1]) != len(self.left[self._cur_height][-1]): return True #Contained group violation. #Minimal in double coset violation. #Property dependant violation. return False def extend_right(self): index = self._cur_height split_index, _ = self._special_lookup[index] cell = self.right[index][split_index] try: split_val = cell[self._cur_position[index]] except IndexError: for i in range(index + 1): a = self._cur_position b = self.right[i] c = self.left[i] print("\n{} -> {}\n{}".format(c,b,a)) #this is where it screws raise IndexError("DIFFERENT") self.right.extend(split_index, [split_val]) self._cur_height += 1
class LeonSearch(): def __init__(self, leon_modifiers, degree): self.degree = degree self.tree_modifiers = leon_modifiers self.left = None self.right = None self._left_split_index = None self._cur_height = None self._r_base = None self._special_level_sizes = None #Tree position tracker to store traversal information. self._cur_position = None #Dictionary with heights as keys and split index and point pairs as values. self._special_lookup = None def initialise_partition_stacks(self): size = self.degree self.left = PartitionStack([0]*size,[-1]*size) self.right = PartitionStack([0]*size,[-1]*size) def initialise_r_base(self): r_base = [] special_cell_sizes = [] special_lookup = dict() height = 0 while height < self.degree -1: funcs = self.tree_modifiers.extension_functions(self.left) if funcs is not None: left_func, right_func = funcs left_func(self.left) r_base.append(right_func) special_cell_sizes.append(0) else: r_base.append(None) cell, cell_size, point = self._split_left_cell() special_cell_sizes.append(cell_size) special_lookup[height] = (cell, point) height += 1 self._r_base = r_base self._special_level_sizes = special_cell_sizes self._special_lookup = special_lookup def initialise_search_tree(self): self._cur_height = 0 self._cur_position = PositionTracker(self._special_level_sizes) def subgroup(self): #Needs to be done in this order. self.initialise_partition_stacks() self.initialise_r_base() self.initialise_search_tree() gens = [] while self._cur_height > -1: #alt_1 rule out. backtrack_index = self.tree_modifiers.exclude_backtrack_index(self.left, self.right, self._cur_position, self._cur_height) if backtrack_index is not None: self.backtrack(backtrack_index) #alt_2 discrete check. elif self.right.discrete(): perm = Permutation.read_partitions(self.left[self._cur_height], self.right[self._cur_height]) if not perm.trivial() and self.tree_modifiers.property_check(perm): gens.append(perm) backtrack_index = self.tree_modifiers.leaf_pass_backtrack_index(self.left,self.right,self._cur_position) else: backtrack_index = self.tree_modifiers.leaf_fail_backtrack_index(self.left,self.right,self._cur_position) self.backtrack(backtrack_index) #alt_3 function to extend. elif self._r_base[self._cur_height] is not None: func = self._r_base[self._cur_height] func(self.right) self._cur_height += 1 #alt_4 special level. else: self._extend_right() return gens def backtrack(self, new_height = None): if new_height is None: new_height = self._cur_height self._cur_height = self._cur_position.increment(new_height) if self._cur_height > -1: while len(self.right) > self._cur_height + 1: self.right.pop() self._extend_right() def _extend_right(self): index = self._cur_height split_index, _ = self._special_lookup[index] cell = self.right[index][split_index] try: split_val = cell[self._cur_position[index]] except IndexError: for i in range(index + 1): a = self._cur_position b = self.right[i] c = self.left[i] print("\n{} -> {}\n{}".format(c,b,a)) #this is where it screws raise IndexError("DIFFERENT") self.right.extend(split_index, [split_val]) self._cur_height += 1 def _split_left_cell(self): #Overwrite this with a clever function that queries the refinements for #clues as to which cell and element to split. top = self.left[-1] if self._left_split_index is None or len(top[self._left_split_index]) < 2: _, self._left_split_index = min([(len(cell),index) for index, cell in enumerate(top) if len(cell) > 1]) cell = top[self._left_split_index] point = cell[0] cell_size = len(cell) #Turn checks off. self.left.extend(self._left_split_index, [point]) return self._left_split_index, cell_size, point