Skip to content

Waypoints

Per-robot waypoint routes: add, advance, insert and draw.

WaypointManager 

WaypointManager(config)
Source code in src/hivebr/engine/waypoints.py 
 8
 9
10
11
12
13
14
def __init__(self, config: dict):
    self.waypoints = []
    self.current_index = 0
    self.goal_tolerance = float(config["waypoints"].get("goal_tolerance", 0.35))
    self.colors = config["colors"]
    self.show_tolerance_circle = bool(config["waypoints"].get("show_tolerance_circle", False))
    self.define_orientation = bool(config["waypoints"].get("define_orientation", False))

waypoints instance-attribute 

waypoints = []

current_index instance-attribute 

current_index = 0

goal_tolerance instance-attribute 

goal_tolerance = float(
    config["waypoints"].get("goal_tolerance", 0.35)
)

colors instance-attribute 

colors = config['colors']

show_tolerance_circle instance-attribute 

show_tolerance_circle = bool(
    config["waypoints"].get("show_tolerance_circle", False)
)

define_orientation instance-attribute 

define_orientation = bool(
    config["waypoints"].get("define_orientation", False)
)

add_waypoint 

add_waypoint(pos)
Source code in src/hivebr/engine/waypoints.py 
16
17
18
19
20
21
22
23
def add_waypoint(self, pos):

    if len(pos) == 2:
        pos = (pos[0], pos[1], np.nan)        

    self.waypoints.append(
        np.array([pos[0], pos[1], pos[2]], dtype=float)
    )

clear 

clear()
Source code in src/hivebr/engine/waypoints.py 
25
26
27
def clear(self):
    self.waypoints.clear()
    self.current_index = 0

has_waypoint 

has_waypoint()
Source code in src/hivebr/engine/waypoints.py 
29
30
def has_waypoint(self):
    return self.current_index < len(self.waypoints)

get_current_waypoint 

get_current_waypoint()
Source code in src/hivebr/engine/waypoints.py 
32
33
def get_current_waypoint(self):
    return self.waypoints[self.current_index] if self.has_waypoint() else None

update 

update(robot_position)
Source code in src/hivebr/engine/waypoints.py 
35
36
37
38
39
40
41
42
def update(self, robot_position):
    goal = self.get_current_waypoint()        

    if goal is None:
        return

    if distance_between(robot_position, goal[:2]) < self.goal_tolerance:
        self.current_index += 1

insert_waypoint_at_current 

insert_waypoint_at_current(pos)
Source code in src/hivebr/engine/waypoints.py 
44
45
46
47
48
49
50
51
def insert_waypoint_at_current(self, pos):
    if len(pos) == 2:
        pos = (pos[0], pos[1], np.nan)

    wp = np.array([pos[0], pos[1], pos[2]], dtype=float)

    # Insere na posição atual, empurrando o anterior para a frente
    self.waypoints.insert(self.current_index, wp)

status_text 

status_text()
Source code in src/hivebr/engine/waypoints.py 
53
54
def status_text(self):
    return "None" if not self.has_waypoint() else f"{self.current_index + 1}/{len(self.waypoints)}"

draw 

draw(screen, camera)
Source code in src/hivebr/engine/waypoints.py 
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
def draw(self, screen, camera):
    if not self.waypoints:
        return
    waypoint_color = tuple(self.colors["waypoint"])
    goal_color = tuple(self.colors["goal"])
    black = tuple(self.colors["black"])
    # for i in range(len(self.waypoints) - 1):
    #     pygame.draw.line(screen, waypoint_color, camera.world_to_screen(*self.waypoints[i]), camera.world_to_screen(*self.waypoints[i+1]), 2)
    for i, wp in enumerate(self.waypoints):
        color = goal_color if i == self.current_index else waypoint_color
        pygame.draw.circle(screen, color, camera.world_to_screen(*wp[:2]), 8)
        pygame.draw.circle(screen, black, camera.world_to_screen(*wp[:2]), 8, 2)


        # =========================
        # Círculo transparente
        # =========================
        if self.show_tolerance_circle:
            radius_px = max(5, int(self.goal_tolerance * camera.scale))

            circle_surface = pygame.Surface(
                (radius_px * 2, radius_px * 2),
                pygame.SRCALPHA
            )

            color2 = (*color, 120)

            pygame.draw.circle(
                circle_surface,
                color2,
                (radius_px, radius_px),
                radius_px
            )

            screen_pos = camera.world_to_screen(*wp[:2])

            screen.blit(
                circle_surface,
                (screen_pos[0] - radius_px,
                screen_pos[1] - radius_px)
            )

        theta = wp[2]
        if not np.isnan(theta):

            px, py = camera.world_to_screen(wp[0], wp[1])

            arrow_len = max(20, int(0.5 * camera.scale))

            tip_x = px + arrow_len * np.cos(theta)
            tip_y = py - arrow_len * np.sin(theta)

            pygame.draw.line(
                screen,
                black,
                (px, py),
                (tip_x, tip_y),
                3
            )

            head_size = 8

            pygame.draw.line(
                screen,
                black,
                (tip_x, tip_y),
                (
                    tip_x - head_size*np.cos(theta - np.pi/6),
                    tip_y + head_size*np.sin(theta - np.pi/6)
                ),
                3
            )

            pygame.draw.line(
                screen,
                black,
                (tip_x, tip_y),
                (
                    tip_x - head_size*np.cos(theta + np.pi/6),
                    tip_y + head_size*np.sin(theta + np.pi/6)
                ),
                3
            )