GUI for driving robot, display of robot pose, and vision images for * RSS-I.
* *Can either be run as a stand-alone program with the provided {@link * #main} (recommended), or you can make an instance of it from within your own * program.
* *A coordinate grid is displayed representing the robot world frame. The X * axis is horizontal and positive to the right. The Y axis is vertical and * positive up. The X and Y axis lines, when in view, are displayed in a * highlighted color. Other grid lines are plotted at 1m intervals.
* *The graphics display is interactively navigable with the mouse.
* *To pan: click and drag (left button).
* *To zoom: *
The most recently plotted pose will always be displayed in a highlighted * color. A fraction of all the older poses (controlled by {@link * #poseSaveInterval}) will also be plotted, in a lighter color, to provide * you with a representation of the history of the robot's motion.
* *When run stand-alone, connects to the Carmen daemons directly and * periodically plots the robot pose. Vision images are displayed in the * upper-left corner as they are received. See the documentation for {@link * #main} for the command line arguments and other details for stand-alone * operation.
* *In stand-alone mode you may clear all prior drawing like this: *
* (new GUIEraseMessage(VisionGUI.ERASE_POSE)).publish(); * (new GUIEraseMessage(VisionGUI.ERASE_POSE_HISTORY)).publish(); * (new GUIEraseMessage(VisionGUI.ERASE_VISION_IMAGE)).publish(); ** *
When run from within your own program, this class does not * automatically plot anything. If you wish to plot the robot pose, you will * need to call {@link #setRobotPose} as you receive odometry messages from * Carmen. Subscribe to the appropriate Carmen messages and call the other * drawing methods as you desire to plot other data.
* *Note that it is generally preferable design your code to run the GUI in * stand-alone mode so that you can run it on a different machine than the one * which runs your robot's operational code.
* *When run from within your own program, you will need to write code to * manually draw things in the GUI.
* *Drawing example: *
* VisionGUI vg = new VisionGUI();
*
* public void handle (OdometryMessage message) {
* vg.setRobotPose(message.x, message.y, message.theta);
* }
*
* public void handle (VisionImageMessage message) {
* vg.setVisionImage(message.image, message.width, message.height);
* }
*
*
* To clear all prior drawing: *
* vg.erasePoseHistory(); * vg.erasePose(); * vg.eraseVisionImage(); ** *
If {@link #maxTV} and {@link #maxRV} are both greater than zero then you * can drive the robot around using the keyboard: *
TBD: *
Node for ROS communication
*/ public Node node; /** *The application name.
**/ public static final String APPNAME = "VisionGUI"; /** *Erase all object types.
**/ public static final int ERASE_ALL = ~0; /** *Needed as JPanel is seralizable
**/ static final long serialVersionUID = 42; /** *Bitfield constant for {@link GUIEraseMessage}.
**/ public static final int ERASE_POSE = 1<<0; /** *Bitfield constant for {@link GUIEraseMessage}.
**/ public static final int ERASE_POSE_HISTORY = 1<<1; /** *Bitfield constant for {@link GUIEraseMessage}.
**/ public static final int ERASE_VISION_IMAGE = 1<<2; /** *Coordinate grid line spacing in meters.
**/ public static final double GRID_SPACING = 1.0; /** *Line width of the lines making up the coordinate grid in pixels.
**/ public static final float GRID_LINE_WIDTH = 1.0f; /** *Line width of axes lines in pixels.
**/ public static final float AXES_LINE_WIDTH = 1.5f; /** *Line width of the lines making up a {@link VisionGUI.Pose} in * pixels.
**/ public static final float POSE_LINE_WIDTH = 1.5f; /** *Default color for the coordinate system grid.
**/ public static final Color GRID_COLOR = Color.LIGHT_GRAY; /** *Default color for the axes.
**/ public static final Color AXES_COLOR = Color.BLUE; /** *Default color for historical {@link VisionGUI.Pose}s.
**/ public static final Color OLD_POSE_COLOR = Color.GRAY; /** *Default color for the newest {@link VisionGUI.Pose}.
**/ public static final Color NEW_POSE_COLOR = Color.GREEN; /** *Factor by which to accelerate pan if SHIFT is down.
**/ public static final double PAN_ACCEL_FACTOR = 4.0; /** *Factor by which to accelerate zoom if SHIFT is down.
**/ public static final double ZOOM_ACCEL_FACTOR = 4.0; /** *Conversion factor of pixels to zoom.
**/ public static final double ZOOM_PER_PIXEL = 0.01; /** *Mouse wheel clicks to drag delta.
**/ public static final double WHEEL_TO_DELTA = 5.0; /** *Default width.
**/ public static final int DEFAULT_WIDTH = 800; /** *Default canvas height.
**/ public static final int DEFAULT_HEIGHT = DEFAULT_WIDTH; /** *Default {@link #scale}.
**/ public static final double DEFAULT_SCALE = DEFAULT_WIDTH/10.0; /** *Default {@link #cx}.
**/ public static final double DEFAULT_CX = 0.0; /** *Default {@link #cy}.
**/ public static final double DEFAULT_CY = 0.0; /** *Default number of odometry updates to wait between saving robot * pose.
**/ public static final int DEFAULT_POSE_SAVE_INTERVAL = 10; /** *Whether to render double-buffered.
**/ public static final boolean DOUBLE_BUFFERED = true; /** *Whether to use RenderingHints.VALUE_ANTIALIAS_ON and
* RenderingHints.VALUE_TEXT_ANTIALIAS_ON.
The rendering quality to use.
**/ public static final Object RENDERING_QUALITY = RenderingHints.VALUE_RENDER_QUALITY; /** *The interpolation to use.
**/ public static final Object INTERPOLATION = RenderingHints.VALUE_INTERPOLATION_BICUBIC; /** *Whether to use
* RenderingHints.VALUE_FRACTIONALMETRICS_ON.
Whether to use RenderingHints.VALUE_STROKE_NORMALIZE.
Width of the body (Y direction) in meters.
**/ public static final double BODY_WIDTH = 0.38; /** *Length of the body (X direction) in meters.
**/ public static final double BODY_LENGTH = 0.38; /** *Width of the wheel (Y direction) in meters.
**/ public static final double WHEEL_WIDTH = 0.02; /** *Length of the wheel (X direction) in meters.
**/ public static final double WHEEL_LENGTH = 0.12; /** *Y displacement of the left wheel rect relative to robot ctr (m).
**/ public static final double WHEEL_DISPLACEMENT = 0.21; /** *X displacement of the body rect relative to robot ctr (m).
**/ public static final double BODY_DISPLACEMENT = -0.09; /** *Wheel shape centered on origin in world frame (m).
**/ public static final Shape WHEEL_SHAPE = new RoundRectangle2D.Double( -WHEEL_LENGTH/2.0, -WHEEL_WIDTH/2.0, WHEEL_LENGTH, WHEEL_WIDTH, 0.005, 0.005); /** *Body shape centered on origin in world frame (m).
**/ public static final Shape BODY_SHAPE = new Rectangle2D.Double( -BODY_LENGTH/2.0, -BODY_WIDTH/2.0, BODY_LENGTH, BODY_WIDTH); /** *Aggregate shape of the robot at pose (0, 0, 0).
**/ public static final GeneralPath ROBOT_SHAPE = new GeneralPath(); static { AffineTransform t = new AffineTransform(); t.setToIdentity(); t.translate(BODY_DISPLACEMENT, 0.0); ROBOT_SHAPE.append(t.createTransformedShape(BODY_SHAPE), false); t.setToIdentity(); t.translate(0.0, WHEEL_DISPLACEMENT); ROBOT_SHAPE.append(t.createTransformedShape(WHEEL_SHAPE), false); t.setToIdentity(); t.translate(0.0, -WHEEL_DISPLACEMENT); ROBOT_SHAPE.append(t.createTransformedShape(WHEEL_SHAPE), false); ROBOT_SHAPE.append(new Line2D.Double(0.0, 0.0, 0.01, 0.0), false); ROBOT_SHAPE.append(new Line2D.Double(0.0, 0.0, 0.0, 0.01), false); } /** *Scale that takes one world meter to one pixel.
**/ protected double scale = DEFAULT_SCALE; /** *View center x world coord in meters.
**/ protected double cx = DEFAULT_CX; /** *View center y world coord in meters.
**/ protected double cy = DEFAULT_CY; protected boolean firstUpdate = true; /** *Whether to sacrifice rendering quality for speed.
**/ protected boolean renderFastest = false; /** *Whether to paint the grid.
**/ protected boolean gridEnabled = true; /** *Whether to paint the current robot pose.
**/ protected boolean currentPoseEnabled = true; /** *Whether to paint the historical robot poses.
**/ protected boolean historicalPosesEnabled = true; /** *Whether to paint the vision image.
**/ protected boolean visionImageEnabled = true; /** *The frame containing this GUI.
**/ protected JFrame frame; /** *Transform that takes points in world coordinates (meters) to points * in view coordinates (pixels).
**/ protected AffineTransform worldToView = new AffineTransform(); /** *Total time it took in milliseconds to render the last frame.
**/ protected double lastFrameTime = 0.0; /** *Frame time in ms above which to force fast rendering.
**/ public static final double FORCE_FAST_RENDER_THRESHOLD = 300.0; /** *Frame time in ms below which to un-force fast rendering.
**/ public static final double UN_FORCE_FAST_RENDER_THRESHOLD = 5.0; public SubscriberA paintable graphical object.
**/ protected abstract class Glyph { /** *Paint this glyph.
* * @param g2d the graphics context **/ public abstract void paint(Graphics2D g2d); } /** *A robot pose.
**/ protected class Pose extends Glyph { /** *The robot shape in the specified pose in world frame.
**/ Shape robot; /** *Create a new unset pose.
**/ Pose() { robot = null; } /** *Create a new set pose.
**/ Pose(double x, double y, double theta) { set(x, y, theta); } /** *(Re)Set this pose.
**/ void set(double x, double y, double theta) { AffineTransform t = new AffineTransform(); t.setToIdentity(); t.translate(x, y); t.rotate(theta); robot = t.createTransformedShape(ROBOT_SHAPE); } /** *Unset this pose.
**/ void unset() { robot = null; } /** *Paints the pose.
* *Assumes line width and color are already set.
* * @param g2d the graphics context **/ public void paint(Graphics2D g2d) { if (robot != null) g2d.draw(robot); } } /** *Displays images from the robot's camera.
**/ protected class VisionImage extends Glyph { /** *The pixel buffer for the displayed image.
**/ int packedImage[] = null; /** *Java image animation machinery.
**/ MemoryImageSource source = null; /** *The actual image we paint, null if none.
**/ java.awt.Image image = null; /** *Currently displayed image width or -1 if none.
**/ int width = -1; /** *Currently displayed image height or -1 if none.
**/ int height = -1; /** *Create a new vision image, initially not displaying anything.
**/ VisionImage() { unset(); } /** *Set to display an image.
* * @param unpackedImage the unpacked RGB image (a copy is made) * @param width image width * @param height image height **/ void set(byte[] unpackedImage, int width, int height) { if ((unpackedImage == null) || (width <= 0) || (height <= 0)) { unset(); return; } boolean reConsedPacked = false; if ((packedImage == null) || (this.width != width) || (this.height != height)) { packedImage = new int[width*height]; reConsedPacked = true; } int srcIndex = 0; int destIndex = 0; for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { int red = unpackedImage[srcIndex++] & 0xff; int green = unpackedImage[srcIndex++] & 0xff; int blue = unpackedImage[srcIndex++] & 0xff; packedImage[destIndex++] = (0xff << 24) | (red << 16) | (green << 8) | blue; } } if (reConsedPacked || (image == null) || (this.width != width) || (this.height != height)) { source = new MemoryImageSource(width, height, packedImage, 0, width); source.setAnimated(true); image = createImage(source); } else { source.newPixels(); } this.width = width; this.height = height; } /** *Disable display.
**/ void unset() { image = null; width = -1; height = -1; } /** *Paints the image, if any.
* *Assumes g2d is in view coordinates.
* * @param g2d the graphics context **/ public void paint(Graphics2D g2d) { if (image == null) return; g2d.drawImage(image, 0, 0, VisionGUI.this); } } /** *The current (i.e. most recent) robot pose.
**/ protected Pose currentPose = new Pose(); /** *All the historical {@link VisionGUI.Pose}s.
**/ protected java.util.ListThe one {@link VisionGUI.VisionImage}.
**/ protected VisionImage visionImage = new VisionImage(); /** *How many poses to skip before saving a history pose.
* *Zero indicates that every pose is saved. Negative indicates that no * poses are saved.
**/ protected int poseSaveInterval; /** *Synchronization object for {@link #poseSaveInterval} and {@link * #posesToGoBeforeSave}.
**/ protected Object poseSaveLock = new Object(); /** *How many poses we have left to go before saving one (if enabled).
**/ protected int posesToGoBeforeSave = 0; /** *Current maximum translation velocity (m/s).
* *Zero or negative disables driving.
**/ protected double maxTV = 0.0; /** *Current maximum rotational velocity (rad/s).
* *Zero or negative disables driving.
**/ protected double maxRV = 0.0; /** *Current translation velocity command (m/s).
**/ protected double tv = 0.0; /** *Current maximum rotation velocity (rad/s).
**/ protected double rv = 0.0; /** *Consruct a new VisionGUI.
* *The new viewer will automatically appear in a new window.
* * @param poseSaveInterval the number of robot pose updates to skip between * saving a persistent snapshot of the pose. Zero indicates that every pose * should be saved. Negative indicates that no poses should be saved. * @param maxTV max translation velocity in m/s. Setting this less than or * equal to 0.0 disables driving. See also {@link #setMaxTV}. * @param maxRV max translation velocity in m/s. Setting this less than or * equal to 0.0 disables driving. See also {@link #setMaxRV}. **/ public VisionGUI(int poseSaveInterval, double maxTV, double maxRV) { this.poseSaveInterval = poseSaveInterval; this.maxTV = maxTV; this.maxRV = maxRV; resetWorldToView(DEFAULT_CX, DEFAULT_CY, DEFAULT_SCALE); //do this here, not in invokeLater, so we can configure frame in //instanceMain() frame = new JFrame(getAppName()); SwingUtilities.invokeLater(new Runnable() { public void run() { setBackground(Color.WHITE); setPreferredSize(new Dimension(DEFAULT_WIDTH, DEFAULT_HEIGHT)); setOpaque(true); setDoubleBuffered(true); Container contentPane = frame.getContentPane(); contentPane.setBackground(Color.WHITE); contentPane.setLayout(new BorderLayout()); contentPane.add(VisionGUI.this, "Center"); frame.pack(); //do this before working on focus! addKeyListener(keyListener); setFocusable(true); requestFocusInWindow(); frame.setLocationByPlatform(true); frame.setVisible(true); setMaxTV(0.50); setMaxRV(0.10); } }); } /** *Covers {@link #VisionGUI(int, double, double)}, disables driving.
**/ public VisionGUI(int poseSaveInterval) { this(poseSaveInterval, 0.0, 0.0); } /** *Covers {@link #VisionGUI(int)}, always uses {@link * #DEFAULT_POSE_SAVE_INTERVAL}.
**/ public VisionGUI() { this(DEFAULT_POSE_SAVE_INTERVAL); } /** *Clamp v to [-|m|, |m|].
* * @param v the value to clamp * @param m the absolute max * @return the clamped value **/ public static double clamp(double v, double m) { m = Math.abs(m); if (v > m) v = m; if (v < -m) v = -m; return v; } /** *Get the frame containing this GUI.
* * @return the frame containing this GUI **/ public JFrame getFrame() { return frame; } /** *Get the title for the GUI frame.
* *Default impl returns {@link #APPNAME}.
* * @return the title for the GUI frame **/ public String getAppName() { return APPNAME; } /** *(Re)Set the max translation velocity for driving.
* * @param maxTV the max translation velocity in m/s. Less than or equal to * zero disables driving. **/ public void setMaxTV(double maxTV) { synchronized(keyListener) { this.maxTV = maxTV; } } /** *(Re)Set the max rotation velocity for driving.
* * @param maxRV the max rotation velocity in rad/s. Less than or equal to * zero disables driving. **/ public void setMaxRV(double maxRV) { synchronized(keyListener) { this.maxRV = maxRV; } } /** *Set a new {@link #poseSaveInterval}.
* * @param poseSaveInterval the number of robot pose updates to skip between * saving a persistent snapshot of the pose. Zero indicates that every pose * should be saved. Negative indicates that no poses should be saved. **/ public void setPoseSaveInterval(int poseSaveInterval) { synchronized (poseSaveLock) { this.poseSaveInterval = poseSaveInterval; posesToGoBeforeSave = 0; } } /** *Set the robot pose to display.
* *Note: if you are running the GUI in stand-alone mode, this will be * automatically called when Carmen odometry messages are recieved.
* *If {@link #poseSaveInterval} is non-negative, then it indicates how * many calls to this method are skipped between a historical pose is * saved.
* * @param x the robot center x position in world frame (m) * @param y the robot center y position in world frame (m) * @param theta the ccw orientation of the robot frame x-axis wrt the world * frame x-axis (rad) **/ public void setRobotPose(double x, double y, double theta) { synchronized (currentPose) { currentPose.set(x, y, theta); } boolean addToHistory = false; synchronized (poseSaveLock) { if (poseSaveInterval >= 0) { if (posesToGoBeforeSave == 0) { addToHistory = true; posesToGoBeforeSave = poseSaveInterval; } else { posesToGoBeforeSave--; } } } if (addToHistory) historicalPoses.add(new Pose(x, y, theta)); repaint(); } /** *Set the vision image for display.
* *Note: if you are running the GUI in stand-alone mode, this will be * automatically called when Carmen vision image messages are recieved.
* * @param image is an unpackedwidth by height RGB
* image
* @param width the image width
* @param height the image height
**/
public void setVisionImage(byte[] image, int width, int height) {
synchronized (visionImage) {
visionImage.set(image, width, height);
}
repaint();
}
/**
* Erase the current robot pose, if any.
**/ public void erasePose() { synchronized (currentPose) { currentPose.unset(); } repaint(); } /** *Erase all historical robot poses.
**/ public void erasePoseHistory() { historicalPoses.clear(); repaint(); } /** *Erase the vision image, if any.
**/ public void eraseVisionImage() { synchronized (visionImage) { visionImage.unset(); } } /** *Set the line width in g2d.
Calls {@link #paintComponent(Graphics2D)}.
* * @param g the paint context **/ public void paintComponent(Graphics g) { paintComponent((Graphics2D) g); } /** *Calls superclass impl, then {@link #paintContents}.
**/ protected void paintComponent(Graphics2D g2d) { super.paintComponent(g2d); paintContents(g2d); } /** *Paint all the graphics.
* * @param g2d the paint context **/ protected void paintContents(Graphics2D g2d) { double startTime = System.currentTimeMillis(); if (lastFrameTime > FORCE_FAST_RENDER_THRESHOLD) renderFastest = true; if (lastFrameTime < UN_FORCE_FAST_RENDER_THRESHOLD) renderFastest = false; if (renderFastest) { g2d.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_SPEED); g2d.setRenderingHint(RenderingHints.KEY_INTERPOLATION, RenderingHints. VALUE_INTERPOLATION_NEAREST_NEIGHBOR); g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_OFF); g2d.setRenderingHint(RenderingHints.KEY_TEXT_ANTIALIASING, RenderingHints.VALUE_TEXT_ANTIALIAS_OFF); g2d.setRenderingHint(RenderingHints.KEY_FRACTIONALMETRICS, RenderingHints.VALUE_FRACTIONALMETRICS_OFF); g2d.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE); } else { g2d.setRenderingHint(RenderingHints.KEY_RENDERING, RENDERING_QUALITY); g2d.setRenderingHint(RenderingHints.KEY_INTERPOLATION, INTERPOLATION); if (ANTIALIASING) { g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); g2d.setRenderingHint(RenderingHints.KEY_TEXT_ANTIALIASING, RenderingHints.VALUE_TEXT_ANTIALIAS_ON); } if (FRACTIONALMETRICS) g2d.setRenderingHint(RenderingHints.KEY_FRACTIONALMETRICS, RenderingHints.VALUE_FRACTIONALMETRICS_ON); if (STROKE_NORMALIZATION) g2d.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_NORMALIZE); } AffineTransform transformWas = g2d.getTransform(); synchronized(worldToView) { g2d.transform(worldToView); paintInWorldUnderGridHook(g2d); if (gridEnabled) paintGrid(g2d); paintInWorldOverGridUnderPosesHook(g2d); if (currentPoseEnabled) paintCurrentPose(g2d); if (historicalPosesEnabled) paintPoseHistory(g2d); paintInWorldOverPosesHook(g2d); } g2d.setTransform(transformWas); paintInViewUnderVisionImageHook(g2d); if (visionImageEnabled) paintVisionImage(g2d); paintInViewOverVisionImageHook(g2d); lastFrameTime = System.currentTimeMillis() - startTime; // System.err.println("frame time = " + lastFrameTime); } /** *Hook to add painting in world frame under the coordinate grid.
**/ protected void paintInWorldUnderGridHook(Graphics2D g2d) { } /** *Hook to add painting in world frame over the coordinate grid but under * the robot poses.
**/ protected void paintInWorldOverGridUnderPosesHook(Graphics2D g2d) { } /** *Hook to add painting in world frame over the robot poses.
**/ protected void paintInWorldOverPosesHook(Graphics2D g2d) { } /** *Hook to add painting in view frame under the vision image.
**/ protected void paintInViewUnderVisionImageHook(Graphics2D g2d) { } /** *Hook to add painting in view frame over the vision image.
**/ protected void paintInViewOverVisionImageHook(Graphics2D g2d) { } /** *Paint the coordinate grid.
* * @param g2d the graphics context **/ protected void paintGrid(Graphics2D g2d) { setLineWidth(g2d, GRID_LINE_WIDTH); g2d.setColor(GRID_COLOR); //current viewport dimensions in meters double width = ((double)getWidth())/scale; double height = ((double)getHeight())/scale; //current viewport bounds in meters double xMin = cx - width/2.0; double xMax = cx + width/2.0; double yMin = cy - height/2.0; double yMax = cy + height/2.0; Line2D.Double l = new Line2D.Double(); //draw vertical lines l.y1 = yMin; l.y2 = yMax; for (double x = Math.ceil(xMin/GRID_SPACING)*GRID_SPACING; x <= xMax; x += GRID_SPACING) { l.x1 = x; l.x2 = x; g2d.draw(l); } //draw horizontal lines l.x1 = xMin; l.x2 = xMax; for (double y = Math.ceil(yMin/GRID_SPACING)*GRID_SPACING; y <= yMax; y += GRID_SPACING) { l.y1 = y; l.y2 = y; g2d.draw(l); } //draw axes, maybe setLineWidth(g2d, AXES_LINE_WIDTH); g2d.setColor(AXES_COLOR); if ((xMin < 0.0) && (xMax > 0.0)) { l.y1 = yMin; l.y2 = yMax; l.x1 = 0.0; l.x2 = 0.0; g2d.draw(l); } if ((yMin < 0.0) && (yMax > 0.0)) { l.y1 = 0.0; l.y2 = 0.0; l.x1 = xMin; l.x2 = xMax; g2d.draw(l); } } /** *Paint {@link #currentPose}.
* * @param g2d the graphics context **/ protected void paintCurrentPose(Graphics2D g2d) { //avoid NPE on init if (currentPose == null) return; setLineWidth(g2d, POSE_LINE_WIDTH); g2d.setColor(NEW_POSE_COLOR); synchronized (currentPose) { currentPose.paint(g2d); } } /** *Paint all {@link #historicalPoses}.
* * @param g2d the graphics context **/ protected void paintPoseHistory(Graphics2D g2d) { //avoid NPE on init if (historicalPoses == null) return; setLineWidth(g2d, POSE_LINE_WIDTH); g2d.setColor(OLD_POSE_COLOR); synchronized (historicalPoses) { for (Iterator it = historicalPoses.iterator(); it.hasNext(); ) ((Pose) it.next()).paint(g2d); } } /** *Paint {@link #visionImage}.
* * @param g2d the graphics context **/ protected void paintVisionImage(Graphics2D g2d) { //avoid NPE on init if (visionImage == null) return; synchronized (visionImage) { visionImage.paint(g2d); } } /** *Updates {@link #worldToView} to be centered at (cx, cy)
* with the given scale.
Updates {@link #worldToView} to be centered at (cx,
* cy).
Updates {@link #worldToView} with the given scale.
* * @param scale scale that takes one world unit to one device unit **/ public void zoomWorldToView(double scale) { resetWorldToView(cx, cy, scale); } /** *Gets the world coordinates from a view point.
* * @param point the point in view coords * * @return the point in world coords **/ public Point2D.Double viewToWorld(Point2D.Double point) { synchronized (worldToView) { try { worldToView.inverseTransform(point, point); return point; } catch (NoninvertibleTransformException ex) { System.err.println("warning: " + ex.getStackTrace()); return null; } } } /** *Implements mouse navigation.
* *See class header doc for details.
**/ protected MouseInputListener mouseInputListener = new MouseInputAdapter() { { addMouseMotionListener(this); addMouseListener(this); addMouseWheelListener(new MouseWheelListener() { public void mouseWheelMoved(MouseWheelEvent e) { int modifiers = e.getModifiersEx(); boolean accel = isAccel(modifiers); zoomCenter.setLocation(e.getPoint()); viewToWorld(zoomCenter); doZoom(deltaToZoom(WHEEL_TO_DELTA*e.getWheelRotation(), accel), modifiers); } }); } /** *Set up for a mouse click or drag.
**/ public void mousePressed(MouseEvent e) { Point2D point = e.getPoint(); prevDragPoint.setLocation(point); startDragPoint.setLocation(point); viewToWorld(startDragPoint); Point2D.Double d = new Point2D.Double(startDragPoint.x - lastClickedPoint.x, startDragPoint.y - lastClickedPoint.y); System.out.println("clicked point: " + startDragPoint); System.out.println(" rel to last: " + d); System.out.println(" dist: " + startDragPoint.distance(lastClickedPoint)); lastClickedPoint.setLocation(startDragPoint); zoomCenter.setLocation(startDragPoint); dragInProgress = false; } /** *End a mouse click or drag.
**/ public void mouseReleased(MouseEvent e) { if (!dragInProgress) { mouseMoved(e); handleClick(e); } else { dragInProgress = false; } } /** *Handle non-drag motion.
* *Currently ignored.
**/ public void mouseMoved(MouseEvent e) { if (dragInProgress) return; } /** *Handle mouse drag.
* *Currently just implements navigation.
**/ public void mouseDragged(MouseEvent e) { dragInProgress = true; if (prevDragPoint == null) return; currentDragPoint.setLocation(e.getPoint()); int modifiers = e.getModifiersEx(); boolean accel = isAccel(modifiers); // System.out.println(e.getModifiersExText(modifiers)); double dx = currentDragPoint.getX() - prevDragPoint.getX(); double dy = currentDragPoint.getY() - prevDragPoint.getY(); prevDragPoint.setLocation(currentDragPoint); viewToWorld(currentDragPoint); if (isPan(modifiers)) doPan(deltaToPanX(dx, accel), deltaToPanY(dy, accel)); if (isZoom(modifiers)) doZoom(deltaToZoom(dy, accel), modifiers); } /** *Handle a mouse click.
* *Currently ignored.
**/ public void handleClick(MouseEvent e) { } /** *Check whether modifiers indicates a zoom.
Current impl just looks for CTRL down or button 2 or 3.
* * @param modifiers the extended modifiers * * @return true iffmodifiers indicates a zoom
**/
public boolean isZoom(int modifiers) {
return
((modifiers & InputEvent.CTRL_DOWN_MASK) != 0) ||
((modifiers & InputEvent.BUTTON2_DOWN_MASK) != 0) ||
((modifiers & InputEvent.BUTTON3_DOWN_MASK) != 0);
}
/**
* Check whether modifiers indicates a pan.
Current impl just returns !{@link #isZoom}.
modifiers indicates a pan
**/
public boolean isPan(int modifiers) {
return !isZoom(modifiers);
}
/**
* Check whether modifiers indicates an accelerated
* navigation.
Current impl just looks for SHIFT down.
* * @param modifiers the extended modifiers * * @return true iffmodifiers indicates an accelerated
* navigation
**/
public boolean isAccel(int modifiers) {
return (modifiers & InputEvent.SHIFT_DOWN_MASK) != 0;
}
/**
* Compute a zoom amount from a drag delta.
* * @param dy delta y, in viewpoint coordinates * @param accel whether to multiply the return value by {@link * #ZOOM_ACCEL_FACTOR} * * @return the new view scale **/ public double deltaToZoom(double dy, boolean accel) { double factor = Math.abs(dy)*ZOOM_PER_PIXEL; if (accel) factor *= ZOOM_ACCEL_FACTOR; factor += 1.0; if (dy < 0.0) factor = 1.0/factor; double newScale = scale; // log.debug("factor: " + factor + ", newScale: " + newScale); newScale *= factor; return newScale; } /** *Compute pan x from delta x, in viewpoint coordinates.
* * @param dx delta x, in viewpoint coordinates * @param accel whether to multiply the return value by {@link * #PAN_ACCEL_FACTOR} * * @return the amount to pan x, in viewpoint coordinates **/ public double deltaToPanX(double dx, boolean accel) { return dx*((accel) ? PAN_ACCEL_FACTOR : 1.0); } /** *Compute pan y from delta y, in viewpoint coordinates.
* * @param dy delta y, in viewpoint coordinates * @param accel whether to multiply the return value by {@link * #PAN_ACCEL_FACTOR} * * @return the amount to pan y, in viewpoint coordinates **/ public double deltaToPanY(double dy, boolean accel) { return dy*((accel) ? PAN_ACCEL_FACTOR : 1.0); } /** *Perform a zoom.
* *Zooms about {@link #zoomCenter} by default, about the view center if * ALT is down.
* * @param newScale the new view scale * @param modifiers the extended modifiers **/ public void doZoom(double newScale, int modifiers) { if ((modifiers & InputEvent.ALT_DOWN_MASK) != 0) { //scale about current view center resetWorldToView(cx, cy, newScale); return; } if (zoomCenter == null) return; //scale about zoomCenter double scale = VisionGUI.this.scale; double ds = newScale - scale; double x = (scale*cx+ds*zoomCenter.x)/newScale; double y = (scale*cy+ds*zoomCenter.y)/newScale; resetWorldToView(x, y, newScale); } /** *Perform a pan in {@link #view}.
* * @param dx the x translation, in viewpoint coordinates * @param dy the y translation, in viewpoint coordinates **/ public void doPan(double dx, double dy) { resetWorldToView(cx-dx/scale, cy+dy/scale); } /** *The previous point of the current drag.
**/ protected Point2D.Double prevDragPoint = new Point2D.Double(); /** *The current drag point.
**/ protected Point2D.Double currentDragPoint = new Point2D.Double(); /** *The start point of the current drag in world coords.
**/ protected Point2D.Double startDragPoint = new Point2D.Double(); protected Point2D.Double lastClickedPoint = new Point2D.Double(); /** *The current zoom center in world coords.
**/ protected Point2D.Double zoomCenter = new Point2D.Double(); /** *Whether a drag is currently ongoing.
**/ protected boolean dragInProgress = false; }; /** *Drives robot iff both {@link #maxTV} and {@link #maxRV} are greater * than zero.
* *See class header doc for details.
**/ protected KeyListener keyListener = new KeyListener() { public void keyPressed(KeyEvent e) { } public void keyReleased(KeyEvent e) { } public synchronized void keyTyped(KeyEvent e) { if ((maxTV <= 0.0) || (maxRV <= 0.0)) return; tv = 0.0; rv = 0.0; switch (e.getKeyChar()) { case ' ': break; case 'i': tv = maxTV; break; case 'u': tv = maxTV; rv = maxRV; break; case 'o': tv = maxTV; rv = -maxRV; break; case 'j': rv = maxRV; break; case 'l': rv = -maxRV; break; case ',': tv = -maxTV; break; case 'm': tv = -maxTV; rv = -maxRV; break; case '.': tv = -maxTV; rv = maxRV; break; case 'q': System.exit(-1); break; } System.out.printf("setting velocity: %f %f\n", tv, rv); MotionMsg msg= new MotionMsg(); msg.translationalVelocity = tv; msg.rotationalVelocity = rv; publisher.publish(msg); } }; /** *Extends default impl to set white background.
**/ public JToolTip createToolTip() { JToolTip toolTip = super.createToolTip(); toolTip.setBackground(Color.WHITE); return toolTip; } /** *Handle size changes.
**/ protected ComponentListener componentListener = new ComponentAdapter() { { addComponentListener(this); } /** *Calls {@link #resetWorldToView}, then {@link #repaint}.
**/ public void componentResized(ComponentEvent e) { resetWorldToView(cx, cy, scale); } }; /** *See {@link #instanceMain}.
**/ @Override public void onStart(Node node) { this.node = node; vidSub = node.newSubscriber("/rss/video", "sensor_msgs/Image"); vidSub.addMessageListener(new MessageListenerDraw some things to test the graphics capabilities.
**/ protected void testGraphics() { try { setRobotPose(0.0, 0.0, 0.0); setRobotPose(0.0, -1.0, Math.PI/4.0); // for (int i = 0; i < 5000; i++) // setRobotPose(Math.random(), Math.random(), // (Math.random()-0.5)*2.0*Math.PI); byte[] testImage = new byte[256*256*3]; int index = 0; for (int r = 0; r < 256; r++) { for (int c = 0; c < 256; c++) { byte val = (byte) c; testImage[index++] = val; testImage[index++] = val; testImage[index++] = val; } } setVisionImage(testImage, 256, 256); Thread.sleep(1000); setVisionImage(testImage, 100, 100); Thread.sleep(1000); eraseVisionImage(); testGraphicsHook(); for (;;) Thread.sleep(1000); } catch (InterruptedException e) { //ignore } } /** *Hook to append to the end of {@link #testGraphics}.
**/ protected void testGraphicsHook() throws InterruptedException { } @Override public void onShutdown(Node arg0) { // TODO Auto-generated method stub if(node != null) { node.shutdown(); } } @Override public void onShutdownComplete(Node node) { } @Override public GraphName getDefaultNodeName() { return new GraphName("rss/visiongui"); } }