123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113 |
- import java.awt.Color;
- import java.awt.Graphics;
- import java.util.ArrayList;
- import java.util.concurrent.Semaphore;
- import javax.swing.JPanel;
- import lejos.geom.Line;
- import lejos.robotics.mapping.LineMap;
- import lejos.robotics.navigation.Pose;
- public class ScannerImage extends JPanel {
- private Pose pose;
- private LineMap map;
- private ArrayList<SonarRead> lista_leituras;
- private Semaphore semaphore;
-
- class SonarRead {
- public double distance;
- public double ang;
- SonarRead (double distance, double ang) {
- this.ang = ang;
- this.distance = distance;
- }
- }
-
- @Override
- protected void paintComponent(Graphics g) {
- int w = this.getWidth()/2;
- int h = this.getHeight();
- int distance = h*2/25;
- g.setColor(new Color(0f, 0f, 0f, 0.4f));
- for (int i = 1; i <= 18; i++) {
- int r = distance * i;
- g.drawArc(w-r, h-r, 2*r, 2*r, 0, 180);
- g.drawString(new Integer(i*20).toString(), w-7, h-r);
- }
- g.setColor(new Color(0f, 0f, 0f, 0.5f));
- for (int i = 1; i < 6; i++) {
- int lw = (int)(Math.cos(Math.PI/6.0*i)*distance * 18);
- int lh = (int)(Math.sin(Math.PI/6.0*i)*distance * 18);
- g.drawLine(w, h, lw+w, h-lh);
- }
- g.setColor(new Color(0f, 0f, 0f, 0.1f));
- for (int i = 1; i < 12; i++) {
- int lw = (int)(Math.cos(Math.PI/12.0*i)*distance * 18);
- int lh = (int)(Math.sin(Math.PI/12.0*i)*distance * 18);
- g.drawLine(w, h, lw+w, h-lh);
- }
-
- if (semaphore.tryAcquire()) {
- double d = h*2.0/25.0/100;
-
- g.setColor(new Color(0f, 1f, 0f));
- if (map != null && pose!= null) {
- Line[] lines = map.getLines();
- for (int i = 0; i < lines.length; i++) {
- Line l = lines[i];
- g.drawLine(
- (int)(w+(l.x1-pose.getX())*d),
- (int)(h-(l.y1-pose.getY())*d),
- (int)(w+(l.x2-pose.getX())*d),
- (int)(h-(l.y2-pose.getY())*d)
- );
- }
- }
-
- drawDots(g, w, h);
- semaphore.release();
- }
- }
- private void drawDots(Graphics g, int w, int h) {
- g.setColor(new Color(1f, 0f, 0f));
- int oval_size = 30;
- int distance = h*2/25;
- double d = distance/20.0;
- double a = -oval_size/4;
- for (SonarRead r: lista_leituras) {
- double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
- double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
- x = w - x;
- y = h - y;
- g.fillOval((int)(x-oval_size/2.0), (int)(y-oval_size/2.0), oval_size/2, oval_size/2);
- }
- if (map == null) return;
- }
-
- public Pose getPose() {
- return pose;
- }
- public void addRead(Pose p, double distance, double ang) {
- if (semaphore.tryAcquire()) {
- if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY()
- && p.getHeading() == pose.getHeading())) {
- pose = new Pose(p.getX(), p.getY(), p.getHeading());
- lista_leituras.clear();
- }
- lista_leituras.add(new SonarRead(distance, ang+90));
- semaphore.release();
- }
- repaint();
- }
-
- public ScannerImage(LineMap map) {
- this.map = map;
- semaphore = new Semaphore(1);
- lista_leituras = new ArrayList<SonarRead>();
- }
- }
|