Skip to content

Commit 6db0a0d

Browse files
cmcgroartycmcgroarty
authored
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.
2 parents 5d67c86 + 79d46f9 commit 6db0a0d

13 files changed

+731
-180
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ allprojects {
2121
}
2222
}
2323

24-
def WPILIB_VER = '2019.1.1'
24+
def WPILIB_VER = '2019.2.1'
2525
def NTCORE_VER = WPILIB_VER
2626
def OPENCV_VER = '3.4.4-4'
2727
def WPIUTIL_VER = WPILIB_VER

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

Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
package org.hyperonline.hyperlib;
2+
3+
import edu.wpi.first.cameraserver.CameraServer;
4+
import edu.wpi.first.networktables.NetworkTableInstance;
5+
6+
/**
7+
* HYPERVision provide code which is mostly the same in every vision application.
8+
*
9+
* @author Chris McGroarty
10+
*/
11+
public abstract class HYPERVision {
12+
13+
protected CameraServer m_cameraServer;
14+
protected NetworkTableInstance m_tableInstance;
15+
16+
// override this property to true in a child class that runs on the robot
17+
protected boolean m_isRobot = false;
18+
19+
/**
20+
* Initialize the HYPERVision application with components initialized in a specific order
21+
*/
22+
public final void visionInit() {
23+
initNetworkTable();
24+
initCameraServer();
25+
initCameras();
26+
initConnectors();
27+
initProcessors();
28+
initPipelines();
29+
initModules();
30+
}
31+
32+
/**
33+
* Method runs throughout the application, allowing for
34+
* the scheduling of runnable tasks in the PeriodicScheduler
35+
*/
36+
private void visionPeriodic() {
37+
PeriodicScheduler.getInstance().run();
38+
}
39+
40+
/**
41+
* Run the Vision application
42+
* Matches startCompetition of a Robot
43+
*/
44+
public void startCompetition() {
45+
visionInit();
46+
startModules();
47+
48+
// only run periodic when not running on the robot
49+
// robot has its own periodics
50+
while (!m_isRobot) {
51+
visionPeriodic();
52+
}
53+
}
54+
55+
/**
56+
* Initialize the NetworkTableInstance and start our team's client
57+
*/
58+
protected void initNetworkTable() {
59+
m_tableInstance = NetworkTableInstance.getDefault();
60+
m_tableInstance.startClientTeam(69);
61+
}
62+
63+
/**
64+
* Initialize the cameras used in this HYPERVision
65+
*/
66+
protected abstract void initCameras();
67+
68+
/**
69+
* Initialize the VisionConnectors used in this HYPERVision
70+
*/
71+
protected abstract void initConnectors();
72+
73+
/**
74+
* Initialize the TargetProcessors used in this HYPERVision
75+
*/
76+
protected abstract void initProcessors();
77+
/**
78+
* Initialize the VIsionGUIPipelines used in this HYPERVision
79+
*/
80+
protected abstract void initPipelines();
81+
/**
82+
* Initialize the VisionModules used in this HYPERVision
83+
*/
84+
protected abstract void initModules();
85+
/**
86+
* Start the VisionModules used in this HYPERVision
87+
*/
88+
protected abstract void startModules();
89+
90+
/**
91+
* Initialize the CameraServer used in this HYPERVision
92+
*/
93+
protected void initCameraServer() {
94+
m_cameraServer = CameraServer.getInstance();
95+
}
96+
97+
/**
98+
* Update properties when Preferences have changed
99+
*/
100+
protected abstract void onPreferencesUpdated();
101+
}
102+

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

Lines changed: 19 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -1,80 +1,42 @@
11
package org.hyperonline.hyperlib.vision;
22

3-
import java.util.List;
4-
5-
import org.hyperonline.hyperlib.pid.DisplacementPIDSource;
63
import org.opencv.core.Rect;
74

8-
import edu.wpi.first.wpilibj.PIDSource;
5+
import java.util.List;
96

