root / trunk / code / projects / mapping / python / map.py @ 935
History | View | Annotate | Download (7 KB)
1 |
#!/usr/bin/env python
|
---|---|
2 |
|
3 |
from scipy import linalg, array |
4 |
from gtk import * |
5 |
from socket import * |
6 |
from struct import * |
7 |
import gobject |
8 |
import sys, pickle, pygtk, gtk, string, cairo, gobject, math, weakref |
9 |
from random import random |
10 |
|
11 |
#mine
|
12 |
from model import model |
13 |
|
14 |
pi = 3.1415927
|
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 |
|
20 |
#endmine
|
21 |
|
22 |
class map_model(model): |
23 |
def __init__(self): |
24 |
self.eqn = []
|
25 |
|
26 |
def __setattr__(self,name,value): |
27 |
self.__dict__[name] = value
|
28 |
self.changed()
|
29 |
|
30 |
def add_eqn(self, eqn): |
31 |
self.eqn = [eqn] + self.eqn |
32 |
|
33 |
hostname = 'localhost'
|
34 |
|
35 |
class Screen(gtk.DrawingArea): |
36 |
__gsignals__ = { "expose-event" : "override" } |
37 |
|
38 |
def __init__ (self, socket): |
39 |
gtk.DrawingArea.__init__(self)
|
40 |
|
41 |
self.xres, self.yres = (800,800) |
42 |
|
43 |
self.set_size_request(self.xres, self.yres) |
44 |
|
45 |
self.socket = socket
|
46 |
self.bots = [i for i in xrange(16)] |
47 |
self.ir_pts = []
|
48 |
|
49 |
self.draw = map_model()
|
50 |
self.draw.bot = [[1,1,pi/2],0] # x y theta |
51 |
self.draw.add_dependent(self.redraw) |
52 |
|
53 |
self.scale_const = 50 |
54 |
self.xdisp = 400 |
55 |
self.ydisp = 400 |
56 |
self.nticks = 21 |
57 |
self.units = 10 |
58 |
|
59 |
def add_scale_button_ref(self, scale_button): |
60 |
self.scale_button = scale_button
|
61 |
|
62 |
|
63 |
def redraw(self, eqn): |
64 |
self.ir_scale = self.scale_button.get_value() |
65 |
self.clear_area()
|
66 |
self.draw_bot()
|
67 |
self.draw_grid()
|
68 |
self.draw_ir_pts()
|
69 |
|
70 |
def do_expose_event(self, event): |
71 |
self.redraw(None) |
72 |
|
73 |
def clear_area(self): |
74 |
self.ctx = self.window.cairo_create() |
75 |
self.ctx.rectangle(0,0,1000,1000) |
76 |
self.ctx.set_source_rgb(1,1,1) |
77 |
self.ctx.fill()
|
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) |
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 |
|
91 |
def draw_bot(self): |
92 |
self.ctx = self.window.cairo_create() |
93 |
self.ctx.new_path()
|
94 |
|
95 |
tbot = self.draw.bot
|
96 |
print tbot
|
97 |
x, y, theta = tbot[:3]
|
98 |
y = -y #because we don't want to draw down more units
|
99 |
|
100 |
#draw blob
|
101 |
self.ctx.set_source_rgb(0,1,0) |
102 |
xscl = (self.xres - self.xdisp) / self.units |
103 |
yscl = (self.yres - self.ydisp) / self.units |
104 |
|
105 |
cx = x * xscl + self.xdisp
|
106 |
cy = y * yscl + self.ydisp
|
107 |
self.ctx.arc(cx, cy, 20, 0, 2*pi) |
108 |
self.ctx.fill()
|
109 |
|
110 |
#draw "arrow"
|
111 |
self.ctx.set_source_rgb(0,0,1) |
112 |
self.ctx.arc(cx,cy, 5, 0, 2*pi) |
113 |
self.ctx.fill()
|
114 |
|
115 |
self.ctx.move_to(cx,cy)
|
116 |
dx = 20 * math.cos(theta + 0.05) |
117 |
dy = -20 * math.sin(theta + 0.05) |
118 |
dx = int(dx)
|
119 |
dy = int(dy)
|
120 |
|
121 |
self.ctx.rel_line_to(dx, dy)
|
122 |
self.ctx.stroke()
|
123 |
|
124 |
def draw_grid(self): |
125 |
|
126 |
self.ctx.set_source_rgb(0,0,0) |
127 |
self.ctx.new_path()
|
128 |
|
129 |
#vertical
|
130 |
self.ctx.move_to(400,0) |
131 |
self.ctx.rel_line_to(0,800) |
132 |
self.ctx.stroke()
|
133 |
|
134 |
#vticks
|
135 |
self.ctx.move_to(400,0) |
136 |
for i in range( self.nticks + 1 ): |
137 |
self.ctx.rel_line_to(5,0) |
138 |
self.ctx.stroke()
|
139 |
self.ctx.move_to(400, i * 800 / self.nticks) |
140 |
|
141 |
#horiz
|
142 |
self.ctx.move_to(0,400) |
143 |
self.ctx.rel_line_to(800,0) |
144 |
self.ctx.stroke()
|
145 |
|
146 |
#hticks
|
147 |
self.ctx.move_to(0,400) |
148 |
for i in range(self.nticks + 1): |
149 |
self.ctx.rel_line_to(0,-5) |
150 |
self.ctx.stroke()
|
151 |
self.ctx.move_to(i * 800 / self.nticks, 400) |
152 |
|
153 |
def draw_graph(self, equations, n): |
154 |
self.ctx = self.window.cairo_create() |
155 |
self.ctx.set_source_rgb(0,0,0) |
156 |
self.ctx.new_path()
|
157 |
|
158 |
for equn in equations: |
159 |
xmin, xmax = equn[-2],equn[-1] |
160 |
eqn = equn[:-2]
|
161 |
dx = float(xmax - xmin) / n
|
162 |
x = xmin |
163 |
y = 0
|
164 |
for i in range(len(eqn)): |
165 |
y += eqn[-1 * i - 1] * ( x ** i) |
166 |
|
167 |
self.ctx.move_to(self.x_disp + x * self.scale_const,400 - y * self.scale_const) |
168 |
|
169 |
for diffel in range(1,n+1): |
170 |
x = xmin + diffel * dx |
171 |
y = 0
|
172 |
for i in range(len(eqn)): |
173 |
y += eqn[-1 * i - 1] * ( x ** i) |
174 |
self.ctx.line_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const)) |
175 |
self.ctx.stroke()
|
176 |
self.ctx.move_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const)) |
177 |
|
178 |
def receive_info(self): |
179 |
|
180 |
buf = self.socket.recv(100) |
181 |
if (buf):
|
182 |
items = unpack('4c2hf5h',buf)
|
183 |
src = ord(items[0]) |
184 |
old = self.draw.bot #the info of bot src at prev iter |
185 |
newx = items[4] / 30.0 |
186 |
newy = items[5] / 30.0 |
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 |
|
205 |
self.draw.bot = new
|
206 |
|
207 |
return True |
208 |
|
209 |
|
210 |
def solve(lisvals): |
211 |
p0y, p1y, m0y, m1y, x0, x1 = lisvals |
212 |
A = array([[x0**3 , x0**2 , x0 , 1], |
213 |
[x1**3 , x1**2 , x1 , 1], |
214 |
[3 * x0**2 , 2 * x0 , 1 , 0], |
215 |
[3 * x1**2 , 2 * x1 , 1 , 0]]) |
216 |
|
217 |
B = array([p0y, p1y, m0y, m1y]) |
218 |
tarr = linalg.solve(A,B) |
219 |
lis = [] |
220 |
for i in range(4): |
221 |
lis.append(tarr[i]) |
222 |
return lis + [0,1] |
223 |
|
224 |
def main(): |
225 |
|
226 |
window = gtk.Window() |
227 |
window.connect("delete-event", gtk.main_quit)
|
228 |
|
229 |
sock = socket() |
230 |
sock.connect((hostname, int(sys.argv[1]))) |
231 |
|
232 |
mywidg = Screen(sock) |
233 |
|
234 |
bigbox = gtk.HBox() |
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) |
244 |
|
245 |
window.add(bigbox) |
246 |
bigbox.show() |
247 |
mywidg.show() |
248 |
buttonbox.show() |
249 |
IRScale.show() |
250 |
window.show() |
251 |
|
252 |
gobject.idle_add(mywidg.receive_info) |
253 |
gtk.main() |
254 |
|
255 |
if __name__=='__main__': |
256 |
if (len(sys.argv) < 2): #or not str.isdecimal(sys.argv[1])): |
257 |
print("Usage: %s <portno>" % sys.argv[0]) |
258 |
sys.exit() |
259 |
|
260 |
main() |