BallDetector(boolean _display) { ctx = Freenect.createContext(); if (ctx.numDevices() > 0) { kinect = ctx.openDevice(0); } else { System.err.println("WARNING: No kinects detected"); return; } display = _display; controlFrame = new JFrame("Controls"); controlFrame.setLayout(new GridLayout(5, 1)); controlFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); pg = new ParameterGUI(); pg.addIntSlider("maxDepth", "max depth", 800, 2047, 1050); pg.addIntSlider("blobThresh", "blob thresh", 1, 500, 125); pg.addIntSlider("thresh", "thresh", 1, 100, 10); pg.addIntSlider("frames", "frames", 1, 1000, 1); pg.addListener( new ParameterListener() { public void parameterChanged(ParameterGUI _pg, String name) { if (name.equals("thresh")) { KinectDepthVideo.THRESH = _pg.gi(name); } else if (name.equals("frames")) { KinectDepthVideo.MAX_FRAMES = _pg.gi(name); } else if (name.equals("maxDepth")) { KinectDepthVideo.MAX_DEPTH = _pg.gi(name); } } }); controlFrame.add(pg, 0, 0); startTracking = new JButton("Start Tracking Balls"); startTracking.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { if (!tracking) { tracking = true; colorStream.pause(); depthStream.pause(); startTracking.setText("Stop Tracking"); } else { tracking = false; colorStream.resume(); depthStream.resume(); startTracking.setText("Start Tracking Balls"); } } }); controlFrame.add(startTracking, 1, 0); resetProjectile = new JButton("Reset Projectile"); resetProjectile.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { try { lcm.publish("6_RESET", "reset"); } catch (IOException ex) { System.out.println("can't publish reset"); } } }); controlFrame.add(resetProjectile, 2, 0); resetDepth = new JButton("Reset Depth Avgs"); resetDepth.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { DepthClearer ic = new DepthClearer(pg); ic.start(); } }); controlFrame.add(resetDepth, 3, 0); JPanel scoreButtons = new JPanel(new GridLayout(1, 3)); JButton addHuman = new JButton("human++"); addHuman.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { try { lcm.publish("6_SCORE_HUMAN", "bish"); } catch (IOException ex) { System.out.println("can't publish score"); } } }); JButton addRobot = new JButton("robot++"); addRobot.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { try { lcm.publish("6_SCORE_ROBOT", "bish"); } catch (IOException ex) { System.out.println("can't publish score"); } } }); JButton resetScores = new JButton("reset scores"); resetScores.addActionListener( new ActionListener() { public void actionPerformed(ActionEvent e) { try { lcm.publish("6_SCORE_RESET", "bish"); } catch (IOException ex) { System.out.println("can't publish score"); } } }); scoreButtons.add(addHuman, 0, 0); scoreButtons.add(addRobot, 0, 1); scoreButtons.add(resetScores, 0, 2); controlFrame.add(scoreButtons, 4, 0); controlFrame.setSize(800, 600); controlFrame.setVisible(true); colorFrame = new JFrame("color feed"); colorMonitor = new Object(); colorStream = new KinectRGBVideo(kinect, colorMonitor, display); colorFrame.setDefaultCloseOperation(JFrame.DO_NOTHING_ON_CLOSE); colorFrame.addWindowListener(new RGBClose()); colorFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT); colorFrame.setContentPane(colorStream); colorFrame.setVisible(true); depthFrame = new JFrame("depth feed"); depthMonitor = new Object(); depthStream = new KinectDepthVideo(kinect, depthMonitor, display); depthFrame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); depthFrame.setSize(KinectVideo.WIDTH, KinectVideo.HEIGHT); depthFrame.setContentPane(depthStream); depthFrame.setVisible(true); rgbImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB); depthImg = new BufferedImage(640, 480, BufferedImage.TYPE_INT_ARGB); validImageValue = new boolean[KinectVideo.WIDTH * KinectVideo.HEIGHT]; try { lcm = new LCM("udpm://239.255.76.67:7667?ttl=1"); } catch (IOException e) { lcm = LCM.getSingleton(); } BALL = new Statistics(); finder = new BallTracker(KinectVideo.WIDTH, KinectVideo.HEIGHT, false); if (display) { depthImg = depthStream.getFrame(); rgbImg = colorStream.getFrame(); } // get robot position from click depthStream.addMouseListener( new MouseAdapter() { public void mouseClicked(MouseEvent e) { Point botPix = e.getPoint(); botStart = depthStream.getWorldCoords(botPix); botStart.z += 0.08; System.out.println("botStart: " + botStart.toString()); depthStream.showSubtraction(); depthStream.botLoc = botPix; } }); DepthClearer ic = new DepthClearer(pg); ic.start(); }
public void run() { while (true) { BALL = null; ball_t ballLCM = new ball_t(); synchronized (depthMonitor) { try { depthMonitor.wait(); } catch (Exception e) { e.printStackTrace(); } } ballLCM.nanoTime = depthStream.latestTimestamp; ArrayList<Statistics> blobs; depthStream.getReadLock().lock(); try { blobs = finder.analyze2(depthStream.getValidImageArray()); } finally { depthStream.getReadLock().unlock(); } Statistics ball = null; Statistics robot = null; int minSize = pg.gi("blobThresh"); // find robot and ball by blob size Collections.sort(blobs, ComparatorFactory.getStatisticsCompareSize()); // find robot and ball by y pixel // Collections.sort(blobs, ComparatorFactory.getStatisticsCompareYPix()); // if (tracking) { // System.out.println("num blobs: " + blobs.size()); // System.out.println("biggest blob size: " + blobs.get(0).N); // } // for (Statistics blob : blobs) { // if (blob.N > 10) { // System.out.println("blob size: " + blob.N); // } // else { // break; // } // } if (blobs.size() == 1) { Statistics first = blobs.get(0); if (first.N > minSize) { ball = first; } } else if (blobs.size() >= 2) { Statistics first = blobs.get(0); Statistics second = blobs.get(1); if (first.N > minSize) { ball = first; } if (second.N > minSize) { robot = first; ball = second; } } // System.out.println("balls points " + depthStream.trajectory.size()); // if not tracking keep kv.depthStream.trajectory to just one index if (!tracking) { depthStream.trajectory.clear(); } if (ball != null) { depthStream.trajectory.add(ball); Point depthPix = ball.center(); Point depthCoord = new Point(); depthCoord.x = depthPix.x - KinectVideo.C_X; depthCoord.y = KinectVideo.C_Y - depthPix.y; // System.out.println("avg depth " + ball.Uz()); // get depth from average depth of blob double realDepth = raw_depth_to_meters(ball.Uz()); Point3D coord = depthStream.getWorldCoords(depthCoord, realDepth); if (depthPix != null) { for (int y = depthPix.y - 3; y < depthPix.y + 3; y++) { for (int x = depthPix.x - 3; x < depthPix.x + 3; x++) { try { depthImg.setRGB(x, y, 0xFFFF0000); } catch (Exception e) { // System.out.println(x + " " + y); } ; } } // if (tracking) { // //save image // depthStream.getReadLock().lock(); // try { // File imgFile = new File("image" + ballNum++ + ".png"); // try { // ImageIO.write(depthImg, "png", imgFile); // } // catch(Exception e) { // System.out.println("can't save img"); // } // } // finally { // depthStream.getReadLock().unlock(); // } // } if (tracking) { ballLCM.x = coord.x; ballLCM.y = coord.y; ballLCM.z = coord.z; // if(tracking) System.out.println("updating new ball (" + System.currentTimeMillis() + ")"); // if (ballLCM.x > CatchController.TARGET_MAX_X) lcm.publish("6_BALL", ballLCM); // else // System.out.println("ball past target zone"); } } } } }