Merge pull request #6 from teamhyper/vision-connector

Convert hyperLib's Vision System to use a VisionConnector to post information from the Raspberry Pi to NetworkTables, and allow the robot code to subscribe to those updates for VIsion Commands.

build.gradle

@@ -21,7 +21,7 @@ allprojects {
     }
 }
 
-def WPILIB_VER = '2019.1.1'
+def WPILIB_VER = '2019.2.1'
 def NTCORE_VER = WPILIB_VER
 def OPENCV_VER = '3.4.4-4'
 def WPIUTIL_VER = WPILIB_VER

src/main/java/org/hyperonline/hyperlib/HYPERVision.java

@@ -0,0 +1,102 @@
+package org.hyperonline.hyperlib;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * HYPERVision provide code which is mostly the same in every vision application.
+ *
+ * @author Chris McGroarty
+ */
+public abstract class HYPERVision {
+
+    protected CameraServer m_cameraServer;
+    protected NetworkTableInstance m_tableInstance;
+
+    // override this property to true in a child class that runs on the robot
+    protected boolean m_isRobot = false;
+
+    /**
+     * Initialize the HYPERVision application with components initialized in a specific order
+     */
+    public final void visionInit() {
+        initNetworkTable();
+        initCameraServer();
+        initCameras();
+        initConnectors();
+        initProcessors();
+        initPipelines();
+        initModules();
+    }
+
+    /**
+     * Method runs throughout the application, allowing for
+     * the scheduling of runnable tasks in the PeriodicScheduler
+     */
+    private void visionPeriodic() {
+        PeriodicScheduler.getInstance().run();
+    }
+
+    /**
+     * Run the Vision application
+     * Matches startCompetition of a Robot
+     */
+    public void startCompetition() {
+        visionInit();
+        startModules();
+
+        // only run periodic when not running on the robot
+        // robot has its own periodics
+        while (!m_isRobot) {
+            visionPeriodic();
+        }
+    }
+
+    /**
+     * Initialize the NetworkTableInstance and start our team's client
+     */
+    protected void initNetworkTable() {
+        m_tableInstance = NetworkTableInstance.getDefault();
+        m_tableInstance.startClientTeam(69);
+    }
+
+    /**
+     * Initialize the cameras used in this HYPERVision
+     */
+    protected abstract void initCameras();
+
+    /**
+     * Initialize the VisionConnectors used in this HYPERVision
+     */
+    protected abstract void initConnectors();
+
+    /**
+     * Initialize the TargetProcessors used in this HYPERVision
+     */
+    protected abstract void initProcessors();
+    /**
+     * Initialize the VIsionGUIPipelines used in this HYPERVision
+     */
+    protected abstract void initPipelines();
+    /**
+     * Initialize the VisionModules used in this HYPERVision
+     */
+    protected abstract void initModules();
+    /**
+     * Start the VisionModules used in this HYPERVision
+     */
+    protected abstract void startModules();
+
+    /**
+     * Initialize the CameraServer used in this HYPERVision
+     */
+    protected void initCameraServer() {
+        m_cameraServer = CameraServer.getInstance();
+    }
+
+    /**
+     * Update properties when Preferences have changed
+     */
+    protected abstract void onPreferencesUpdated();
+}
+

src/main/java/org/hyperonline/hyperlib/vision/AbstractTargetProcessor.java

@@ -1,80 +1,42 @@
 package org.hyperonline.hyperlib.vision;
 
-import java.util.List;
-
-import org.hyperonline.hyperlib.pid.DisplacementPIDSource;
 import org.opencv.core.Rect;
 
