Project

General

Profile

Revision 935

Added by Tudor Achim over 15 years ago

untested realtime ir plotting

View differences:

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