Exemple #1
0
 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)
Exemple #2
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
Exemple #3
0
 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
Exemple #5
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
Exemple #6
0
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
Exemple #7
0
 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)
Exemple #8
0
 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)
Exemple #9
0
 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
Exemple #10
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')
Exemple #11
0
    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)]
Exemple #14
0
 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
Exemple #15
0
    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
Exemple #16
0
    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
Exemple #17
0
    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])
Exemple #20
0
 def local_to_global(self, p):
     return add(p, self.local_to_global_offset())
Exemple #21
0
 def local_to_global_offset(self):
     d = self.topleft
     parent = self.parent
     if parent:
         d = add(d, parent.local_to_global_offset())
     return d
Exemple #22
0
 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)
Exemple #24
0
 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
Exemple #26
0
 def new_function(v):
     return add(scalar, v)
Exemple #27
0
 def local_to_global(self, p):
     return add(p, self.local_to_global_offset())
Exemple #28
0
def linear_combination(scalars, *vectors):
    scaled = [scale(s, v) for s,v in zip(scalars, vectors)]
    return add(*scaled)
Exemple #29
0
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
Exemple #30
0
 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)
Exemple #32
0
def apply_A(v):
    return add(scale(v[0], Ae1), scale(v[1], Ae2), scale(v[2], Ae3))
Exemple #33
0
 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()
Exemple #35
0
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)