root / branches / colonetmk2 / code / projects / swarm / BOMPosRender / src / BOMPosServer.java @ 1456
History | View | Annotate | Download (5.39 KB)
1 |
import java.awt.*; |
---|---|
2 |
import java.io.*; |
3 |
import java.net.*; |
4 |
import java.util.Arrays; |
5 |
import java.util.Scanner; |
6 |
import java.util.Vector; |
7 |
|
8 |
import javax.swing.JApplet; |
9 |
|
10 |
public class BOMPosServer extends JApplet implements Runnable { |
11 |
private static final long serialVersionUID = 0L; |
12 |
|
13 |
private boolean running; |
14 |
|
15 |
class Client implements Runnable { |
16 |
final Socket socket; |
17 |
Scanner stream;
|
18 |
|
19 |
public Client(Socket s) throws IOException |
20 |
{ |
21 |
this.socket = s;
|
22 |
this.stream = new Scanner(this.socket.getInputStream()); |
23 |
new Thread(this).run(); |
24 |
} |
25 |
|
26 |
public void close() |
27 |
{ |
28 |
try {
|
29 |
socket.close(); |
30 |
} catch (IOException e) { |
31 |
e.printStackTrace(); |
32 |
} |
33 |
} |
34 |
|
35 |
public void run() |
36 |
{ |
37 |
System.out.println("Client connected"); |
38 |
try {
|
39 |
SensorReading[] readings = new SensorReading[16]; |
40 |
int robot = -1; |
41 |
int source, dest = -1; |
42 |
while(this.socket.isConnected()) |
43 |
{ |
44 |
while(!"st".equals(stream.next())) |
45 |
System.out.println("bad token"); |
46 |
source = stream.nextInt(); |
47 |
int destp = stream.nextInt();
|
48 |
if (dest >= destp || robot != source)
|
49 |
{ |
50 |
if (robot > 0) |
51 |
{ |
52 |
updateMap(robot, readings); |
53 |
BOMPosServer.this.repaint(); |
54 |
} |
55 |
Arrays.fill(readings, null); |
56 |
robot = source; |
57 |
} |
58 |
dest = destp; |
59 |
float dir = stream.nextInt()*2.0f*(float)Math.PI/160.0f; |
60 |
int dist = stream.nextInt();
|
61 |
//System.out.print(source + " " + dest + " " + dir + " " + dist + "\n");
|
62 |
if (dir > Math.PI*2) |
63 |
readings[dest] = null;
|
64 |
else
|
65 |
readings[dest] = new SensorReading(dir, dist);
|
66 |
BOMPosServer.this.readings[source][dest] = readings[dest]; |
67 |
} |
68 |
} catch (Exception e) { |
69 |
e.printStackTrace(); |
70 |
try {
|
71 |
this.socket.close();
|
72 |
} catch (IOException ex) {} |
73 |
} |
74 |
System.out.println("Client exit"); |
75 |
} |
76 |
} |
77 |
|
78 |
ServerSocket server;
|
79 |
Vector<Client> clients;
|
80 |
|
81 |
@Override
|
82 |
public void run() { |
83 |
try {
|
84 |
/*while(!server.isClosed())
|
85 |
{
|
86 |
Socket s = server.accept();
|
87 |
clients.add(new Client(s));
|
88 |
}*/
|
89 |
while (running)
|
90 |
{ |
91 |
new Client(new Socket("localhost", 2000)).run(); |
92 |
} |
93 |
} catch (IOException e) { |
94 |
e.printStackTrace(); |
95 |
} |
96 |
/*positions[0].th = 0;
|
97 |
float a = 0.0f;
|
98 |
while(running)
|
99 |
{
|
100 |
repaint();
|
101 |
readings[0][1] = new SensorReading(a, 2);
|
102 |
a += Math.PI/20;
|
103 |
try {
|
104 |
Thread.sleep(50);
|
105 |
} catch (InterruptedException e) {}
|
106 |
}*/
|
107 |
} |
108 |
|
109 |
@Override
|
110 |
public void init() { |
111 |
setSize(400, 400); |
112 |
|
113 |
clients = new Vector<Client>(); |
114 |
|
115 |
/*try {
|
116 |
server = new ServerSocket(2000);
|
117 |
} catch (IOException e) {
|
118 |
e.printStackTrace();
|
119 |
}*/
|
120 |
|
121 |
running = true;
|
122 |
|
123 |
new Thread(this).start(); |
124 |
} |
125 |
|
126 |
@Override
|
127 |
public void stop() { |
128 |
for(Client c : clients) {
|
129 |
c.close(); |
130 |
} |
131 |
} |
132 |
|
133 |
@Override
|
134 |
public void paint(Graphics g) |
135 |
{ |
136 |
g.clearRect(0, 0, getWidth(), getHeight()); |
137 |
|
138 |
for(int i=0; i<16; i++) |
139 |
{ |
140 |
if (Float.isNaN(positions[i].th)) continue; |
141 |
//System.out.println(i + ": (" + positions[i].x + "," + positions[i].y + ") " + positions[i].th);
|
142 |
drawBot(g, i); |
143 |
} |
144 |
} |
145 |
|
146 |
private void drawBot(Graphics g, int i) { |
147 |
int x = 3*positions[i].x + getWidth()/2; |
148 |
int y = -3*positions[i].y + getHeight()/2; |
149 |
g.drawOval(x-8, y-8, 16, 16); |
150 |
g.drawString(String.valueOf(i), x-4, y+4); |
151 |
for(int j=0; j<16; j++) |
152 |
{ |
153 |
if (readings[i][j]==null) continue; |
154 |
float ang = readings[i][j].dir + positions[i].th;
|
155 |
g.drawString(String.valueOf(j), x+(int)(12*Math.cos(ang)-4), y-(int)(14*Math.sin(ang)-5)); |
156 |
} |
157 |
if (!Float.isNaN(positions[i].th)) |
158 |
{ |
159 |
g.drawLine(x, y, (int)(x + 8*Math.cos(positions[i].th)), (int)(y - 8*Math.sin(positions[i].th))); |
160 |
} |
161 |
} |
162 |
|
163 |
public int get_num_robots() |
164 |
{ |
165 |
return 3; |
166 |
} |
167 |
|
168 |
static class SensorReading |
169 |
{ |
170 |
public float dir; |
171 |
public int dist; |
172 |
|
173 |
public SensorReading(float dir, int dist) |
174 |
{ |
175 |
this.dir = dir;
|
176 |
this.dist = dist;
|
177 |
} |
178 |
} |
179 |
|
180 |
SensorReading[][] readings = new SensorReading[16][16]; |
181 |
|
182 |
static class R2 |
183 |
{ |
184 |
public int x; |
185 |
public int y; |
186 |
public float th; |
187 |
|
188 |
public R2(int x, int y, float th) |
189 |
{ |
190 |
this.x = x;
|
191 |
this.y = y;
|
192 |
this.th = th;
|
193 |
} |
194 |
} |
195 |
|
196 |
static R2[] positions; |
197 |
static {
|
198 |
positions = new R2[16]; |
199 |
for(int i=0; i<16; i++) |
200 |
positions[i] = new R2(0, 0, Float.NaN); |
201 |
} |
202 |
|
203 |
public void updateMap(int robot, SensorReading[] readings) |
204 |
{ |
205 |
float a = 0.0f; |
206 |
int count = 0; |
207 |
for(int target=0; target<16; target++) |
208 |
{ |
209 |
if (readings[target] == null) continue; |
210 |
|
211 |
a += (Math.atan2(positions[target].y - positions[robot].y, positions[target].x - positions[robot].x) - readings[target].dir + 4*Math.PI) % (2*Math.PI); |
212 |
count++; |
213 |
} |
214 |
if (count > 0) |
215 |
{ |
216 |
a /= count; |
217 |
} |
218 |
positions[robot].th = /*robot==1 ? 0 :*/ a;
|
219 |
|
220 |
for(int target=0; target<16; target++) |
221 |
{ |
222 |
if (readings[target] == null) continue; |
223 |
if (readings[target].dist < 2) continue; |
224 |
if (readings[target].dist > 200) continue; |
225 |
/*System.out.println(target + " " + readings[target].dist + " " + readings[target].dir + " (" +
|
226 |
readings[target].dist*Math.cos(readings[target].dir) + "," + readings[target].dist*Math.sin(readings[target].dir) + ")");*/
|
227 |
int scale = 2 * get_num_robots(); // readings[target].dist/5; |
228 |
positions[target].x = ( (scale-1)*positions[target].x + (int)(readings[target].dist*Math.cos(positions[robot].th + readings[target].dir)) )/scale; |
229 |
positions[target].y = ( (scale-1)*positions[target].y + (int)(readings[target].dist*Math.sin(positions[robot].th + readings[target].dir)) )/scale; |
230 |
} |
231 |
|
232 |
System.out.println(robot + ": (" + positions[robot].x + "," + positions[robot].y + ") " + positions[robot].th); |
233 |
} |
234 |
} |