-import edu.wpi.first.wpilibj.PIDSource;
+import java.util.List;
 
 /**
  * Base class with common functionality used by TargetProcessors. In particular,
  * this provides methods to store a result, and access it via PID sources.
- * 
- * @author James Hagborg
  *
- * @param <T>
- * 			VisionResult type of this AbstractTargetProcessor
+ * @param <T> VisionResult type of this AbstractTargetProcessor
+ * @author James Hagborg
  */
 public abstract class AbstractTargetProcessor<T extends VisionResult> implements TargetProcessor {
 
-    private volatile T m_lastResult;
-
+    private AbstractVisionConnector<T> m_visionConnector;
+
+    protected AbstractTargetProcessor(AbstractVisionConnector<T> connector) {
+        m_visionConnector = connector;
+    }
+
     /**
-     * 
-     * @param targets
-     * 			list of targets to compute
+     * @param targets list of targets to compute
      * @return {T}
      */
     public abstract T computeResult(List<Rect> targets);
-
-    /**
-     * 
-     * @return {T}
-     */
-    public abstract T getDefaultValue();
-
-    @Override
-    public final void process(List<Rect> targets) {
-        m_lastResult = computeResult(targets);
-    }
 
-    /**
-     * Get the most recent result of this processor. If no result has been
-     * produced yet, returns the default value.
-     * 
-     * @return The most recent result.
-     */
-    public T getLastResult() {
-        T res = m_lastResult;
-        return res == null ? getDefaultValue() : res;
-    }
-
-    /**
-     * Get a PID source which tracks the x-coordinate of the target.
-     * 
-     * @return A PID source tracking the x-coordinate of the target.
-     */
-    public PIDSource xPID() {
-        return new DisplacementPIDSource() {
-            @Override
-            public double pidGet() {
-                return getLastResult().xError();
-            }
-        };
+    protected T getLastResult(){
+        return m_visionConnector.getLastResult();
     }
 
     /**
-     * Get a PID source which tracks the y-coordinate of the target.
-     * 
-     * @return A PID source tracking the y-coordinate of the target.
+     *
+     * @param targets The list of targets found.
      */
-    public PIDSource yPID() {
-        return new DisplacementPIDSource() {
-            @Override
-            public double pidGet() {
-                return getLastResult().yError();
-            }
-        };
+    @Override
+    public final void process(List<Rect> targets) {
+        T res = computeResult(targets);
+        m_visionConnector.updateResult(res);
     }
 }
+

src/main/java/org/hyperonline/hyperlib/vision/AbstractVisionConnector.java

@@ -0,0 +1,164 @@
+package org.hyperonline.hyperlib.vision;
+
+import edu.wpi.first.networktables.*;
+import edu.wpi.first.wpilibj.PIDSource;
+import org.hyperonline.hyperlib.pid.DisplacementPIDSource;
+
+import java.util.Objects;
+
+/**
+ * Base class with common functionality used by VisionConnectors. In particular,
+ * this provides methods to store, publish, and subscribe to a result,
+ * and access it via PID sources.
+ *
+ * @param <T> VisionResult type of this AbstractVisionConnector
+ * @author Chris McGroarty
+ */
+public abstract class AbstractVisionConnector<T extends VisionResult> implements VisionConnector {
+
+    protected final NetworkTableEntry m_xError;
+    protected final NetworkTableEntry m_yError;
+    protected final NetworkTableEntry m_xAbs;
+    protected final NetworkTableEntry m_yAbs;
+    protected final NetworkTableEntry m_foundTarget;
+    protected final NetworkTableEntry m_lastResultTimestamp;
+    protected T m_lastResult;
+    protected NetworkTable m_table;
+    private String m_mainTableName = "hypervision";
+    private String m_subTableName;
+    private NetworkTableInstance m_inst;
+    private int m_listenerID;
+
+    protected AbstractVisionConnector(String subTableName, NetworkTableInstance inst) {
+        m_subTableName = Objects.requireNonNull(subTableName);
+
+        m_inst = Objects.requireNonNull(inst);
+        m_table = m_inst.getTable(getTableName());
+        m_lastResult = getDefaultValue();
+
+        m_xError = m_table.getEntry("xError");
+        m_yError = m_table.getEntry("yError");
+        m_xAbs = m_table.getEntry("xAbsolute");
+        m_yAbs = m_table.getEntry("yAbsolute");
+        m_foundTarget = m_table.getEntry("foundTarget");
+        m_lastResultTimestamp = m_table.getEntry("lastResultTimestamp");
+    }
+
+    private String getTableName() {
+        return "/" + m_mainTableName + "/" + m_subTableName;
+    }
+
+    /**
+     * add listener for the lastResultTimestamp entry in NetworkTables
+     * process the next result on an update
+     */
+    public void subscribe() {
+        m_listenerID = m_table.addEntryListener("lastResultTimestamp", this::next, EntryListenerFlags.kUpdate);
+    }
+
+    /**
+     * remove listener for the lastResultTimestamp entry in NetworkTables
+     */
+    public void unsubscribe() {
+        m_table.removeEntryListener(m_listenerID);
+    }
+
+    /**
+     * @return {T}
+     */
+    public abstract T getDefaultValue();
+
+    /**
+     * @param result {T}
+     */
+    public void updateResult(T result) {
+        if (m_lastResult == null) {
+            m_lastResult = getDefaultValue();
+        }
+        if (!m_lastResult.equals(result)) {
+            m_lastResult = (result == null ? getDefaultValue() : result);
+            if (m_inst.isConnected()) {
+                this.publish();
+            }
+        } else {
+            System.out.println("NetworkTableInstance(69) not connected");
+        }
+    }
+
+    /**
+     * publish VisionResult values to corresponding NetworkTable Entries
+     * should be overriden and supered by any children of type T extends VisionResult
+     */
+    public void publish() {
+        m_lastResultTimestamp.setDouble(System.currentTimeMillis());
+        m_xError.setDouble(m_lastResult.xError());
+        m_yError.setDouble(m_lastResult.yError());
+        m_xAbs.setDouble(m_lastResult.xAbsolute());
+        m_yAbs.setDouble(m_lastResult.yAbsolute());
+        m_foundTarget.setBoolean(m_lastResult.foundTarget());
+    }
+
+    /**
+     * retrieve a VisionResult from NetworkTable Entries
+     * should be overriden and supered by any children of type T extends VisionResult
+     *
+     * @return {VisionResult}
+     */
+    protected VisionResult retrieve() {
+        return new VisionResult(
+                m_xError.getDouble(0),
+                m_yError.getDouble(0),
+                m_xAbs.getDouble(0),
+                m_yAbs.getDouble(0),
+                m_foundTarget.getBoolean(false)
+        );
+    }
+
+    /**
+     * evaluate and process the results in NetworkTables to my lastResult
+     *
+     * @param table NetworkTable to get values from
+     * @param key Key of the entry that triggered the Listener
+     * @param entry Entry that was updated
+     * @param value the new Value
+     * @param flags Listener flags (created, updated, removed, etc)
+     */
+    protected abstract void next(NetworkTable table, String key, NetworkTableEntry entry,
+                                 NetworkTableValue value, int flags);
+
+
+    /**
+     * @return {T}
+     */
+    public T getLastResult() {
+        return m_lastResult;
+    }
+
+    /**
+     * Get a PID source which tracks the x-coordinate of the target.
+     *
+     * @return A PID source tracking the x-coordinate of the target.
+     */
+    public PIDSource xPID() {
+        return new DisplacementPIDSource() {
+            @Override
+            public double pidGet() {
+                return getLastResult().xError();
+            }
+        };
+    }
+
+    /**
+     * Get a PID source which tracks the y-coordinate of the target.
+     *
+     * @return A PID source tracking the y-coordinate of the target.
+     */
+    public PIDSource yPID() {
+        return new DisplacementPIDSource() {
+            @Override
+            public double pidGet() {
+                return getLastResult().yError();
+            }
+        };
+    }
+}