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
