Revision 935
| trunk/code/projects/mapping/python/map.py (revision 935) | ||
|---|---|---|
| 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