1 package cz.cuni.amis.pogamut.usar2004.samples.AirScanner;
2
3 import cz.cuni.amis.pogamut.base3d.worldview.object.*;
4 import java.awt.*;
5 import java.awt.geom.Line2D;
6 import java.awt.geom.Point2D;
7 import java.awt.geom.Rectangle2D;
8 import java.awt.image.BufferedImage;
9 import java.io.*;
10 import java.nio.ByteBuffer;
11 import java.util.List;
12 import java.util.*;
13 import java.util.logging.Level;
14 import java.util.logging.Logger;
15 import java.util.zip.GZIPOutputStream;
16 import javax.imageio.ImageIO;
17
18 public class ScanPreview extends javax.swing.JFrame
19 {
20 public void runScanPreview()
21 {
22 setTitle("Scan Preview");
23 setVisible(true);
24 }
25
26 /**
27 * Creates new form ScanPreview
28 */
29 public ScanPreview()
30 {
31 initComponents();
32 data = ToolBox.initArray(startSize, startSize);
33 initBlackImage(tmp);
34 }
35 private int dataArrayXLimit;
36 private int dataArrayYLimit;
37 private int offsetX = 0;
38 private int offsetY = 0;
39 private final int extension = 400;
40 private final int extLimit = 100;
41
42 /**
43 * This wathes over the preview bitmap and array of heights during
44 * simulation to prevent writing outside the structures. If it wants to
45 * write outside this will resize the image and the array.
46 *
47 * @param x X coordinate.
48 * @param y Y coordinate.
49 * @return Reutrns true if the data is in limit.
50 */
51 private boolean keepDataArrayGreat(int x, int y)
52 {
53 if(x > dataArrayXLimit)
54 {
55 tmp = ToolBox.resizeImage(tmp, extension, 0, 0, 0);
56 tmpG = tmp.createGraphics();
57 data = ToolBox.resizeArray(data, extension, 0, 0, 0);
58 dataArrayXLimit = tmp.getWidth() - extLimit;
59 return true;
60 }
61 else if(x < tmp.getWidth() - dataArrayXLimit)
62 {
63 tmp = ToolBox.resizeImage(tmp, extension, 0, extension, 0);
64 tmpG = tmp.createGraphics();
65 data = ToolBox.resizeArray(data, extension, 0, extension, 0);
66 dataArrayXLimit = tmp.getWidth() - extLimit;
67 offsetX += extension;
68 translate.x -= extension;
69 return true;
70 }
71 if(y > dataArrayYLimit)
72 {
73 tmp = ToolBox.resizeImage(tmp, 0, extension, 0, 0);
74 tmpG = tmp.createGraphics();
75 data = ToolBox.resizeArray(data, 0, extension, 0, 0);
76 dataArrayYLimit = tmp.getHeight() - extLimit;
77 return true;
78 }
79 else if(y < tmp.getHeight() - dataArrayYLimit)
80 {
81 tmp = ToolBox.resizeImage(tmp, 0, extension, 0, extension);
82 tmpG = tmp.createGraphics();
83 data = ToolBox.resizeArray(data, 0, extension, 0, extension);
84 dataArrayYLimit = tmp.getHeight() - extLimit;
85 offsetY += extension;
86 translate.y -= extension;
87 return true;
88 }
89 return false;
90 }
91
92 private void initBlackImage(BufferedImage tmp)
93 {
94 tmpG.setColor(Color.BLACK);
95 tmpG.fillRect(0, 0, tmp.getWidth(), tmp.getHeight());
96
97 offsetX = tmp.getWidth() / 2;
98 offsetY = tmp.getHeight() / 2;
99 dataArrayXLimit = tmp.getWidth() - extLimit;
100 dataArrayYLimit = tmp.getHeight() - extLimit;
101 this.translate = new Point(this.getWidth() / 2 - offsetX, this.getHeight() / 2 - offsetY);
102 }
103
104 /**
105 * This method is called from within the constructor to initialize the form.
106 * WARNING: Do NOT modify this code. The content of this method is always
107 * regenerated by the Form Editor.
108 */
109 @SuppressWarnings("unchecked")
110 // <editor-fold defaultstate="collapsed" desc="Generated Code">//GEN-BEGIN:initComponents
111 private void initComponents() {
112
113 jLabel1 = new javax.swing.JLabel();
114
115 setDefaultCloseOperation(javax.swing.WindowConstants.EXIT_ON_CLOSE);
116 setTitle("Scan viewer");
117 setBackground(new java.awt.Color(51, 51, 51));
118 setForeground(java.awt.Color.white);
119 addWindowListener(new java.awt.event.WindowAdapter() {
120 public void windowClosing(java.awt.event.WindowEvent evt) {
121 formWindowClosing(evt);
122 }
123 });
124 addKeyListener(new java.awt.event.KeyAdapter() {
125 public void keyPressed(java.awt.event.KeyEvent evt) {
126 formKeyPressed(evt);
127 }
128 });
129
130 jLabel1.setBackground(new java.awt.Color(255, 255, 255));
131 jLabel1.setText("jLabel1");
132
133 javax.swing.GroupLayout layout = new javax.swing.GroupLayout(getContentPane());
134 getContentPane().setLayout(layout);
135 layout.setHorizontalGroup(
136 layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING)
137 .addGroup(layout.createSequentialGroup()
138 .addContainerGap(796, Short.MAX_VALUE)
139 .addComponent(jLabel1)
140 .addGap(227, 227, 227))
141 );
142 layout.setVerticalGroup(
143 layout.createParallelGroup(javax.swing.GroupLayout.Alignment.LEADING)
144 .addGroup(layout.createSequentialGroup()
145 .addContainerGap()
146 .addComponent(jLabel1)
147 .addContainerGap(717, Short.MAX_VALUE))
148 );
149
150 pack();
151 }// </editor-fold>//GEN-END:initComponents
152
153 private void formKeyPressed(java.awt.event.KeyEvent evt)//GEN-FIRST:event_formKeyPressed
154 {//GEN-HEADEREND:event_formKeyPressed
155 switch(evt.getKeyCode())
156 {
157 case 37:
158 translate.x += 10;
159 break;
160 case 39:
161 translate.x -= 10;
162 break;
163 case 40:
164 translate.y -= 10;
165 break;
166 case 38:
167 translate.y += 10;
168 break;
169 }
170 }//GEN-LAST:event_formKeyPressed
171
172 /**
173 * On closing we save the image to file and we generate text output as well.
174 *
175 * @param evt
176 */
177 private void formWindowClosing(java.awt.event.WindowEvent evt)//GEN-FIRST:event_formWindowClosing
178 {//GEN-HEADEREND:event_formWindowClosing
179 int counter = 0;
180 File directory = new File(System.getProperty("user.home") + "\\Desktop\\USAR_Scans\\");
181 File file;
182 do
183 {
184 if(!directory.exists())
185 {
186 directory.mkdir();
187 }
188 file = new File(System.getProperty("user.home") + "\\Desktop\\USAR_Scans\\img" + ((counter < 10)?"0" + counter:counter) + ".png");
189 counter++;
190 }
191 while(file.exists());
192 saveImage(createImage(file.getAbsolutePath()), file);
193 }//GEN-LAST:event_formWindowClosing
194 private final int border = 20;
195
196 /**
197 * Estimates the left bound of valid data.
198 *
199 * @return Returns left edge of recorded data.
200 */
201 private int getDataLeftMargin()
202 {
203 for(int i = border; i < data.length; i++)
204 {
205 for(int j = 0; j < data[i].length; j++)
206 {
207 if(data[i][j] != Double.MIN_VALUE)
208 {
209 return i - border;
210 }
211 }
212 }
213 return data.length / 2;
214 }
215
216 /**
217 * Estimates the right bound of valid data.
218 *
219 * @return Returns right edge of recorded data.
220 */
221 private int getDataRightMargin()
222 {
223 for(int i = data.length - border - 1; i >= 0; i--)
224 {
225 for(int j = 0; j < data[i].length; j++)
226 {
227 if(data[i][j] != Double.MIN_VALUE)
228 {
229 return i + border;
230 }
231 }
232 }
233 return data[0].length / 2;
234 }
235
236 /**
237 * Estimates the top bound of valid data.
238 *
239 * @return Returns top edge of recorded data.
240 */
241 private int getDataTopMargin()
242 {
243 for(int i = border; i < data[0].length; i++)
244 {
245 for(int j = 0; j < data.length; j++)
246 {
247 if(data[j][i] != Double.MIN_VALUE)//data[i][j] > Double.MIN_VALUE)
248 {
249 return i - border;
250 }
251 }
252 }
253 return data.length / 2;
254 }
255
256 /**
257 * Estimates the bottom bound of valid data.
258 *
259 * @return Returns lower edge of recorded data.
260 */
261 private int getDataBottomMargin()
262 {
263 for(int i = data[0].length - border - 1; i >= 0; i--)
264 {
265 for(int j = 0; j < data.length; j++)
266 {
267 if(data[j][i] != Double.MIN_VALUE)
268 {
269 return i + border;
270 }
271 }
272 }
273 return data.length / 2;
274 }
275 private final int infoPanelWidth = 280;
276 // private int getImportantPartIndex(int importance)
277 // {
278 // for(int i = histogram.length - 1; i >= histogram.length / 2; i--)
279 // {
280 // if(histogram[i] > importance)
281 // {
282 // return i;
283 // }
284 // }
285 // return histogram.length / 2;
286 // }
287 private boolean datFile = false;
288
289 /**
290 * Creates a final output image from collected data.
291 *
292 * @param path Specifies the location of the output.
293 */
294 private BufferedImage createImage(String path)
295 {
296 int left = getDataLeftMargin();
297 int top = getDataTopMargin();
298 int width = Math.max(getDataRightMargin() - left + 1, minWidth);
299 int height = Math.max(getDataBottomMargin() - top + 1, minHeight);
300 tmpG.drawRect(left, top, width - 1, height - 1);
301
302 BufferedImage img = new BufferedImage(width + infoPanelWidth, height, BufferedImage.TYPE_INT_ARGB);//257 = histogram
303 Graphics2D g = img.createGraphics();
304 Graphics gr = g.create();
305
306 gr.setColor(Color.BLACK);
307 gr.fillRect(0, 0, img.getWidth(), img.getHeight());
308
309
310
311 ByteBuffer buf = ByteBuffer.allocate((width + 1) * (height + 1) * 8);
312
313 //offset and scale match 255 shades according to max and min value obtained while scanning
314 double shift = 0 - dMin;
315 double scale = 255 / (shift + dMax / 2);//*7/4;//posunutí spektra o polovinu-zvýrazní se malé změnu u země(pahorkatina)->zmizí extrémní odchylky, které tvoří stromy.
316 for(int i = left; i < width + left; i++)
317 {
318 for(int j = top; j < height + top; j++)
319 {
320 //Double.MIN_VALUE means no value was scanned at that point. We represent this situation by BLACK color
321 if(data[i][j] == Double.MIN_VALUE)
322 {
323 img.setRGB(i - left + infoPanelWidth, j - top, Color.BLACK.getRGB());
324 }
325 else
326 {
327 img.setRGB(i - left + infoPanelWidth, j - top, convertToGray(data[i][j], shift, scale).getRGB());
328 }
329
330 if(datFile)
331 {
332 buf.putDouble(data[i][j]);
333 //buf.putFloat((float)data[i][j]);
334 }
335 }
336 if(datFile)
337 {
338 buf.putDouble(Double.MAX_VALUE);//end of line
339 //buf.putFloat(Float.MAX_VALUE);
340 }
341 }
342 drawHistogram(g);
343 //drawObstacles(g.create(), (offsetX - left) + infoPanelWidth, offsetY - top);
344 drawRobotPath(g.create(), (offsetX - left) + infoPanelWidth, offsetY - top);
345 drawControlPoints(g.create(), (offsetX - left) + infoPanelWidth, offsetY - top);
346 drawLegend(g.create());
347
348 if(datFile)
349 {
350 try
351 {
352 FileOutputStream ostream = new FileOutputStream(path.substring(0, path.length() - 3).concat("dat"), false);
353 GZIPOutputStream str = new GZIPOutputStream(ostream);
354 str.write(buf.array(), 0, buf.array().length);
355 str.finish();
356 str.close();
357 ostream.close();
358 }
359 catch(IOException ex)
360 {
361 Logger.getLogger(ScanPreview.class.getName()).log(Level.SEVERE, null, ex);
362 }
363 }
364
365 return img;
366 }
367
368 /**
369 * Draws a histogram of used colors(shades of gray) at [10,10] 255px wide,
370 * 100px tall.
371 *
372 * @param g Graphics to draw histogram to.
373 */
374 private void drawHistogram(Graphics2D g)
375 {
376 int max = ToolBox.getMax(histogram);
377
378 g.setPaint(Color.GREEN);
379 Shape rect = new Rectangle2D.Double(9, 9, 257, 102);
380 g.fill(rect);
381 for(int i = 0; i < histogram.length; i++)
382 {
383 int count = histogram[i];
384 drawImageLine(i + 10, 10, i + 10, (double) count * 100 / (double) max + 10, new Color(i, i, i), g);
385 }
386 }
387
388 /**
389 * Draws points and meaning of it to the final ouptput map.
390 *
391 * @param g Graphics to draw data to.
392 */
393 private void drawLegend(Graphics g)
394 {
395 Location corner = new Location(20, 140);
396 int spacing = 20;
397 int yOffset = 0;
398 drawStartPose(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
399 drawString(g, "Landing/Recharging Point", (int) corner.x + spacing, (int) corner.y + yOffset);
400 yOffset += 25;
401 drawRechargePoint(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
402 drawString(g, "Recharge Needed Location", (int) corner.x + spacing, (int) corner.y + yOffset);
403 yOffset += 25;
404 drawDiversion(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
405 drawString(g, "Diversion Point", (int) corner.x + spacing, (int) corner.y + yOffset);
406 yOffset += 25;
407 drawHighRisk(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
408 drawString(g, "HighRisk Situation", (int) corner.x + spacing, (int) corner.y + yOffset);
409 yOffset += 25;
410 drawLowRisk(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
411 drawString(g, "LowRisk Situation", (int) corner.x + spacing, (int) corner.y + yOffset);
412 yOffset += 25;
413 drawPathDot(g, Location.ZERO, (int) corner.x, (int) corner.y + yOffset);
414 drawString(g, "Robot Path Point", (int) corner.x + spacing, (int) corner.y + yOffset);
415 yOffset += 25;
416 g.setColor(Color.WHITE);
417 drawData(g, (int) corner.x + spacing, (int) corner.y + yOffset);
418
419 }
420
421 /**
422 * Final information about the scanning is written by this method.
423 *
424 * @param g Graphics to draw data to.
425 * @param x X offset.
426 * @param y Y offset.
427 */
428 private void drawData(Graphics g, int x, int y)
429 {
430 g.setColor(Color.WHITE);
431 for(String line : postInfo.split("\n"))
432 {
433 g.drawString(line, x, y += g.getFontMetrics().getHeight());
434 }
435 }
436
437 // private void drawObstacles(Graphics g, int offsetX, int offsetY)
438 // {
439 // g.setColor(Color.PINK);
440 // for(Iterator<Obstacle1> it = obstacles.iterator(); it.hasNext();)
441 // {
442 // Obstacle1 obstacle = it.next();
443 // g.drawRect((int) (obstacle.getCorner().x * drawScale) + offsetX, (int) (obstacle.getCorner().y * drawScale) + offsetY, (int) (obstacle.getWidth() * drawScale), (int) (obstacle.getHeight() * drawScale));
444 // }
445 // }
446 /**
447 * Drawing of points that testify the robot situations.
448 *
449 * @param g Graphics to draw to.
450 * @param offsetX X offset.
451 * @param offsetY Y offset.
452 */
453 private void drawControlPoints(Graphics g, int offsetX, int offsetY)
454 {
455 for(Location location : robotRechargePoints)
456 {
457 drawRechargePoint(g, location, offsetX, offsetY);
458 }
459 for(Location location : robotDiversions)
460 {
461 drawDiversion(g, location, offsetX, offsetY);
462 }
463 for(Location location : robotStartPoints)
464 {
465 drawStartPose(g, location, offsetX, offsetY);
466 }
467 for(Location location : robotLowRiskPoints)
468 {
469 drawLowRisk(g, location, offsetX, offsetY);
470 }
471 for(Location location : robotHighRiskPoints)
472 {
473 drawHighRisk(g, location, offsetX, offsetY);
474 }
475 }
476
477 /**
478 * Every recorded point is drawn to show where the robot was moving
479 *
480 * @param g Graphics to draw to.
481 * @param offsetX X offset.
482 * @param offsetY Y offset.
483 */
484 private void drawRobotPath(Graphics g, int offsetX, int offsetY)
485 {
486 for(Location location : robotPath)
487 {
488 drawPathDot(g, location, offsetX, offsetY);
489 }
490 }
491
492 /**
493 * Saves an image to file system at specified path.
494 *
495 * @param img Image to save.
496 * @param path Path to save image to.
497 */
498 public void saveImage(BufferedImage img, File outputfile)
499 {
500 try
501 {
502 if(outputfile.createNewFile())
503 {
504 ImageIO.write(img, "png", outputfile);
505 }
506 else
507 {
508 System.out.println("Couldnt creaate file");
509 }
510 }
511 catch(IOException e)
512 {
513 System.out.println(e.getCause());
514 }
515 }
516 private final int minWidth = 200;
517 private final int minHeight = 350;
518 int[] histogram = new int[256];
519 double dMax = Double.MIN_VALUE;
520 double dMin = Double.MAX_VALUE;
521 public Point translate;
522 public Record record;
523 public Record recordPrev;
524 Rotation rot;// = new Rotation(0,0,0);
525 Rotation rotPrev;// = new Rotation(0,0,0);
526 Location loc;// = new Location(0,0,0);
527 Location locPrev;// = new Location(0,0,0);
528 int offset = 0;
529 List<Obstacle1> obstacles = new ArrayList<Obstacle1>();
530 List<Location> robotPath = new ArrayList<Location>();
531 List<Location> robotDiversions = new ArrayList<Location>();
532 List<Location> robotRechargePoints = new ArrayList<Location>();
533 List<Location> robotStartPoints = new ArrayList<Location>();
534 List<Location> robotHighRiskPoints = new ArrayList<Location>();
535 List<Location> robotLowRiskPoints = new ArrayList<Location>();
536 Collection<Double> sonars;
537 private String actInfo = "";
538 private String postInfo = "";
539 private final double rangeLimit = 19.9;
540 private final int drawScale = 6;
541 private final int panelWidth = 45 * drawScale;
542 //private final int panelHeight = 70 * drawScale;
543 private final Point2D pos = new Point((int) (panelWidth / 2), 50);
544 private final List<Double> highRisk = new ArrayList<Double>()
545 {
546
547 {
548 add(0.47d);
549 add(0.61d);
550 add(0.86d);
551 add(1.22d);
552 add(1.5d);
553 add(1.22d);
554 add(0.86d);
555 add(0.61d);
556 add(0.47d);
557 }
558 };
559 private final List<Double> lowRisk = new ArrayList<Double>()
560 {
561
562 {
563 add(1.57d);
564 add(1.91d);
565 add(2.72d);
566 add(3.5d);
567 add(4.5d);
568 add(3.5d);
569 add(2.72d);
570 add(1.91d);
571 add(1.57d);
572 }
573 };
574 private final int startSize = 500;
575 BufferedImage tmp = new BufferedImage(startSize, startSize, BufferedImage.TYPE_INT_ARGB);
576 Graphics2D tmpG = tmp.createGraphics();
577 double[][] data;
578
579 /**
580 * Red is for invalid range, Blue is for valid range and magenta is for the
581 * rays from which the altitude of the robot is estimated.
582 *
583 * @param range Range of a ray to draw.
584 * @param index Index of a ray to draw.
585 * @return Returns either one of Magenta, Blue and Red according to input
586 * properties.
587 */
588 private Color getScanColor(double range, int index)
589 {
590 if(index >= offset + 85 && index <= offset + 95)
591 {
592 return Color.MAGENTA;
593 }
594 else if(range <= rangeLimit)
595 {
596 return Color.BLUE;
597 }
598 else
599 {
600 return Color.RED;
601 }
602 }
603 Image backBuffer;
604
605 /**
606 * Checking the second buffer for resizing and that it exists.
607 */
608 private void checkOffscreenImage()
609 {
610 Dimension d = getSize();
611 if(backBuffer == null || backBuffer.getWidth(null) != d.width
612 || backBuffer.getHeight(null) != d.height)
613 {
614 backBuffer = createImage(d.width, d.height);
615 }
616 }
617
618 /**
619 * Double buffered system of repainting takes place here.
620 *
621 * @param sharpGraphics Graphics to show.
622 */
623 @Override
624 public void paint(Graphics sharpGraphics)
625 {
626 //super.paint(g);
627 Dimension d = getSize();
628 checkOffscreenImage();
629 Graphics backGraphics = backBuffer.getGraphics();
630 backGraphics.setColor(getBackground());
631 backGraphics.fillRect(0, 0, d.width, d.height);
632 // Draw into the offscreen image.
633 paintSituation(backGraphics);
634 // Put the offscreen image on the screen.
635 sharpGraphics.drawImage(backBuffer, 0, 0, null);
636 }
637
638 /**
639 * For each ray from the Range scanner range list there is a height computed
640 * and recorded. For more coherent data between each ray there is one
641 * aproximated and between every two records there is one aproximated as
642 * well. So we have artificially doubled the resolution of the scanner and
643 * doubeled the sample rate. All auxiliary data are drawn also here.
644 *
645 * @param g Graphics to draw to.
646 */
647 private void paintSituation(Graphics g)
648 {
649 if(recordPrev == null)
650 {
651 return;
652 }
653 g.drawImage(tmp, translate.x, translate.y, null);
654
655
656 g.setColor(Color.white);
657 g.fillRect(0, 0, panelWidth, getSize().height);//(int) pos.getX() - panelWidth/2 * drawScale, (int) pos.getY() - panelWidth/2 * drawScale, (int) (2*panelWidth * drawScale), (int) (panelHeight * drawScale));
658
659 drawSonars(g);
660 drawInfo(g, 20, 450);
661
662
663 double angle = 90 + ((record.getFOV() / 2 - rot.roll) * 180 / Math.PI);//angle of the first ray(90°=straight down+FOV/2, but we have to count the robot pitch!)
664 double anglePrev = 90 + ((record.getFOV() / 2 - rotPrev.roll) * 180 / Math.PI);//angle of the first ray(90°=straight down+FOV/2, but we have to count the robot pitch!)
665
666 if(Math.abs(angle - anglePrev) > 180) //abychom mohli udělat aritmetickej průměr, jinak mám -178.9 a 178.8 třeba a výjde mi nesmysl. A to díky rotaci, která je napravo >0 a nalevo <6.28
667 {
668 anglePrev *= -1;
669 }
670 //Point2D pos=record.getPosition();
671 for(int i = 0; i < record.getRanges().size(); i++)
672 {
673 double range = record.getRanges().get(i);
674 double rangePrev = recordPrev.getRanges().get(i);
675 double value;
676 if(range < rangeLimit && rangePrev < rangeLimit)
677 {
678 value = (range + rangePrev) / 2;
679 }
680 else if(range < rangeLimit)
681 {
682 value = range;
683 }
684 else
685 {
686 value = rangePrev;
687 }
688
689 issueRay(g, i, range, angle, loc, rot);
690 issueRay(g, i, value, (anglePrev + angle) / 2, getMidPoint(loc, locPrev), getMidTurn(rot, rotPrev));
691 angle -= (record.getFOV() / Math.PI * 180 / record.getRanges().size()) / 2;
692 anglePrev -= (recordPrev.getFOV() / Math.PI * 180 / recordPrev.getRanges().size()) / 2;
693
694 if(i == 0)
695 {
696 continue;
697 }
698
699 angle -= (record.getFOV() / Math.PI * 180 / record.getRanges().size()) / 2;
700 anglePrev -= (recordPrev.getFOV() / Math.PI * 180 / recordPrev.getRanges().size()) / 2;
701
702 range = (record.getRanges().get(i) + record.getRanges().get(i - 1)) / 2;
703 rangePrev = (recordPrev.getRanges().get(i) + recordPrev.getRanges().get(i)) / 2;
704 value = (range + rangePrev) / 2;
705 issueRay(g, i - 1, range, angle, loc, rot);
706 issueRay(g, i - 1, value, (anglePrev + angle) / 2, getMidPoint(loc, locPrev), getMidTurn(rot, rotPrev));
707
708
709
710 //angle -= record.getFOV() / Math.PI * 180 / record.getRanges().size();
711 //anglePrev -= recordPrev.getFOV() / Math.PI * 180 / recordPrev.getRanges().size();
712
713 }
714
715 //tmpG.setColor(Color.RED);
716 //tmpG.drawOval((int) (drawScale * loc.x) + tmp.getWidth() / 2, (int) (drawScale * loc.y) + tmp.getHeight() / 2, 2, 2);
717 g.setColor(Color.GREEN);
718 //g.drawLine(0, 0, (int) (range * 10), (int) (range * 10));
719
720 //drawObstacles(g, this.getWidth() / 2, this.getHeight() / 2);
721
722 //this.drawLine(g, loc.x * 10 + Math.sin(rot.yaw) * 10 + 100, loc.y * 10 - Math.cos(rot.yaw) * 10 + 100, loc.x * 10 - Math.sin(rot.yaw) * 10 + 100, loc.y * 10 + Math.cos(rot.yaw) * 10 + 100);
723
724 }
725
726 /**
727 * Utilizes all possible information from one ray. Draws a situtaion in the
728 * left white stripe and records data to scanning preview. Ranges longer
729 * than 19.5 are ignoret for the range scanner has range of 20-noise.
730 *
731 * @param g Graphics to write to.
732 * @param index Index of the ray.
733 * @param range Range measured.
734 * @param angle Angle of the ray.
735 * @param loc Robot's location.
736 * @param rot Robot's orientation.
737 */
738 public void issueRay(Graphics g, int index, double range, double angle, Location loc, Rotation rot)
739 {
740 //TODO:mezi každýma dvěma body v lajně udělat jeden průměrovanej
741 double x = Math.cos(angle * Math.PI / 180) * range * drawScale;//distance from the center
742 double y = Math.sin(angle * Math.PI / 180) * range * drawScale;//distance from the aircraft
743 double xOffset = Math.sin(rot.pitch) * y;//offset due to yaw of an aircraft
744 double z = Math.cos(rot.pitch) * y;//real range
745 int height = (int) (loc.z * drawScale - z + 10 * drawScale) * 255 / 20 / (int) drawScale; //we have to be aware of the fact, that the aircraft can pitch, therefore we can get a height value greater than the maxRange of the laser scanner(which is the twenty)
746 //angle -= record.getFOV() / Math.PI * 180 / record.getRanges().size();
747
748 Color c = getScanColor(range, index);
749 this.drawLine(g, c, pos.getX(), pos.getY(), (pos.getX() - x), (pos.getY() + y));
750
751
752 // if(loc.z * drawScale - z > 5.8 * drawScale && range < rangeLimit)
753 // {
754 // addObstacle(loc.x - Math.sin(rot.yaw) * (x / drawScale) - Math.cos(rot.yaw) * xOffset / drawScale,
755 // loc.y + Math.cos(rot.yaw) * (x / drawScale) - Math.sin(rot.yaw) * xOffset / drawScale);
756 //// g.drawRect((int) ((loc.x-0.5) * drawScale - Math.sin(rot.yaw) * (x) - Math.cos(rot.yaw) * xOffset + this.getWidth() / 2),
757 //// (int) ((loc.y-0.5) * drawScale + Math.cos(rot.yaw) * (x) - Math.sin(rot.yaw) * xOffset + this.getHeight() / 2), drawScale, drawScale);
758 //// g.drawOval((int) (loc.x * drawScale - Math.sin(rot.yaw) * (x) - Math.cos(rot.yaw) * xOffset + this.getWidth() / 2),
759 //// (int) (loc.y * drawScale + Math.cos(rot.yaw) * (x) - Math.sin(rot.yaw) * xOffset + this.getHeight() / 2), 5, 5);
760 // }
761 height = Math.max(height, 0);
762 height = Math.min(height, 255);
763
764 if(range <= rangeLimit)
765 {
766 this.drawLine(g, new Color(height, height, height), pos.getX() - x, 40 * drawScale, pos.getX() - x, 40 * drawScale - (loc.z * drawScale - z));
767 this.drawPoint(tmpG, new Color(height, height, height),
768 loc.x * drawScale - Math.sin(rot.yaw) * (x) - Math.cos(rot.yaw) * xOffset + offsetX,
769 loc.y * drawScale + Math.cos(rot.yaw) * (x) - Math.sin(rot.yaw) * xOffset + offsetY);
770
771
772 //actual location + rotation coeficient of the point - yaw coeficient -which is xOffset
773 // g.setColor(new Color(height2, height2, height2));
774 // this.drawLine(tmpG, (loc.x+locPrev.x)/2 * drawScale - Math.sin((rotPrev.yaw+rot.yaw)/2) * (x+x2)/2 - Math.cos((rot.yaw+rotPrev.yaw)/2) *(xOffset+xOffset2)/2 + tmpG.getWidth()/2,
775 // (locPrev.y+loc.y)/2 * drawScale + Math.cos((rotPrev.yaw+rot.yaw)/2) * (x+x2)/2 - Math.sin((rot.yaw+rotPrev.yaw)/2) *(xOffset+xOffset2)/2+ tmpG.getHeight()/2,
776 // (locPrev.x+loc.x)/2 * drawScale - Math.sin((rot.yaw+rotPrev.yaw)/2) * (x+x2)/2 - Math.cos((rot.yaw+rotPrev.yaw)/2) *(xOffset+xOffset2)/2+ tmpG.getWidth()/2,
777 // (locPrev.y+loc.y)/2 * drawScale + Math.cos((rot.yaw+rotPrev.yaw)/2) * (x+x2)/2 - Math.sin((rot.yaw+rotPrev.yaw)/2) *(xOffset+xOffset2)/2+ tmpG.getHeight()/2);
778 setDataAt(loc.x * drawScale - Math.sin(rot.yaw) * (x) - Math.cos(rot.yaw) * xOffset + offsetX,
779 loc.y * drawScale + Math.cos(rot.yaw) * (x) - Math.sin(rot.yaw) * xOffset + offsetY, loc.z * drawScale - z);
780
781 }
782 }
783
784 /**
785 * Computes a location that is between two input ones.
786 *
787 * @param locA First Location.
788 * @param locB Second Location.
789 * @return Returns location that is between locA and locB.
790 */
791 private Location getMidPoint(Location locA, Location locB)
792 {
793 double x = locA.x + locB.x;
794 double y = locA.y + locB.y;
795 double z = locA.z + locB.z;
796 return new Location(x / 2, y / 2, z / 2);
797 }
798
799 /**
800 * The angle is 0-2PI and this takes into account the closest angle possible
801 * from a circle.
802 *
803 * @param value1 First angle.
804 * @param value2 Second angle.
805 * @return Returns angle between value1 and value2
806 */
807 private double getMidAngle(double value1, double value2)
808 {
809 double val;
810 if(value1 > 6 && value2 < 0.28)
811 {
812 val = (value1 - 2 * Math.PI + value2) / 2;
813 }
814 else if(value2 > 6 && value1 < 0.28)
815 {
816 val = (value2 - 2 * Math.PI + value1) / 2;
817 }
818 else
819 {
820 val = (value1 + value2) / 2;
821 }
822
823 if(val < 0)
824 {
825 val += 2 * Math.PI;
826 }
827 return val;
828 }
829
830 /**
831 * Returns a rotation that is between the specified input ones.
832 *
833 * @param rotA First rotation.
834 * @param rotB Second rotation.
835 * @return Returns rotation between rotA and rotB.
836 */
837 private Rotation getMidTurn(Rotation rotA, Rotation rotB)
838 {
839 double pitch = getMidAngle(rotA.pitch, rotB.pitch);
840 double yaw = getMidAngle(rotA.yaw, rotB.yaw);
841 double roll = getMidAngle(rotA.roll, rotB.roll);
842 return new Rotation(pitch, yaw, roll);
843 }
844
845 /**
846 * Draws information string to a grahpics.
847 *
848 * @param g Graphics to use.
849 * @param x X coordinate.
850 * @param y Y coordinate.
851 */
852 private void drawInfo(Graphics g, int x, int y)
853 {
854 g.setColor(Color.BLACK);
855 for(String line : actInfo.split("\n"))
856 {
857 g.drawString(line, x, y += g.getFontMetrics().getHeight());
858 }
859 }
860
861 /**
862 * Converts Height to corresponding shade of gray.
863 *
864 * @param data Height data to convert.
865 * @param offset Offset of data(can be negative).
866 * @param scale Difference between max and min height.
867 * @return
868 */
869 private Color convertToGray(double data, double offset, double scale)
870 {
871 int height = (int) ((offset + data) * scale);
872 height = Math.max(0, height);
873 height = Math.min(255, height);
874 histogram[height]++;
875 return new Color(height, height, height);
876 }
877
878 /**
879 * Sets a height to the array of heights. When overwriting existing value it
880 * approximates both values.
881 *
882 * @param x X coordinate.
883 * @param y Y coordinate.
884 * @param data Recorded height - double number to write.
885 */
886 private void setDataAt(double x, double y, double data)
887 {
888 if(data > dMax)
889 {
890 dMax = data;
891 }
892 if(data < dMin)
893 {
894 dMin = data;
895 }
896 //pokud tam nic neni, tak zapiš, jinak udělej průměr z obou hodnot.
897 //double prev = this.data[(int) x][(int) y];
898 if(this.data[(int) x][(int) y] == Double.MIN_VALUE)
899 {
900 this.data[(int) x][(int) y] = data;
901 }
902 else
903 {
904 this.data[(int) x][(int) y] = (this.data[(int) x][(int) y] + data) / 2;
905 }
906 }
907
908 /**
909 * Specifies the style of writing that appears on the preview form and on
910 * the final output image.
911 *
912 * @param g Graphics to use.
913 * @param text String to write.
914 * @param x X offset.
915 * @param y Y offset.
916 */
917 private void drawString(Graphics g, String text, int x, int y)
918 {
919 g.drawChars(text.toCharArray(), 0, text.length(), x, y + g.getFontMetrics().getHeight() / 4);
920 }
921
922 /**
923 * Specifies style of point to represent a point that is computed when
924 * evading obstacles.
925 *
926 * @param g Graphics to use
927 * @param diversion Center of the point to draw
928 * @param offsetX Offset on x axis.
929 * @param offsetY Offset on y axis.
930 */
931 private void drawDiversion(Graphics g, Location diversion, int offsetX, int offsetY)
932 {
933 g.setColor(Color.BLUE);
934 g.fillRect((int) (diversion.x * drawScale + offsetX) - 2, (int) (diversion.y * drawScale + offsetY) - 2, 5, 5);
935 }
936
937 /**
938 * Specifies style of point to represent a start position of the robot.
939 *
940 * @param g Graphics to use
941 * @param startPose Center of the point to draw
942 * @param offsetX Offset on x axis.
943 * @param offsetY Offset on y axis.
944 */
945 private void drawStartPose(Graphics g, Location startPose, int offsetX, int offsetY)
946 {
947 g.setColor(Color.MAGENTA);
948 g.drawOval(offsetX + (int) (startPose.x * drawScale) - 10, offsetY + (int) (startPose.y * drawScale) - 10, 20, 20);
949 // g.drawOval(offsetX + (int) (startPose.x * drawScale) - 10, offsetY + (int) (startPose.y * drawScale) - 10, 20, 20);
950 // g.drawOval(offsetX + (int) (startPose.x * drawScale) - 9, offsetY + (int) (startPose.y * drawScale) - 9, 18, 18);
951 // g.drawOval(offsetX + (int) (startPose.x * drawScale) - 8, offsetY + (int) (startPose.y * drawScale) - 8, 16, 16);
952 }
953
954 /**
955 * Specifies style of point to represent a point where the robot was
956 * interrupted by low battery and a need to recharge.
957 *
958 * @param g Graphics to use
959 * @param rechargePoint Center of the point to draw
960 * @param offsetX Offset on x axis.
961 * @param offsetY Offset on y axis.
962 */
963 private void drawRechargePoint(Graphics g, Location rechargePoint, int offsetX, int offsetY)
964 {
965 g.setColor(Color.YELLOW);
966 g.fillRect(offsetX + (int) (rechargePoint.x * drawScale) - 3, offsetY + (int) (rechargePoint.y * drawScale) - 3, 6, 6);
967 //g.fillRect(offsetX + (int) (rechargePoint.x * drawScale) - 5, offsetY + (int) (rechargePoint.y * drawScale) - 5, 10, 10);
968 }
969
970 /**
971 * Specifies style of point to represent a low situation location of the
972 * robot.
973 *
974 * @param g Graphics to use
975 * @param lowRisk Center of the point to draw
976 * @param offsetX Offset on x axis.
977 * @param offsetY Offset on y axis.
978 */
979 private void drawLowRisk(Graphics g, Location lowRisk, int offsetX, int offsetY)
980 {
981 g.setColor(Color.orange);
982 g.drawOval(offsetX + (int) (lowRisk.x * drawScale) - 1, offsetY + (int) (lowRisk.y * drawScale) - 1, 3, 3);
983 //g.fillOval(offsetX + (int) (lowRisk.x * drawScale) - 3, offsetY + (int) (lowRisk.y * drawScale) - 3, 6, 6);
984 }
985
986 /**
987 * Specifies style of point to represent a high situation location of the
988 * robot.
989 *
990 * @param g Graphics to use
991 * @param highRisk Center of the point to draw
992 * @param offsetX Offset on x axis.
993 * @param offsetY Offset on y axis.
994 */
995 private void drawHighRisk(Graphics g, Location highRisk, int offsetX, int offsetY)
996 {
997 g.setColor(Color.red);
998 g.drawOval(offsetX + (int) (highRisk.x * drawScale) - 2, offsetY + (int) (highRisk.y * drawScale) - 2, 4, 4);
999 //g.fillOval(offsetX + (int) (highRisk.x * drawScale) - 4, offsetY + (int) (highRisk.y * drawScale) - 4, 8, 8);
1000 }
1001
1002 /**
1003 * Specifies style of point to represent a path of the robot.
1004 *
1005 * @param g Graphics to use
1006 * @param dot Center of the point to draw
1007 * @param offsetX Offset on x axis.
1008 * @param offsetY Offset on y axis.
1009 */
1010 private void drawPathDot(Graphics g, Location dot, int offsetX, int offsetY)
1011 {
1012 g.setColor(Color.GREEN);
1013 g.fillOval(offsetX + (int) (dot.x * drawScale) - 1, offsetY + (int) (dot.y * drawScale) - 1, 3, 3);
1014 //g.fillOval(offsetX + (int) (dot.x * drawScale) - 3, offsetY + (int) (dot.y * drawScale) - 3, 6, 6);
1015 }
1016
1017 /**
1018 * Draws state of Sonar sensors somewhere in the middle of the white stripe.
1019 * Blue column means NORISK, Green column means LOWRISK and Red means
1020 * HIGHRISK threat.
1021 *
1022 * @param g Graphics to use.
1023 */
1024 private void drawSonars(Graphics g)
1025 {
1026 if(sonars == null)
1027 {
1028 return;
1029 }
1030 int count = 0;
1031 int scale = (panelWidth - 2 * drawScale) / (sonars.size());
1032 for(Double sonar : sonars)
1033 {
1034 if(sonar < highRisk.get(count))
1035 {
1036 g.setColor(Color.red);
1037 }
1038 else if(sonar < lowRisk.get(count))
1039 {
1040 g.setColor(Color.green);
1041 }
1042 else
1043 {
1044 g.setColor(Color.BLUE);
1045 }
1046 g.fillRect(drawScale * 2 + count * scale, 350, scale - 1, (int) (sonar.doubleValue() * drawScale * 2));
1047 count++;
1048 }
1049 }
1050
1051 /**
1052 * Draws a line into the preview bitmap.
1053 *
1054 * @param x1 Start X coordinate.
1055 * @param y1 Start Y coordinate
1056 * @param x2 End X coordinate.
1057 * @param y2 End Y coordinate.
1058 * @param c Color of the line
1059 * @param g Graphics to use.
1060 */
1061 private void drawImageLine(double x1, double y1, double x2, double y2, Color c, Graphics2D g)
1062 {
1063 Line2D line = new Line2D.Double(x1, y1, x2, y2);
1064 g.setColor(c);
1065 g.draw(line);
1066 }
1067
1068 /**
1069 * Draws a line to input graphics with specified color.
1070 *
1071 * @param g Graphics to use.
1072 * @param c Color of the line.
1073 * @param x1 Start X coordinate.
1074 * @param y1 Start Y coordinate
1075 * @param x2 End X coordinate.
1076 * @param y2 End Y coordinate.
1077 */
1078 private void drawLine(Graphics g, Color c, double x1, double y1, double x2, double y2)
1079 {
1080 g.setColor(c);
1081 g.drawLine((int) x1, (int) y1, (int) x2, (int) y2);
1082 }
1083
1084 /**
1085 * Draws point to input graphics with sepcified color.
1086 *
1087 * @param g Graphics to use.
1088 * @param c Color of the point.
1089 * @param x X coordinate.
1090 * @param y Y coordinate.
1091 */
1092 private void drawPoint(Graphics g, Color c, double x, double y)
1093 {
1094 g.setColor(c);
1095 int xn = (int) x - offsetX;
1096 int yn = (int) y - offsetY;
1097 if(keepDataArrayGreat((int) x, (int) y))
1098 {//old raw values plus new fresh offsets
1099 g.drawLine(xn + offsetX, yn + offsetY, xn + offsetX, yn + offsetY);
1100 }
1101 else
1102 {
1103 g.drawLine((int) x, (int) y, (int) x, (int) y);
1104 }
1105 }
1106
1107 /**
1108 * [x,y] is inside some obstacle -> return obstacle can not be extended ->
1109 * take another obstacle that can try to extend it, if extended obstacle !=
1110 * null return(succesfully extended)
1111 *
1112 * @param x X coordinate.
1113 * @param y Y coordinate.
1114 */
1115 private void addObstacle(double x, double y)
1116 {
1117 for(Obstacle1 obstacle : obstacles)
1118 {
1119 if(obstacle.isWithin(x, y))
1120 {
1121 return;
1122 }
1123 if(!obstacle.canExtend())
1124 {
1125 continue;
1126 }
1127 Obstacle1 newObstacle = obstacle.tryExtend(x, y);
1128 if(newObstacle != null)
1129 {
1130 return;
1131 }
1132 }
1133 obstacles.add(new Obstacle1(x, y));
1134 }
1135 //<editor-fold defaultstate="collapsed" desc="Setters - methods used to fill Collections and variables used to display current and past situation">
1136
1137 /**
1138 * The Robot keeps its altitude based on an avarage value of ten rays below
1139 * the robot. The <B>offset</B> is computed based on Robots roll, so if the
1140 * Robot gets itself to a tilt position it needs to be able to correctly
1141 * estimate the rays heading straight towards the ground.
1142 *
1143 * @param offset Index of the first ray of the bunch which are used to
1144 * estimate an altitude of the Robot
1145 */
1146 public void setOffset(int offset)
1147 {
1148 this.offset = offset;
1149 }
1150
1151 /**
1152 * Sets the sonar data that bear an information about what is going on in
1153 * front of the Robot.
1154 *
1155 * @param sonars Collection of double values sorted from left to right.
1156 */
1157 public void setSonars(Collection<Double> sonars)
1158 {
1159 this.sonars = sonars;
1160 }
1161
1162 /**
1163 * Adds a diversion point to local collection of Diversion Point.
1164 *
1165 * @param diversion Location of a point that pulls the Robot out of crisis
1166 * sitation.
1167 */
1168 public void setDivPoint(Location diversion)
1169 {
1170 robotDiversions.add(diversion);
1171 drawDiversion(tmpG, diversion, offsetX, offsetY);
1172 }
1173
1174 /**
1175 * Assd a start location point to local collection of Start Locations.
1176 *
1177 * @param start A location that signifies where the robot started from.
1178 */
1179 public void setStartLocation(Location start)
1180 {
1181 robotStartPoints.add(start);
1182 drawStartPose(tmpG, start, offsetX, offsetY);
1183 }
1184
1185 /**
1186 * Adds a point to local collection of Battery_needed Points.
1187 *
1188 * @param breakPoint Location of a point when the scanning process was
1189 * interrupted by low battery level.
1190 *
1191 */
1192 public void setRechargeBreakPoint(Location breakPoint)
1193 {
1194 robotRechargePoints.add(breakPoint);
1195 drawRechargePoint(tmpG, breakPoint, offsetX, offsetY);
1196 }
1197
1198 /**
1199 * Adds a point to local collection of High risk situation points.
1200 *
1201 * @param highRisk Location of a point where the robot was in high risk
1202 * situation.
1203 */
1204 public void setHighRiskPoint(Location highRisk)
1205 {
1206 robotHighRiskPoints.add(highRisk);
1207 drawHighRisk(tmpG, highRisk, offsetX, offsetY);
1208 }
1209
1210 /**
1211 * Adds a point to local collection of Low risk situation points.
1212 *
1213 * @param lowRisk Location of a point where the robot was in low risk
1214 * situation.
1215 */
1216 public void setLowRiskPoint(Location lowRisk)
1217 {
1218 robotLowRiskPoints.add(lowRisk);
1219 drawLowRisk(tmpG, lowRisk, offsetX, offsetY);
1220 }
1221
1222 /**
1223 * Adds a record of location and list of ranges from Range scanner.
1224 *
1225 * @param rec Object from the robot's sensor estimating location and ranges
1226 * of Range scanner.
1227 */
1228 public void setRecord(Record rec)///*objekt kterej má v sobě 2D pozici[x,y] a hodnoty laserscanneru*/)
1229 {
1230 recordPrev = record;
1231 record = rec;
1232 }
1233
1234 /**
1235 * Actualizes an info set that will be inserted to the output image.
1236 *
1237 * @param info Set of name/value pairs to write to the output image.
1238 */
1239 public void setPostInfo(String info)
1240 {
1241 this.postInfo = info;
1242 }
1243
1244 /**
1245 * Flag for saving the dat fila along with the output image
1246 *
1247 * @param write True to save a *.dat file, false to not.
1248 */
1249 public void setDatFile(boolean write)
1250 {
1251 this.datFile = write;
1252 }
1253
1254 /**
1255 * Invalidates the form.
1256 */
1257 public void refreshGraphics()
1258 {
1259 this.update(this.getGraphics());
1260 }
1261
1262 /**
1263 * Actualizes an info set that will be inserted to the white stripe on the
1264 * preview form.
1265 *
1266 * @param text Set of name/value pairs to write to the white stripe on the
1267 * preview form.
1268 */
1269 public void setInfo(String text)
1270 {
1271 this.actInfo = text;
1272 }
1273
1274 /**
1275 * Sets the estimated rotation of the robot.
1276 *
1277 * @param rot Orientation of the robot
1278 */
1279 public void setOrientation(Rotation rot)
1280 {
1281 rotPrev = this.rot;
1282 this.rot = rot;
1283 }
1284 private int everyOther = 0;
1285
1286 /**
1287 * Sets the estimated locatio nof the robot.
1288 *
1289 * @param loc Location of the robot.
1290 */
1291 public void setLocation(Location loc)
1292 {
1293 locPrev = this.loc;
1294 this.loc = loc;
1295 drawPathDot(tmpG, loc, offsetX, offsetY);
1296 if(everyOther > 10)
1297 {
1298 everyOther = 0;
1299 robotPath.add(loc);
1300 }
1301 else
1302 {
1303 everyOther++;
1304 }
1305 }
1306 //</editor-fold>
1307 // Variables declaration - do not modify//GEN-BEGIN:variables
1308 private javax.swing.JLabel jLabel1;
1309 // End of variables declaration//GEN-END:variables
1310 }