107
/**
118
* Base class with common functionality used by TargetProcessors. In particular,
129
* this provides methods to store a result, and access it via PID sources.
13-
*
14-
* @author James Hagborg
1510
*
16-
* @param <T>
17-
* VisionResult type of this AbstractTargetProcessor
11+
* @param <T> VisionResult type of this AbstractTargetProcessor
12+
* @author James Hagborg
1813
*/
1914
public abstract class AbstractTargetProcessor<T extends VisionResult> implements TargetProcessor {
2015

21-
private volatile T m_lastResult;
22-
16+
private AbstractVisionConnector<T> m_visionConnector;
17+
18+
protected AbstractTargetProcessor(AbstractVisionConnector<T> connector) {
19+
m_visionConnector = connector;
20+
}
21+
2322
/**
24-
*
25-
* @param targets
26-
* list of targets to compute
23+
* @param targets list of targets to compute
2724
* @return {T}
2825
*/
2926
public abstract T computeResult(List<Rect> targets);
30-
31-
/**
32-
*
33-
* @return {T}
34-
*/
35-
public abstract T getDefaultValue();
36-
37-
@Override
38-
public final void process(List<Rect> targets) {
39-
m_lastResult = computeResult(targets);
40-
}
4127

42-
/**
43-
* Get the most recent result of this processor. If no result has been
44-
* produced yet, returns the default value.
45-
*
46-
* @return The most recent result.
47-
*/
48-
public T getLastResult() {
49-
T res = m_lastResult;
50-
return res == null ? getDefaultValue() : res;
51-
}
52-
53-
/**
54-
* Get a PID source which tracks the x-coordinate of the target.
55-
*
56-
* @return A PID source tracking the x-coordinate of the target.
57-
*/
58-
public PIDSource xPID() {
59-
return new DisplacementPIDSource() {
60-
@Override
61-
public double pidGet() {
62-
return getLastResult().xError();
63-
}
64-
};
28+
protected T getLastResult(){
29+
return m_visionConnector.getLastResult();
6530
}
6631

6732
/**
68-
* Get a PID source which tracks the y-coordinate of the target.
69-
*
70-
* @return A PID source tracking the y-coordinate of the target.
33+
*
34+
* @param targets The list of targets found.
7135
*/
72-
public PIDSource yPID() {
73-
return new DisplacementPIDSource() {
74-
@Override
75-
public double pidGet() {
76-
return getLastResult().yError();
77-
}
78-
};
36+
@Override
37+
public final void process(List<Rect> targets) {
38+
T res = computeResult(targets);
39+
m_visionConnector.updateResult(res);
7940
}
8041
}
42+

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

Lines changed: 164 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,164 @@
1+
package org.hyperonline.hyperlib.vision;
2+
3+
import edu.wpi.first.networktables.*;
4+
import edu.wpi.first.wpilibj.PIDSource;
5+
import org.hyperonline.hyperlib.pid.DisplacementPIDSource;
6+
7+
import java.util.Objects;
8+
9+
/**
10+
* Base class with common functionality used by VisionConnectors. In particular,
11+
* this provides methods to store, publish, and subscribe to a result,
12+
* and access it via PID sources.
13+
*
14+
* @param <T> VisionResult type of this AbstractVisionConnector
15+
* @author Chris McGroarty
16+
*/
17+
public abstract class AbstractVisionConnector<T extends VisionResult> implements VisionConnector {
18+
19+
protected final NetworkTableEntry m_xError;
20+
protected final NetworkTableEntry m_yError;
21+
protected final NetworkTableEntry m_xAbs;
22+
protected final NetworkTableEntry m_yAbs;
23+
protected final NetworkTableEntry m_foundTarget;
24+
protected final NetworkTableEntry m_lastResultTimestamp;
25+
protected T m_lastResult;
26+
protected NetworkTable m_table;
27+
private String m_mainTableName = "hypervision";
28+
private String m_subTableName;
29+
private NetworkTableInstance m_inst;
30+
private int m_listenerID;
31+
32+
protected AbstractVisionConnector(String subTableName, NetworkTableInstance inst) {
33+
m_subTableName = Objects.requireNonNull(subTableName);
34+
35+
m_inst = Objects.requireNonNull(inst);
36+
m_table = m_inst.getTable(getTableName());
37+
m_lastResult = getDefaultValue();
38+
39+
m_xError = m_table.getEntry("xError");
40+
m_yError = m_table.getEntry("yError");
41+
m_xAbs = m_table.getEntry("xAbsolute");
42+
m_yAbs = m_table.getEntry("yAbsolute");
43+
m_foundTarget = m_table.getEntry("foundTarget");
44+
m_lastResultTimestamp = m_table.getEntry("lastResultTimestamp");
45+
}
46+
47+
private String getTableName() {
48+
return "/" + m_mainTableName + "/" + m_subTableName;
49+
}
50+
51+
/**
52+
* add listener for the lastResultTimestamp entry in NetworkTables
53+
* process the next result on an update
54+
*/
55+
public void subscribe() {
56+
m_listenerID = m_table.addEntryListener("lastResultTimestamp", this::next, EntryListenerFlags.kUpdate);
57+
}
58+
59+
/**
60+
* remove listener for the lastResultTimestamp entry in NetworkTables
61+
*/
62+
public void unsubscribe() {
63+
m_table.removeEntryListener(m_listenerID);
64+
}
65+
66+
/**
67+
* @return {T}
68+
*/
69+
public abstract T getDefaultValue();
70+
71+
/**
72+
* @param result {T}
73+
*/
74+
public void updateResult(T result) {
75+
if (m_lastResult == null) {
76+
m_lastResult = getDefaultValue();
77+
}
78+
if (!m_lastResult.equals(result)) {
79+
m_lastResult = (result == null ? getDefaultValue() : result);
80+
if (m_inst.isConnected()) {
81+
this.publish();
82+
}
83+
} else {
84+
System.out.println("NetworkTableInstance(69) not connected");
85+
}
86+
}
87+
88+
/**
89+
* publish VisionResult values to corresponding NetworkTable Entries
90+
* should be overriden and supered by any children of type T extends VisionResult
91+
*/
92+
public void publish() {
93+
m_lastResultTimestamp.setDouble(System.currentTimeMillis());
94+
m_xError.setDouble(m_lastResult.xError());
95+
m_yError.setDouble(m_lastResult.yError());
96+
m_xAbs.setDouble(m_lastResult.xAbsolute());
97+
m_yAbs.setDouble(m_lastResult.yAbsolute());
98+
m_foundTarget.setBoolean(m_lastResult.foundTarget());
99+
}
100+
101+
/**
102+
* retrieve a VisionResult from NetworkTable Entries
103+
* should be overriden and supered by any children of type T extends VisionResult
104+
*
105+
* @return {VisionResult}
106+
*/
107+
protected VisionResult retrieve() {
108+
return new VisionResult(
109+
m_xError.getDouble(0),
110+
m_yError.getDouble(0),
111+
m_xAbs.getDouble(0),
112+
m_yAbs.getDouble(0),
113+
m_foundTarget.getBoolean(false)
114+
);
115+
}
116+
117+
/**
118+
* evaluate and process the results in NetworkTables to my lastResult
119+
*
120+
* @param table NetworkTable to get values from
121+
* @param key Key of the entry that triggered the Listener
122+
* @param entry Entry that was updated
123+
* @param value the new Value
124+
* @param flags Listener flags (created, updated, removed, etc)
125+
*/
126+
protected abstract void next(NetworkTable table, String key, NetworkTableEntry entry,
127+
NetworkTableValue value, int flags);
128+
129+
130+
/**
131+
* @return {T}
132+
*/
133+
public T getLastResult() {
134+
return m_lastResult;
135+
}
136+
137+
/**
138+
* Get a PID source which tracks the x-coordinate of the target.
139+
*
140+
* @return A PID source tracking the x-coordinate of the target.
141+
*/
142+
public PIDSource xPID() {
143+
return new DisplacementPIDSource() {
144+
@Override
145+
public double pidGet() {
146+
return getLastResult().xError();
147+
}
148+
};
149+
}
150+
151+
/**
152+
* Get a PID source which tracks the y-coordinate of the target.
153+
*
154+
* @return A PID source tracking the y-coordinate of the target.
155+
*/
156+
public PIDSource yPID() {
157+
return new DisplacementPIDSource() {
158+
@Override
159+
public double pidGet() {
160+
return getLastResult().yError();
161+
}
162+
};
163+
}
164+
}

0 commit comments

Comments
 (0)