def __init__(self) -> None:
self.last_pong = time.monotonic()
# 50Hz control loop for 2 seconds
self.odometry: Deque[Odometry] = deque(maxlen=50 * 2)
self.ntinst = NetworkTablesInstance()
if hal.isSimulation():
self.ntinst.startTestMode(server=False)
else:
self.ntinst.startClient("10.47.74.6") # Raspberry pi's IP
self.ntinst.setUpdateRate(1) # ensure our flush calls flush immediately
self.fiducial_x_entry = self.ntinst.getEntry("/vision/fiducial_x")
self.fiducial_y_entry = self.ntinst.getEntry("/vision/fiducial_y")
self.fiducial_time_entry = self.ntinst.getEntry("/vision/fiducial_time")
self.ping_time_entry = self.ntinst.getEntry("/vision/ping")
self.raspi_pong_time_entry = self.ntinst.getEntry("/vision/raspi_pong")
self.rio_pong_time_entry = self.ntinst.getEntry("/vision/rio_pong")
self.latency_entry = self.ntinst.getEntry("/vision/clock_offset")
self.processing_time_entry = self.ntinst.getEntry("/vision/processing_time")
self.camera_entry = self.ntinst.getEntry("/vision/game_piece")
def main():
global tableInstance
pipeline = TapeRecognitionPipeline()
#cs = CameraServer.getInstance()
# Capture from the first USB Camera on the system
#camera = UsbCamera(name="Camera rPi Camera 0",path="/dev/video0")
#server = cs.startAutomaticCapture(camera=camera,return_server=True)
#camera.setConnectionStrategy(VideoSource.ConnectionStrategy.kKeepOpen)
#camera.setResolution(320, 240)
capture = cv2.VideoCapture(0)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
tableInstance = NetworkTablesInstance()
tableInstance.initialize(server="roboRIO-3319-FRC.local")
print(tableInstance.isConnected())
# Get a CvSink. This will capture images from the camera
#cvSink = cs.getVideo()
#cvSink.setEnabled(True)
# Allocating new images is very expensive, always try to preallocate
img = numpy.zeros(shape=(240, 320, 3), dtype=numpy.uint8)
while True:
# Tell the CvSink to grab a frame from the camera and put it
# in the source image. If there is an error notify the output.
retval, img = capture.read(img)
#
# Insert your image processing logic here!
#
pipeline.process(img)
class NetworkTableWrapper:
connected = False
made = True
# all tables
tables = NetworkTablesInstance()
ccTable = None
noteTable = None
CCReturn = None
NoteReturn = None
CCreceiveAction = None
NoteReceiveAction = None
CCreceivers = []
NoteReceivers = []
conectedAction = None
# magic copied code to make network tables start
def __init__(self, ip, conectedAction, CCreciveAction, NoteReciveAction):
self.connected = False
self.conectedAction = conectedAction
self.CCreceiveAction = CCreciveAction
self.NoteReceiveAction = NoteReciveAction
cond = threading.Condition()
notified = [False]
def connection_listener(connected, info):
print(info)
self.connected = connected
self.ccTable = self.tables.getTable("ccTable")
self.noteTable = self.tables.getTable("noteTable")
self.CCReturn = self.tables.getTable("CCReturn")
self.NoteReturn = self.tables.getTable("NoteReturn")
with cond:
notified[0] = True
cond.notify()
# to prevent runing agin if robot disconects and reconects
if self.made:
self.create_receivers()
self.create_received_listeners()
self.made = False
self.conectedAction(connected)
self.tables.startClient(ip)
self.tables.setUpdateRate(0.01)
self.tables.addConnectionListener(connection_listener,
immediateNotify=True)
with cond:
if not notified[0]:
cond.wait()
# actually runs the action
def update_on_cc_receive(self, key, value):
# in python, but not java, the keys are formated like paths so we need to take off all the garbage
k = str(key).split("/")
id_ = k[2]
self.CCreceiveAction(int(id_), value)
def update_on_note_receive(self, key, value):
# in python, but not java, the keys are formated like paths so we need to take off all the garbage
k = str(key).split("/")
id_ = k[2]
self.NoteReceiveAction(int(id_), value)
# creates a array of receivers to receive MIDI updates from networktables
def create_receivers(self):
for x in range(128):
self.CCreceivers.append(self.CCReturn.getEntry(str(x)))
self.NoteReceivers.append(self.NoteReturn.getEntry(str(x)))
# adds listeners to the receivers that run the action on change
def create_received_listeners(self):
for x in self.CCreceivers:
x.addListener(lambda a, b, c, d: self.update_on_cc_receive(b, c),
20)
for x in self.NoteReceivers:
x.addListener(lambda a, b, c, d: self.update_on_note_receive(b, c),
20)
# sends a updated note to the robot
def update_note(self, note, value):
self.noteTable.putBoolean(str(note), value)
# sends a updated cc to the robot
def update_cc(self, cc, value):
self.ccTable.putNumber(str(cc), value)
# stops networktables
def stop(self):
self.tables.stopClient()
frame = createAnnotatedDisplay(frame, ((x, y), radius))
dist, offset = getDistance(closestToMiddle)
return (frame, dist, offset)
if __name__ == "__main__":
if len(sys.argv) >= 2:
configFile = sys.argv[1]
# read configuration
cameraConfigs = readConfig()
# start NetworkTables
ntinst = NetworkTablesInstance()
ntinst.startServer()
ntinst.setUpdateRate(1)
NetworkTables.initialize(server="10.47.74.2")
NetworkTables.setUpdateRate(1)
ping = ntinst.getEntry("/vision/ping")
raspi_pong = ntinst.getEntry("/vision/raspi_pong")
rio_pong = ntinst.getEntry("/vision/rio_pong")
entry_game_piece = ntinst.getEntry("/vision/game_piece")
entry_dist = ntinst.getEntry("/vision/fiducial_x")
entry_offset = ntinst.getEntry("/vision/fiducial_y")
entry_fiducial_time = ntinst.getEntry("/vision/fiducial_time")
entry_camera = ntinst.getEntry("/vision/using_cargo_camera")