/
main.py
138 lines (120 loc) · 4.46 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
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
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''Entry point for simulator. Intended for behaviour with no
team communication.
'''
from datetime import timedelta
import itertools
import logging
import sys
import time
import pygame
from pygame.locals import *
import Box2D
import conf
from pk_architecture import pk_set_debug_options
import ui
field_width = 700
field_height = 600
screen_size = (field_width+500, field_height)
robot_info_list = (
('31', (57, -42), 0, '31', 'defender', 'blue'),
('32', (30, -30), 0, '31', 'defender', 'blue')
)
ball_info_list = (
('ball', (12, -12)),
# ('ball', (45, -40))
)
def main():
pk_set_debug_options(warning=False,
trace=False,
debug=True,
info=False,
verbous=False)
logging.basicConfig(level=logging.WARN)
pygame.init()
screen = pygame.display.set_mode(screen_size)
gw = ui.GameWindow(screen)
allsprites = pygame.sprite.RenderUpdates()
clock = pygame.time.Clock()
world = Box2D.b2World(gravity=(0,0))
lwall, rwall, twall, bwall = conf.create_playground(world)
rbl = [] # robot body list
rsl = [] # robot sprite list
rrl = [] # robot role list
bbl = [] # ball body list
bsl = [] # ball sprite list
for ID,pos,heading,leader,role,team in robot_info_list:
body, sprite, role = conf.create_robot(ID, pos, heading,
world, leader, role, team)
rbl.append(body)
rsl.append(sprite)
rrl.append(role)
for name,pos in ball_info_list:
body, sprite = conf.create_ball(name, pos, world)
bbl.append(body)
bsl.append(sprite)
allsprites.add(rsl)
allsprites.add(bsl)
gw.add_spritegroup(allsprites)
fps = 60
dt = 1.0 / 60
vel_iters = 10
pos_iters = 10
t0 = time.time()
counter = 0
while True:
clock.tick(fps)
for e in pygame.event.get():
if e.type == QUIT:
pygame.quit()
sys.exit()
elif e.type == KEYDOWN:
if e.key == K_ESCAPE:
pygame.quit()
sys.exit()
wb_r_data = conf.build_wb_robot_data(rbl)
wb_o_data = conf.build_wb_object_data(bbl)
for i, body, role in itertools.izip(range(len(rbl)), rbl, rrl):
if not counter:
ml = []
for b, r in itertools.izip(rbl, rrl):
if body.ID == b.ID:
continue
elif role.team != r.team:
continue
ml.extend(r.wm.comm_out)
role.wm.comm_update(ml)
vi_r_data = conf.build_vi_robot_data(body, role,
itertools.izip(rbl, rrl))
vi_o_data = conf.build_vi_object_data(body, role, bbl)
in_grabber = conf.object_in_grabber(body, role, bbl)
role.feed_workbench_data(wb_r_data, wb_o_data)
role.feed_vision_data(vi_r_data, vi_o_data, in_grabber)
role.run()
# v_t, v_r = conf.get_speed(role.speed())
v_t = role.wm.translational_velocity
v_r = role.wm.phi_dot
li = body.set_linear_impulse(v_t, v_r, dt)
ai = body.set_angular_impulse(v_t, v_r, dt)
text = '{}: {} [{}]'.format(role.ID, role.role, role.state)
gw.add_dynamictext(text, (705, 120 + 20*i))
counter = (counter + 1) % 12
world.Step(dt, vel_iters, pos_iters)
score_red = 0
score_blue = 0
for body, sprite in itertools.izip(rbl, rsl):
sprite.set_pos(conf.b2g_pos(body.get_pos()))
sprite.rotate(conf.b2g_angle(body.get_angle()))
for body, sprite in itertools.izip(bbl, bsl):
sprite.set_pos(conf.b2g_pos(body.get_pos()))
sprite.rotate(conf.b2g_angle(body.get_angle()))
score_red += sprite.score_red
score_blue += sprite.score_blue
running_time = timedelta(seconds=int(time.time()-t0))
gw.add_dynamictext('Time: {}'.format(running_time), (705,20))
gw.add_dynamictext('Team Red: {}'.format(score_red), (705,50))
gw.add_dynamictext('Team Blue: {}'.format(score_blue), (705,70))
gw.update()
if __name__ == '__main__':
main()