Revision 935
untested realtime ir plotting
trunk/code/projects/mapping/python/map.py | ||
---|---|---|
13 | 13 |
|
14 | 14 |
pi = 3.1415927 |
15 | 15 |
|
16 |
#stolen from emily |
|
17 |
BOT_IR = [[4.5, 3.5], [7.5, 0], [4.5, -3.5], [-4, -3.5], [-4, 3.5]] |
|
18 |
ANGLE_IR = [[.78, .63], [1, 0], [0.78, -.63], [0, -1], [0,1]] |
|
19 |
|
|
16 | 20 |
#endmine |
17 | 21 |
|
18 | 22 |
class map_model(model): |
... | ... | |
40 | 44 |
|
41 | 45 |
self.socket = socket |
42 | 46 |
self.bots = [i for i in xrange(16)] |
47 |
self.ir_pts = [] |
|
43 | 48 |
|
44 | 49 |
self.draw = map_model() |
45 | 50 |
self.draw.bot = [[1,1,pi/2],0] # x y theta |
... | ... | |
51 | 56 |
self.nticks = 21 |
52 | 57 |
self.units = 10 |
53 | 58 |
|
59 |
def add_scale_button_ref(self, scale_button): |
|
60 |
self.scale_button = scale_button |
|
61 |
|
|
62 |
|
|
54 | 63 |
def redraw(self, eqn): |
64 |
self.ir_scale = self.scale_button.get_value() |
|
55 | 65 |
self.clear_area() |
56 | 66 |
self.draw_bot() |
57 | 67 |
self.draw_grid() |
68 |
self.draw_ir_pts() |
|
58 | 69 |
|
59 | 70 |
def do_expose_event(self, event): |
60 | 71 |
self.redraw(None) |
... | ... | |
65 | 76 |
self.ctx.set_source_rgb(1,1,1) |
66 | 77 |
self.ctx.fill() |
67 | 78 |
|
79 |
def draw_ir_pts(self): |
|
80 |
self.ctx = self.window.cairo_create() |
|
81 |
self.ctx.new_path() |
|
82 |
self.ctx.set_source_rgb(0,0,1) |
|
68 | 83 |
|
84 |
for pt in self.ir_pts: |
|
85 |
x,y = pt |
|
86 |
self.ctx.move_to(x * self.ir_scale, y * self.ir_scale) |
|
87 |
self.ctx.rel_line_to(2,2) |
|
88 |
self.ctx.stroke() |
|
89 |
|
|
90 |
|
|
69 | 91 |
def draw_bot(self): |
70 | 92 |
self.ctx = self.window.cairo_create() |
71 | 93 |
self.ctx.new_path() |
... | ... | |
120 | 142 |
self.ctx.move_to(0,400) |
121 | 143 |
self.ctx.rel_line_to(800,0) |
122 | 144 |
self.ctx.stroke() |
123 |
|
|
145 |
|
|
124 | 146 |
#hticks |
125 | 147 |
self.ctx.move_to(0,400) |
126 | 148 |
for i in range(self.nticks + 1): |
... | ... | |
162 | 184 |
old = self.draw.bot #the info of bot src at prev iter |
163 | 185 |
newx = items[4] / 30.0 |
164 | 186 |
newy = items[5] / 30.0 |
165 |
new = [newx, newy] + [items[i] for i in range(6,12)] |
|
187 |
new = [newx, newy] + [items[i] for i in range(6,12)] |
|
188 |
|
|
189 |
for ir_ind in range(5): |
|
190 |
ir_reading = items[ir_ind + 7] |
|
191 |
if ir_reading != -1: |
|
192 |
vec = map(lambda y: ir_reading * y, ANGLE_IR[ir_ind]) |
|
193 |
vec = map(lambda x,y: x+y, vec, BOT_IR[ir_ind]) |
|
194 |
|
|
195 |
R = math.sqrt(vec[0]**2 + vec[1] ** 2) |
|
196 |
ttheta = math.atan2(vec[1], vec[0]) |
|
197 |
ttheta += items[6] |
|
198 |
|
|
199 |
reading_x = (R * math.cos(ttheta)) + newx |
|
200 |
reading_y = (R * math.sin(ttheta)) + newy |
|
201 |
|
|
202 |
self.ir_pts.append([reading_x, reading_y]) |
|
203 |
|
|
204 |
|
|
166 | 205 |
self.draw.bot = new |
167 | 206 |
|
168 | 207 |
return True |
... | ... | |
193 | 232 |
mywidg = Screen(sock) |
194 | 233 |
|
195 | 234 |
bigbox = gtk.HBox() |
196 |
bigbox.pack_end(mywidg, True, True, 0) |
|
235 |
bigbox.pack_start(mywidg, True, True, 0) |
|
236 |
|
|
237 |
buttonbox = gtk.VBox() |
|
238 |
bigbox.pack_end(buttonbox, False, False, 0) |
|
239 |
|
|
240 |
IRScale = gtk.ScaleButton(10, 0, 100, 10) |
|
241 |
buttonbox.pack_start(IRScale, False, False, 0) |
|
242 |
|
|
243 |
mywidg.add_scale_button_ref(IRScale) |
|
197 | 244 |
|
198 | 245 |
window.add(bigbox) |
199 | 246 |
bigbox.show() |
200 | 247 |
mywidg.show() |
248 |
buttonbox.show() |
|
249 |
IRScale.show() |
|
201 | 250 |
window.show() |
202 | 251 |
|
203 | 252 |
gobject.idle_add(mywidg.receive_info) |
Also available in: Unified diff