def __init__(self, shell, filename, **kwds):
text = get_text(filename)
text_pages = text.split("\nPAGE\n")
pages = []
page_size = (0, 0)
for text_page in text_pages:
lines = text_page.strip().split("\n")
page = Page(self, lines[0], lines[1:])
pages.append(page)
page_size = maximum(page_size, page.size)
self.pages = pages
bf = self.button_font
b1 = Button("Prev Page", font=bf, action=self.prev_page)
b2 = Button("Menu", font=bf, action=self.go_back)
b3 = Button("Next Page", font=bf, action=self.next_page)
b = self.margin
page_rect = Rect((b, b), page_size)
gap = (0, 18)
b1.topleft = add(page_rect.bottomleft, gap)
b2.midtop = add(page_rect.midbottom, gap)
b3.topright = add(page_rect.bottomright, gap)
Screen.__init__(self, shell, **kwds)
self.size = add(b3.bottomright, (b, b))
self.add(b1)
self.add(b2)
self.add(b3)
self.prev_button = b1
self.next_button = b3
self.set_current_page(0)
def from_point(point: Tuple[float, float],
distance: int) -> Set[Tuple[int, int]]:
""" Gets the sector the point is in plus the sectors it is within the given distance of """
start_sector = containing_sector(point)
sectors = {start_sector}
x_mod = 0
y_mod = 0
if point[0] % config.SECTOR_SIZE < distance:
x_mod = -1
elif config.SECTOR_SIZE - (point[0] % config.SECTOR_SIZE) < distance:
x_mod = 1
if point[1] % config.SECTOR_SIZE < distance:
y_mod = -1
elif config.SECTOR_SIZE - (point[1] % config.SECTOR_SIZE) < distance:
y_mod = 1
sectors.add(vectors.add(start_sector, (x_mod, 0)))
sectors.add(vectors.add(start_sector, (0, y_mod)))
sectors.add(vectors.add(start_sector, (x_mod, y_mod)))
return sectors
def __init__(self, shell, filename, **kwds):
text = get_text(filename)
text_pages = text.split("\nPAGE\n")
pages = []
page_size = (0, 0)
for text_page in text_pages:
lines = text_page.strip().split("\n")
page = Page(self, lines[0], lines[1:])
pages.append(page)
page_size = maximum(page_size, page.size)
self.pages = pages
bf = self.button_font
b1 = Button("Prev Page", font=bf, action=self.prev_page)
b2 = Button("Menu", font=bf, action=self.go_back)
b3 = Button("Next Page", font=bf, action=self.next_page)
b = self.margin
page_rect = Rect((b, b), page_size)
gap = (0, 18)
b1.topleft = add(page_rect.bottomleft, gap)
b2.midtop = add(page_rect.midbottom, gap)
b3.topright = add(page_rect.bottomright, gap)
Screen.__init__(self, shell, **kwds)
self.size = add(b3.bottomright, (b, b))
self.add(b1)
self.add(b2)
self.add(b3)
self.prev_button = b1
self.next_button = b3
self.set_current_page(0)
def move(self, milliseconds, thrust_vector=(0, 0), gravity_sources=[]):
ax, ay = thrust_vector
gx, gy = gravitational_field(gravity_sources, self.x, self.y)
ax += gx
ay += gy
self.vx += ax * milliseconds / 1000
self.vy += ay * milliseconds / 1000
dx, dy = self.vx * milliseconds / 1000.0, self.vy * milliseconds / 1000.0
self.x, self.y = vectors.add((self.x, self.y), (dx, dy))
if bounce:
if self.x < -10 or self.x > 10:
self.vx = -self.vx
if self.y < -10 or self.y > 10:
self.vy = -self.vy
else:
if self.x < -10:
self.x += 20
if self.y < -10:
self.y += 20
if self.x > 10:
self.x -= 20
if self.y > 10:
self.y -= 20
self.rotation_angle += self.angular_velocity * milliseconds / 1000.0
def closest_analogies(
left2: str, left1: str, right2: str, words: List[Word]
) -> List[Tuple[float, Word]]:
word_left1 = find_word(left1, words)
word_left2 = find_word(left2, words)
word_right2 = find_word(right2, words)
if (not word_left1) or (not word_left2) or (not word_right2):
return []
vector = v.add(
v.sub(word_left1.vector, word_left2.vector),
word_right2.vector)
closest = most_similar(vector, words)[:10]
def is_redundant(word: str) -> bool:
"""
Sometimes the two left vectors are so close the answer is e.g.
"shirt-clothing is like phone-phones". Skip 'phones' and get the next
suggestion, which might be more interesting.
"""
word_lower = word.lower()
return (
left1.lower() in word_lower or
left2.lower() in word_lower or
right2.lower() in word_lower)
closest_filtered = [(dist, w) for (dist, w) in closest if not is_redundant(w.text)]
return closest_filtered
def calculate_score(input, wt, wh, wb):
for query, query_data in input.items():
for url, doc in query_data.items():
qv = query.split()
tft = doc['tf_title_vector']
tfh = doc['tf_header_vector']
tfb = doc['tf_body_vector']
tft = mul(tft, wt)
tfh = mul(tfh, wh)
tfb = mul(tfb, wb)
t = add(tft, tfh)
t = add(t, tfb) # ( ... )
t = dot(t, doc['idf'])
score = t
doc['score'] = score
def shrink_wrap(self):
contents = self.subwidgets
if contents:
rects = [widget.rect for widget in contents]
#rmax = Rect.unionall(rects) # broken in PyGame 1.7.1
rmax = rects.pop()
for r in rects:
rmax = rmax.union(r)
self._rect.size = add(rmax.topleft, rmax.bottomright)
def shrink_wrap(self):
contents = self.subwidgets
if contents:
rects = [widget.rect for widget in contents]
#rmax = Rect.unionall(rects) # broken in PyGame 1.7.1
rmax = rects.pop()
for r in rects:
rmax = rmax.union(r)
self._rect.size = add(rmax.topleft, rmax.bottomright)
def move(self, milliseconds):
dx, dy = self.vx * milliseconds / 1000.0, self.vy * milliseconds / 1000.0
self.x, self.y = vectors.add((self.x, self.y), (dx, dy))
if self.x < -10:
self.x += 20
if self.y < -10:
self.y += 20
if self.x > 10:
self.x -= 20
if self.y > 10:
self.y -= 20
self.rotation_angle += self.angular_velocity * milliseconds / 1000.0
def offset_point(point, rotate_function, offset_length, predecessor,
successor):
pre_offset_vector = None
suc_offset_vector = None
if predecessor:
pre_direction = vectors.unit(vectors.sub(point, predecessor))
pre_offset_vector = vectors.mult(rotate_function(pre_direction),
offset_length)
if successor:
suc_direction = vectors.unit(vectors.sub(successor, point))
suc_offset_vector = vectors.mult(rotate_function(suc_direction),
offset_length)
if predecessor and successor:
return intersection((vectors.add(predecessor, pre_offset_vector),
vectors.add(point, pre_offset_vector)),
(vectors.add(point, suc_offset_vector),
vectors.add(successor, suc_offset_vector)))
elif predecessor:
return vectors.add(point, pre_offset_vector)
elif successor:
return vectors.add(point, suc_offset_vector)
else:
raise Exception(
'at least one of predecessor and successor have to be set')
def _zoom_change(self, step, center):
new_level = self._zoom_at(self._zoom_increment + step)
old_world = screen_to_world(center, self.pan, self.zoom)
new_world = screen_to_world(center, self.pan, new_level)
world_pan = vectors.sub(new_world, old_world)
self.zoom = new_level
self.pan = vectors.add(self.pan, world_to_screen(world_pan, (0, 0), new_level))
self._zoom_increment += step
return
def pnt2line(pnt, start, end):
line_vec = vec.vector(start, end)
pnt_vec = vec.vector(start, pnt)
line_len = vec.length(line_vec)
line_unitvec = vec.unit(line_vec)
pnt_vec_scaled = vec.scale(pnt_vec, 1.0 / line_len)
t = vec.dot(line_unitvec, pnt_vec_scaled)
if t < 0.0:
t = 0.0
elif t > 1.0:
t = 1.0
nearest = vec.scale(line_vec, t)
dist = vec.distance(nearest, pnt_vec)
nearest = vec.add(nearest, start)
return (dist, nearest)
def closest_analogies(left2: str, left1: str, right2: str,
words: List[Word]) -> List[Tuple[float, Word]]:
word_left1 = find_word(words, left1)
word_left2 = find_word(words, left2)
word_right2 = find_word(words, right2)
vector = v.add(v.sub(word_left1.vector, word_left2.vector),
word_right2.vector)
closest = sorted_by_similarity(words, vector)[:10]
def is_redundant(word: str) -> bool:
#Sometimes the two left vectors are so close the answer is e.g.
#"shirt-clothing is like phone-phones". Skip 'phones' and get the next
#suggestion, which might be more interesting.
return (left1.lower() in word.lower() or left2.lower() in word.lower()
or right2.lower() in word.lower())
return [(dist, w) for (dist, w) in closest if not is_redundant(w.text)]
def update(self):
#self.rect.y -= 1
target_vector = v.sub(self.target, self.pos)
move_vector = [c * self.speed for c in v.normalize(target_vector)]
self.x, self.y = v.add(self.pos, move_vector)
self.rect.x = self.x
self.rect.y = self.y
for wall in walls:
if self.rect.colliderect(wall.rect):
if self.rect.x > 0: # Moving right; Hit the left side of the wall
self.rect.right = wall.rect.left
if self.rect.x < 0: # Moving left; Hit the right side of the wall
self.rect.left = wall.rect.right
if self.rect.y > 0: # Moving down; Hit the top side of the wall
self.rect.bottom = wall.rect.top
if self.rect.y < 0: # Moving up; Hit the bottom side of the wall
self.rect.top = wall.rect.bottom
def get_line(self,p1,p2,img):
a,b = v.vector(p1, p2)
if a>b:l=a
else:l=b
if l==0: return [],[]
li=cv.InitLineIterator(img, p1, p2,8)
points=[]
for i,c in enumerate(li):
points.append(c)
l=len(points)
if len(points)==0:
return [],[]
vec=(a/float(l),b/float(l))
real_p=[]
for i in range(l):
# list.append(c)
# for i in range(l):
p=v.int_point(v.add(p1,vec,i))
real_p.append(p)
# points.append(img[p[1],p[0]]);
return points,real_p
def set_new_position(self, points_or_corners, offset=True, scale=1):
'''
Sets new position for this marker using points (in order)
@param points_or_corners: list of points or corners
@param offset: if true, image ROI is checked and points are shifted
'''
if len(points_or_corners) > 0 and type(points_or_corners[0]) == tuple:
self.predicted = -1
points = points_or_corners
img = self.m_d.img
(x, y, _, _) = rect = cv.GetImageROI(img)
if offset and (x, y) <> (0, 0):
points = map(lambda z: add((x, y), z), points)
cv.ResetImageROI(img)
crit = (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)
if (scale > 1):
points = cv.FindCornerSubPix(self.m_d.gray_img, points,
(scale * 2 + 4, scale * 2 + 4),
(-1, -1), crit)
else:
points = cv.FindCornerSubPix(self.m_d.gray_img, points, (3, 3),
(-1, -1), crit)
ncorners = Corner.get_corners(points, self.m_d.time)
if len(self.corners) <> 0:
for i, cor in enumerate(ncorners):
cor.compute_change(self.corners[i])
cv.SetImageROI(img, rect)
else:
ncorners = points_or_corners
self.predicted += len(filter(lambda x: x.is_predicted, ncorners))
for i, c in enumerate(ncorners):
c.black_inside = self.black_inside
# if len(self.corners)==4:
# if dist_points(c.p, self.corners[i].p)<4:
# c.p=self.corners[i].p
self.corners = ncorners
self.area = abs(cv.ContourArea(self.points))
self.last_seen = self.m_d.time
self.model_view = None
def set_new_position(self, points_or_corners, offset=True, scale=1):
'''
Sets new position for this marker using points (in order)
@param points_or_corners: list of points or corners
@param offset: if true, image ROI is checked and points are shifted
'''
if len(points_or_corners) > 0 and type(points_or_corners[0]) == tuple:
self.predicted = -1
points = points_or_corners
img = self.m_d.img
(x, y, _, _) = rect = cv.GetImageROI(img)
if offset and (x, y) <> (0, 0):
points = map(lambda z: add((x, y), z), points)
cv.ResetImageROI(img)
crit = (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)
if (scale > 1):
points = cv.FindCornerSubPix(self.m_d.gray_img, points, (scale * 2 + 4, scale * 2 + 4), (-1, -1), crit)
else:
points = cv.FindCornerSubPix(self.m_d.gray_img, points, (3, 3), (-1, -1), crit)
ncorners = Corner.get_corners(points, self.m_d.time)
if len(self.corners) <> 0:
for i, cor in enumerate(ncorners):
cor.compute_change(self.corners[i])
cv.SetImageROI(img, rect)
else:
ncorners = points_or_corners
self.predicted += len(filter(lambda x:x.is_predicted, ncorners))
for i,c in enumerate(ncorners):
c.black_inside=self.black_inside
# if len(self.corners)==4:
# if dist_points(c.p, self.corners[i].p)<4:
# c.p=self.corners[i].p
self.corners = ncorners
self.area = abs(cv.ContourArea(self.points))
self.last_seen = self.m_d.time
self.model_view = None
def move(self, milliseconds):
dx, dy = self.vx * milliseconds / 1000.0, self.vy * milliseconds / 1000.0
self.x, self.y = vectors.add((self.x, self.y), (dx, dy))
self.rotation_angle += self.angular_velocity * milliseconds / 1000.0
def test_can_add_vectors(self):
vector_a = [1, 2]
vector_b = [2, 1]
self.assertEqual(vectors.add(vector_a, vector_b), [3, 3])
def local_to_global(self, p):
return add(p, self.local_to_global_offset())
def local_to_global_offset(self):
d = self.topleft
parent = self.parent
if parent:
d = add(d, parent.local_to_global_offset())
return d
def transformed(self):
rotated = (vectors.rotate2d(self.rotation_angle, v)
for v in self.points)
return [vectors.add((self.x, self.y), v) for v in rotated]
def gravitational_field(sources, x, y):
fields = [
vectors.scale(-source.gravity, (x - source.x, y - source.y))
for source in sources
]
return vectors.add(*fields)
def local_to_global_offset(self):
d = self.topleft
parent = self.parent
if parent:
d = add(d, parent.local_to_global_offset())
return d
def find_path(goal, robots, minx, maxx, miny, maxy):
# Set the robot closest to the target as leader. Either check them here or go to a formation at first.
leader_assignment.set_leader(goal, robots, True)
# Find the robots' positions.
for i in range(0, len(robots)):
if robots[i].get_node_in_system() == 0:
leader_position = robots[i].get_position_simulation()
elif robots[i].get_node_in_system() == 1:
follower_1_position = robots[i].get_position_simulation()
elif robots[i].get_node_in_system() == 2:
follower_2_position = robots[i].get_position_simulation()
# Now let's create a matrix representing the space in which the robots operate.
# 10 intervals per meter
scale = 10
nx = int((maxx - minx) * scale)
ny = int((maxy - miny) * scale)
matrix = [[0 for i in range(ny)] for j in range(nx)]
# Place the goal and start in the matrix (or get their representation at least).
start_matrix = (int((leader_position[0] - minx) * scale),
int((leader_position[1] - miny) * scale))
goal_matrix = (int((goal[0] - minx) * scale), int(
(goal[1] - miny) * scale))
# Place followers in matrix and add some margins around them so that the leader at least not is aiming straight
# towards them.
follower_1_position_matrix = [
int((follower_1_position[0] - minx) * scale),
int((follower_1_position[1] - miny) * scale)
]
follower_2_position_matrix = [
int((follower_2_position[0] - minx) * scale),
int((follower_2_position[1] - miny) * scale)
]
# How big are they (in meters * scale)?
width_follower = 0.8 * scale
for i in range(nx):
for j in range(ny):
if vectors.distance_points(follower_1_position_matrix, [i,j]) <= width_follower or \
vectors.distance_points(follower_2_position_matrix, [i, j]) <= width_follower:
matrix[i][j] = 1
# Place obstacles in matrix and add some space around them so that they are not just one matrix element wide.
global obstacles
width_obstacle = 1.5 * scale
if obstacles[0] != [0]:
obstacle_0_position_matrix = [
int((obstacles[0][0] - minx) * scale),
int((obstacles[0][1] - miny) * scale)
]
print obstacle_0_position_matrix
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_0_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
if obstacles[1] != [0]:
obstacle_1_position_matrix = [
int((obstacles[1][0] - minx) * scale),
int((obstacles[1][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_1_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
# Find the shortest path using the A* algorithm
sim_area = numpy.array(matrix)
path = astar.astar(sim_area, goal_matrix, start_matrix)
# Shift the path so that it corresponds to the actual area that is considered.
if path is not False:
for i in range(len(path)):
path[i] = vectors.add(vectors.multiply(path[i], 1. / scale),
[minx, miny])
# It seems to be a good idea to not tell the robot to go to a point really close to it, so just cut out the first
# 5 or so points. Also add the goal position at the end since it is not given from the A* algorithm.
short_path = [0 for i in range(len(path) - 4)]
for i in range(4, len(path) - 1):
short_path[i - 4] = path[i]
short_path[len(short_path) - 1] = goal
return short_path
def new_function(v):
return add(scalar, v)
def local_to_global(self, p):
return add(p, self.local_to_global_offset())
def linear_combination(scalars, *vectors):
scaled = [scale(s, v) for s,v in zip(scalars, vectors)]
return add(*scaled)
def find_path(goal, robots, minx, maxx, miny, maxy):
global subscriber
subscriber = rospy.Subscriber("/position", Pos, path_callback, robots)
# Make sure we've collected the data by waiting a while (the subscriber will unsubscribe itself when done).
time.sleep(1)
# Set the robot closest to the target as leader.
leader_assignment.set_leader(goal, robots, False)
# Find the robots' positions.
for i in range(0, len(robots)):
if robots[i].get_node_in_system() == 0:
leader_position = robots[i].get_position()
elif robots[i].get_node_in_system() == 1:
follower_1_position = robots[i].get_position()
elif robots[i].get_node_in_system() == 2:
follower_2_position = robots[i].get_position()
# Now let's create a matrix representing the space in which the robots operate.
# Number of intervals per meter.
scale = 10
nx = int((maxx - minx) * scale)
ny = int((maxy - miny) * scale)
# A matrix filled with zeros representing the area that the camera sees.
matrix = [[0 for j in range(ny)] for i in range(nx)]
# The start and goal positions represented in the matrix.
start_matrix = (int((leader_position[0] - minx) * scale),
int((leader_position[1] - miny) * scale))
goal_matrix = (int((goal[0] - minx) * scale), int(
(goal[1] - miny) * scale))
# Place followers in matrix and add some margins around them so that the leader at least not is aiming straight
# towards them.
follower_1_position_matrix = [
int((follower_1_position[0] - minx) * scale),
int((follower_1_position[1] - miny) * scale)
]
follower_2_position_matrix = [
int((follower_2_position[0] - minx) * scale),
int((follower_2_position[1] - miny) * scale)
]
# Set size of the followers so that they are represented correctly in the matrix. Number in meters, later scaled.
width_follower = 0.7 * scale
# Place ones in the matrix where the followers are + a region around them so that the leader will not move there.
for i in range(nx):
for j in range(ny):
if vectors.distance_points(follower_1_position_matrix, [i,j]) <= width_follower or \
vectors.distance_points(follower_2_position_matrix, [i, j]) <= width_follower:
matrix[i][j] = 1
# Place obstacles in matrix and add some space around them so that they are not just one matrix element wide.
global obstacles
width_obstacle = 1 * scale
if obstacles[0] != 0:
obstacle_0_position_matrix = [
int((obstacles[0][0] - minx) * scale),
int((obstacles[0][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_0_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
if obstacles[1] != 0:
obstacle_1_position_matrix = [
int((obstacles[1][0] - minx) * scale),
int((obstacles[1][1] - miny) * scale)
]
for i in range(nx):
for j in range(ny):
if vectors.distance_points(obstacle_1_position_matrix,
[i, j]) <= width_obstacle:
matrix[i][j] = 1
# Find the shortest path using the A* algorithm
cam_area = numpy.array(matrix)
path = astar.astar(cam_area, goal_matrix, start_matrix)
# Shift the path so that it corresponds to the actual area that is considered.
if path is not False:
for i in range(len(path)):
path[i] = vectors.add(vectors.multiply(path[i], 1. / scale),
[minx, miny])
# It seems to be a good idea to not tell the robot to go to a point really close to it, so just cut out the first
# 5 or so points. Also add the goal position at the end since it is not given from the A* algorithm.
short_path = [0 for i in range(len(path) - 4)]
for i in range(5, len(path)):
short_path[i - 5] = path[i]
short_path[len(short_path) - 1] = goal
return short_path
def get_points(p1,p2,f1,f2,points):
a,b=self.get_line(v.add(p1, vec1, f1), v.add(p2,vec2,f2), img)
points.extend(a)
real_p.extend(b)
def new_function(v):
return add(translation,v)
def apply_A(v):
return add(scale(v[0], Ae1), scale(v[1], Ae2), scale(v[2], Ae3))
def add_line(a,b,c,d,factor):
p1=v.add(r[a],v.vector(r[a],r[b]),factor)
p2=v.add(r[c],v.vector(r[c],r[d]),factor)
lines.append(((p1,p2),(a,c)))
def main():
pygame.init()
drawing.init()
screen_data = drawing.ScreenData(
pygame.display.set_mode(config.SCREEN_RES, pygame.RESIZABLE), (0, 0))
input_data = InputData()
path_data = pathing.PathData()
selection = None
lots = []
city = generation.generate()
city_labels = []
for road in city.roads:
city_labels.append((str(road.global_id), road.point_at(0.5)))
prev_time = pygame.time.get_ticks()
running = True
while running:
if pygame.time.get_ticks() - prev_time < 16:
continue
input_data.pos = pygame.mouse.get_pos()
input_data.pressed = pygame.mouse.get_pressed()
prev_time = pygame.time.get_ticks()
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.VIDEORESIZE:
screen_data.screen = pygame.display.set_mode(
event.dict["size"], pygame.RESIZABLE)
config.SCREEN_RES = event.dict["size"]
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_g:
debug.SHOW_ROAD_ORDER = False
city_labels = []
selection = None
path_data = pathing.PathData()
city = generation.generate()
for road in city.roads:
city_labels.append(
(str(road.global_id), road.point_at(0.5)))
if event.key == pygame.K_b:
lots = build_gen.gen_lots(city)
# Pathing
elif event.key == pygame.K_z:
path_data.start = road_near_point(input_data.pos,
screen_data, city)
elif event.key == pygame.K_x:
path_data.end = road_near_point(input_data.pos,
screen_data, city)
elif event.key == pygame.K_c:
pathing.astar(path_data, city.roads)
elif event.key == pygame.K_v:
pathing.dijkstra(path_data, city.roads)
# Debug Views
else:
handle_keys_debug(event.key)
elif event.type == pygame.MOUSEBUTTONDOWN:
# Zooming
if event.button == 4:
screen_data.zoom_in(input_data.pos)
elif event.button == 5:
screen_data.zoom_out(input_data.pos)
# Dragging & Selection
if input_data.prev_pressed[0]:
if input_data.pressed[0]: # Continue drag
screen_data.pan = vectors.add(
screen_data.pan,
vectors.sub(input_data.pos, input_data.drag_prev_pos))
input_data.drag_prev_pos = input_data.pos
else:
if input_data.pos == input_data.drag_start: # Select road
selection = selection_from_road(
road_near_point(input_data.drag_start, screen_data,
city))
# Clear out drag information
input_data.drag_start = None
input_data.drag_prev_pos = (0, 0)
else:
if input_data.pressed[0]: # Drag started
input_data.drag_start = input_data.pos
input_data.drag_prev_pos = input_data.pos
# Drawing
screen_data.screen.fill((0, 0, 0))
if debug.SHOW_HEATMAP:
drawing.draw_heatmap(50, city, screen_data)
if debug.SHOW_SECTORS:
drawing.draw_sectors(screen_data)
color = (125, 255, 50)
for poly in lots:
temp = []
for point in poly:
temp.append(
drawing.world_to_screen(point, screen_data.pan,
screen_data.zoom))
pygame.draw.polygon(screen_data.screen, color, temp)
color = (color[0], color[1] - 11, color[2] + 7)
if color[1] < 0:
color = (color[0], 255, color[2])
if color[2] > 255:
color = (color[0], color[1], 0)
# Draw roads
if debug.SHOW_ISOLATE_SECTOR and selection is not None:
for sector in sectors.from_seg(selection.road):
drawing.draw_all_roads(city.sectors[sector], screen_data)
elif debug.SHOW_MOUSE_SECTOR:
mouse_sec = sectors.containing_sector(
drawing.screen_to_world(input_data.pos, screen_data.pan,
screen_data.zoom))
if mouse_sec in city.sectors:
drawing.draw_all_roads(city.sectors[mouse_sec], screen_data)
else:
tl_sect = sectors.containing_sector(
drawing.screen_to_world((0, 0), screen_data.pan,
screen_data.zoom))
br_sect = sectors.containing_sector(
drawing.screen_to_world(config.SCREEN_RES, screen_data.pan,
screen_data.zoom))
for x in range(tl_sect[0], br_sect[0] + 1):
for y in range(tl_sect[1], br_sect[1] + 1):
if (x, y) in city.sectors:
drawing.draw_all_roads(city.sectors[(x, y)],
screen_data)
drawing.draw_roads_selected(selection, screen_data)
drawing.draw_roads_path(path_data, screen_data)
if debug.SHOW_INFO:
debug_labels = debug.labels(screen_data, input_data, path_data,
selection, city)
for x in range(len(debug_labels[0])):
label_pos = (10, 10 + x * 15)
drawing.draw_label_screen((debug_labels[0][x], label_pos),
screen_data, 1)
for x in range(len(debug_labels[1])):
label_pos = (config.SCREEN_RES[0] - 10, 10 + x * 15)
drawing.draw_label_screen((debug_labels[1][x], label_pos),
screen_data, -1)
if debug.SHOW_ROAD_ORDER:
for label in city_labels:
drawing.draw_label_world(label, screen_data, 1)
pygame.display.flip()
def move_follower(robot, robots, max_speed, pid_leader, pid_follower,
distances):
global at_goal
# Find what the other robots are in the system.
for i in range(0, len(robots)):
if robots[i].get_node_in_system() == 0:
leader = robots[i]
elif (robots[i].get_node_in_system() != 0) and (robots[i]
is not robot):
follower = robots[i]
# Desired distances to keep to the other robots. Leader and follower are either correctly assigned or something else
# is terribly wrong. Shouldn't initialise them as a safety since that wouldn't allow you to identify the problem.
desired_distance_leader = distances[leader.get_node_in_system()][
robot.get_node_in_system()]
desired_distance_follower = distances[follower.get_node_in_system()][
robot.get_node_in_system()]
# The robots' positions.
robot_position = robot.get_position()
follower_position = follower.get_position()
leader_position = leader.get_position()
# This follower's orientation
robot_orientation = robot.get_orientation()
# The orientation of the leader
leader_orientation = leader.get_orientation()
# Calculate the actual distances to the other robots.
distance_leader = vectors.distance_points(leader_position, robot_position)
distance_follower = vectors.distance_points(follower_position,
robot_position)
# Use PID-controller to go to a position which is at the desired distance from both the other robots.
error_leader = distance_leader - desired_distance_leader
error_follower = distance_follower - desired_distance_follower
u_leader = pid_leader.pid(error_leader)
u_follower = pid_follower.pid(error_follower)
# u is always going to be a positive value. As long as the robots don't overshoot this shouldn't be a problem, but
# if they do they might keep moving and even crash into the leader. Have this in consideration.
u = math.sqrt(u_follower**2 + u_leader**2)
# Create vectors to the leader, the other follower, the orientation of the leader and from these decide on a
# direction vector, which this follower should move along. The further away the robot is from either the leader or
# the other follower, the bigger the tendency is to move towards that robot. If it's more or less at the right
# distance from the other robots it is favorable to move in the orientation of the leader, which is implemented by
# the usage of the variable scale which is an exponential decaying function squared and scaled by 2/3, in other
# words the follower will only move in the direction of the leader's orientation if it's more or less perfectly in
# the right position.
if u_leader != 0 and u_follower != 0:
vector_leader = vectors.multiply(
vectors.normalise(vectors.subtract(leader_position,
robot_position)),
u_leader * u_follower)
else:
vector_leader = vectors.multiply(
vectors.normalise(vectors.subtract(leader_position,
robot_position)), 0.001)
if u_follower != 0:
vector_follower = vectors.multiply(
vectors.normalise(
vectors.subtract(follower_position, robot_position)),
u_follower)
else:
vector_follower = vectors.multiply(
vectors.normalise(
vectors.subtract(follower_position, robot_position)), 0.001)
scale = (math.exp(-u)**2) * 2 / 3
vector_orientation_leader = vectors.multiply(
[math.cos(leader_orientation),
math.sin(leader_orientation)], scale)
direction_vector = vectors.add(vectors.add(vector_follower, vector_leader),
vector_orientation_leader)
# Calculate a goal angle from the direction vector.
goal_angle = math.atan2(direction_vector[1], direction_vector[0])
if goal_angle < 0:
goal_angle += 2 * math.pi
# Calculate a target angle from the goal angle and the orientation of this robot.
target_angle = angles.angle_difference(goal_angle, robot_orientation)
# Spin the robot towards the desired orientation. In general a P-regulator should be enough.
twist.angular.z = target_angle * 0.5
# Move the robot forward. The further away it is from the goal, as well as earlier error and predicted future error
# by the PID is considered in the variable u. Also the robot will move by full speed when oriented correctly, but
# slower the further away it is from its desired orientation given by target_angle.
twist.linear.x = u * math.fabs(math.pi - math.fabs(target_angle)) / math.pi
# If the robot is within an acceptable range, named tol, it is considered "in position". The robot will still keep
# moving though until all robots are at the goal, which is taken care of later.
tol = 0.05
if math.fabs(error_follower) < tol and math.fabs(error_leader) < tol:
robot.set_at_position(True)
else:
robot.set_at_position(False)
# Make sure the robot is not moving too fast.
if twist.linear.x > max_speed:
twist.linear.x = max_speed
if u_leader < 0:
twist.linear.x = 0.07
# If all the robots are at goal we have to stop moving of course.
if at_goal:
twist.linear.x = 0
twist.angular.z = 0
# If the robots are colliding it would be a good thing to just stop before an accident happens.
if distance_leader < 0.7 or distance_follower < 0.5:
twist.linear.x = 0
twist.angular.z = 0
robot.pub.publish(twist)
def translate1left(v):
return add((-1, 0, 0), v)
