root / branches / colonetmk2 / code / projects / swarm / BOMPosRender / src / BOMPosServer.java @ 1456
History | View | Annotate | Download (5.4 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 | } |