def nt(request):
"""Starts/stops global networktables instance for testing"""
NetworkTables.startTestMode(server=request.param)
yield NetworkTables
NetworkTables.shutdown()
def init_networktables():
NetworkTables.setNetworkIdentity("pynetworktables2js")
logger.info("Connecting to networktables at %s", "127.0.0.1")
NetworkTables.initialize(server="127.0.0.1")
logger.info("Networktables Initialized")
def shutdownNtClient(self):
"""
Shuts down the NT Client.
:return:
"""
try:
self._set_status(self.STATUS_CLIENT_STOPPING)
NetworkTable.shutdown()
self._set_status(self.STATUS_CLIENT_STOPPED)
except:
self._set_status(self.STATUS_ERROR)
self.logger.error('Error stopping NT client: %s' % traceback.format_exc())
def getTable(cls) -> NetworkTable:
if cls.table is None:
from networktables import NetworkTables
cls.table = NetworkTables.getTable("SmartDashboard")
hal.report(hal.UsageReporting.kResourceType_SmartDashboard, 0)
return cls.table
def collect_feedbacks(component, cname: str, prefix="components"):
"""
Finds all methods decorated with :func:`feedback` on an object
and returns a list of 2-tuples (method, NetworkTables entry).
.. note:: This isn't useful for normal use.
"""
if prefix is None:
prefix = "/%s" % cname
else:
prefix = "/%s/%s" % (prefix, cname)
nt = NetworkTables.getTable(prefix)
feedbacks = []
for name, method in inspect.getmembers(component, inspect.ismethod):
if getattr(method, "_magic_feedback", False):
key = method._magic_feedback_key
if key is None:
if name.startswith("get_"):
key = name[4:]
else:
key = name
entry = nt.getEntry(key)
feedbacks.append((method, entry))
return feedbacks
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = [
'logger', 'members'
]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector('autonomous')
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable('/robot')
self.__nt.putBoolean('is_simulation', self.isSimulation())
self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
def __init__(self):
parser = ArgumentParser()
parser.add_argument("client", nargs="?", default=None)
parser.add_argument("-r", "--rate", default=None, type=float)
parser.add_argument("--send", default=False, action="store_true")
args = parser.parse_args()
if args.client:
NetworkTables.initialize(server=args.client)
else:
NetworkTables.initialize()
# Default write flush is 0.05, could adjust for less latency
if args.rate is not None:
print("Setting rate to %s" % args.rate)
NetworkTables.setUpdateRate(args.rate)
self.nt = NetworkTables.getTable("/benchmark")
self.updates = 0
self.nt.addTableListener(self.on_update)
if args.send:
self.send_benchmark()
else:
self.recv_benchmark()
def _get_status(self):
"""
Private - Reutrns the NTAL status as a string.
:return:
"""
if self.ntal_status == self.STATUS_CLIENT_STARTED_CONNECTING and NetworkTable.isConnected():
return self.STATUS_CLIENT_CONNECTED
else:
return self.ntal_status
def __init__(self):
"""Creates a preference class that will automatically read the file in
a different thread. Any call to its methods will be blocked until the
thread is finished reading.
"""
self.table = NetworkTables.getTable(self.TABLE_NAME)
self.table.addTableListenerEx(self.valueChangedEx, NetworkTables.NotifyFlags.NEW | NetworkTables.NotifyFlags.IMMEDIATE)
hal.report(hal.UsageReporting.kResourceType_Preferences, 0)
def __init__(self):
# TODO: StartCAPI()
# TODO: See if the next line is necessary
# Resource.RestartProgram()
NetworkTables.setNetworkIdentity("Robot")
if not RobotBase.isSimulation():
NetworkTables.setPersistentFilename("/home/lvuser/networktables.ini")
NetworkTables.setServerMode()
from .driverstation import DriverStation
self.ds = DriverStation.getInstance()
NetworkTables.getTable("") # forces network tables to initialize
NetworkTables.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", False)
self.__initialized = True
def __init__(self):
client = False
if len(sys.argv) > 1:
client = True
NetworkTables.initialize(server=sys.argv[1])
# Default write flush is 0.05, could adjust for less latency
#NetworkTable.setWriteFlushPeriod(0.01)
self.nt = NetworkTables.getTable('/benchmark')
self.updates = 0
if client:
self.nt.addTableListener(self.on_update)
self.recv_benchmark()
else:
self.send_benchmark()
def nt(request):
'''Starts/stops global networktables instance for testing'''
NetworkTables.setTestMode(server=request.param)
NetworkTables.initialize()
yield NetworkTables
NetworkTables.shutdown()
def main():
global idNum
while True:
plDone = False
didStreamSucceed = False
while didStreamSucceed is False:
if plDone is False:
try:
print "Creating pipeline"
pipeline = GripPipeline.GripPipeline()
print "Pipeline created"
plDone = True
except Exception as err:
print "Error creating pipeline: " + str(err)
if plDone is True:
try:
print "Opening Stream"
camTable = NetworkTables.getTable('CameraPublisher/USB Camera 0')
cameras = camTable.getStringArray('streams')
print cameras
return
# for item in cameras:
# print item
stream = cv2.VideoCapture('http://10.42.37.2:1181/stream.mjpg')
print "Stream Opened"
didStreamSucceed = True
except Exception as err:
print "Error creating stream: " + str(err)
print "Initialization Complete"
while stream.isOpened():
ret, img = stream.read()
if ret is True:
pipeline.process(img)
if len(pipeline.filter_contours_output) >= 1 and (idNum % 10) is 0:
print "Writing image to usb"
cv2.imwrite("/mnt/usb/" + str(idNum) + "raw.jpg", img)
cv2.imwrite("/mnt/usb/" + str(idNum) + "filtered.jpg", pipeline.hsl_threshold_output)
extra_processing(pipeline)
else:
print "Error reading from camera"
break
def update(self):
"""
Updates both Network Table and CBHAL with new data if connected.
:return:
"""
try:
if NetworkTable.isConnected():
self.putNtData()
self.getNtData()
if self._get_status() == self.STATUS_ERROR:
self.logger.info('Error cleared.')
self._set_status(self.STATUS_CLIENT_STARTED_CONNECTING)
except:
if self._get_status() != self.STATUS_ERROR:
self.logger.error('Error during update: %s' % traceback.format_exc())
self._set_status(self.STATUS_ERROR)
def log_status_changes(self):
"""
Log any chagnes to the logger.
:return:
"""
status = self._get_status()
if status != self.last_ntal_log_status or self.address != self.last_address:
if status == self.STATUS_CLIENT_CONNECTED:
self.logger.info('Connected to %s' % NetworkTable.getRemoteAddress())
if status == self.STATUS_CLIENT_STARTED_CONNECTING and self.last_ntal_log_status == self.STATUS_CLIENT_CONNECTED:
self.logger.info('Disconnected')
if status == self.STATUS_CLIENT_STARTED_CONNECTING:
self.logger.info('Trying to connect to %s' % self.address)
if status == self.STATUS_CLIENT_STOPPED:
self.logger.info('Disabled')
self.last_address = self.address
self.last_ntal_log_status = status
def __init__(self) -> None:
self.table = NetworkTables.getTable("FMSInfo")
self.typeMetadata = self.table.getEntry(".type")
self.typeMetadata.forceSetString("FMSInfo")
self.gameSpecificMessage = self.table.getEntry("GameSpecificMessage")
self.gameSpecificMessage.forceSetString("")
self.eventName = self.table.getEntry("EventName")
self.eventName.forceSetString("")
self.matchNumber = self.table.getEntry("MatchNumber")
self.matchNumber.forceSetDouble(0)
self.replayNumber = self.table.getEntry("ReplayNumber")
self.replayNumber.forceSetDouble(0)
self.matchType = self.table.getEntry("MatchType")
self.matchType.forceSetDouble(0)
self.alliance = self.table.getEntry("IsRedAlliance")
self.alliance.forceSetBoolean(True)
self.station = self.table.getEntry("StationNumber")
self.station.forceSetDouble(0)
self.controlWord = self.table.getEntry("FMSControlData")
self.controlWord.forceSetDouble(0)
def startNtClient(self):
"""
Starts up the NT Client.
:return:
"""
if self.ntal_status is not self.STATUS_CLIENT_STARTED_CONNECTING and not NetworkTable.isConnected():
self._set_status(self.STATUS_CLIENT_STARTING)
try:
NetworkTable.setUpdateRate(interval=self.flush_period)
NetworkTable.initialize(self.address)
self.nt = NetworkTable.getTable('OperatorInterfaceControlBoard')
self._set_status(self.STATUS_CLIENT_STARTED_CONNECTING)
except:
self._set_status(self.STATUS_ERROR)
self.logger.error('Error starting NT client: %s' % traceback.format_exc())
def setup_tunables(component, cname: str, prefix: Optional[str] = "components") -> None:
"""
Connects the tunables on an object to NetworkTables.
:param component: Component object
:param cname: Name of component
:param prefix: Prefix to use, or no prefix if None
.. note:: This is not needed in normal use, only useful
for testing
"""
cls = component.__class__
if prefix is None:
prefix = "/%s" % cname
else:
prefix = "/%s/%s" % (prefix, cname)
tunables = {}
for n in dir(cls):
if n.startswith("_"):
continue
prop = getattr(cls, n)
if not isinstance(prop, tunable):
continue
if prop._ntsubtable:
key = "%s/%s/%s" % (prefix, prop._ntsubtable, n)
else:
key = "%s/%s" % (prefix, n)
ntvalue = NetworkTables.getGlobalAutoUpdateValue(
key, prop._ntdefault, prop._ntwritedefault
)
tunables[prop] = ntvalue
component._tunables = tunables
def mainloop():
#Get currnt time as a string
currentTime = time.localtime(time.time())
timeString = str(currentTime.tm_year) + str(currentTime.tm_mon) + str(
currentTime.tm_mday) + str(currentTime.tm_hour) + str(
currentTime.tm_min)
#Open a log file
logFilename = '/data/Logs/Run_Log_' + timeString + '.txt'
log_file = open(logFilename, 'w')
log_file.write('Run started on %s.\n' % datetime.datetime.now())
log_file.write('')
#Load VMX module
vmxpi = imp.load_source('vmxpi_hal_python',
'/usr/local/lib/vmxpi/vmxpi_hal_python.py')
vmx = vmxpi.VMXPi(False, 50)
if vmx.IsOpen() is False:
log_file.write('Error: Unable to open VMX Client.\n')
log_file.write('\n')
log_file.write(
' - Is pigpio (or the system resources it requires) in use by another process?\n'
)
log_file.write(' - Does this application have root privileges?')
log_file.close()
sys.exit(0)
#Open connection to USB camera (video device 0)
camera = cs.UsbCamera("usbcam", 0)
camera.setVideoMode(cs.VideoMode.PixelFormat.kMJPEG, imgwidth, imgheight,
frames_per_sec)
#Start raw Video Streaming Server
mjpegServer = cs.MjpegServer("httpserver", 8081)
mjpegServer.setSource(camera)
#Define video sink
cvsink = cs.CvSink("cvsink")
cvsink.setSource(camera)
#Define video source
cvsource = cs.CvSource("cvsource", cs.VideoMode.PixelFormat.kMJPEG,
imgwidth, imgheight, frames_per_sec)
#Start processed Video Streaming Server
cvMjpegServer = cs.MjpegServer("cvhttpserver", 8082)
cvMjpegServer.setSource(cvsource)
#Create blank image
img = np.zeros(shape=(imgheight, imgwidth, 3), dtype=np.uint8)
#Set video codec and create VideoWriter
fourcc = cv.VideoWriter_fourcc(*'XVID')
videoFilename = '/data/Match Videos/robotcam-' + timeString + '.avi'
imgout = cv.VideoWriter(videoFilename, fourcc, 20.0, (320, 240))
#Connect NetworkTables
NetworkTables.initialize(server="10.41.21.2")
navxTable = NetworkTables.getTable("navx")
visionTable = NetworkTables.getTable("vision")
smartDash = NetworkTables.getTable("SmartDashboard")
#Reset yaw gyro
if not vmx.getAHRS().IsCalibrating:
vmx.getAHRS().ZeroYaw()
#Reset displacement
vmx.getAHRS().ResetDisplacement()
#Start main processing loop
while (True):
#Grab a frame from video feed
video_timestamp, img = cvsink.grabFrame(img)
#Check for frame error
if video_timestamp == 0:
log_file.write('Video error: \n')
log_file.write(cvsink.getError())
log_file.write('\n')
sleep(float(frames_per_sec * 2) / 1000.0)
continue
#Find contours in image
img_contours, cubeDistance, cubeAngle = detect_contours(img)
#Put contour info on image
if cubeDistance != -1:
distanceString = 'Distance: %d' % cubeDistance
cv.putText(img_contours, distanceString, (10, 15),
cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
cv.LINE_AA)
angleString = 'Offset angle: %d' % cubeAngle
cv.putText(img_contours, angleString, (10, 30),
cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
cv.LINE_AA)
#Put IMU info on image
angleString = 'Angle: %.2f' % vmx.getAHRS().GetAngle()
cv.putText(img, angleString, (30, 70), cv.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1, cv.LINE_AA)
yVelocityString = 'Y Vel: %.2f' % vmx.getAHRS().GetVelocityY()
cv.putText(img, yVelocityString, (30, 90), cv.FONT_HERSHEY_SIMPLEX,
0.5, (0, 255, 0), 1, cv.LINE_AA)
xVelocityString = 'X Vel: %.2f' % vmx.getAHRS().GetVelocityX()
cv.putText(img, xVelocityString, (30, 110), cv.FONT_HERSHEY_SIMPLEX,
0.5, (0, 255, 0), 1, cv.LINE_AA)
yDispString = 'Y Disp: %.2f' % vmx.getAHRS().GetDisplacementY()
cv.putText(img, yDispString, (30, 130), cv.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1, cv.LINE_AA)
xDispString = 'X Disp: %.2f' % vmx.getAHRS().GetDisplacementX()
cv.putText(img, xDispString, (30, 150), cv.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 1, cv.LINE_AA)
#Update network tables
navxTable.putNumber("SensorTimestamp",
vmx.getAHRS().GetLastSensorTimestamp())
navxTable.putNumber("DriveAngle", vmx.getAHRS().GetAngle())
navxTable.putNumber("YVelocity", vmx.getAHRS().GetVelocityY())
navxTable.putNumber("XVelocity", vmx.getAHRS().GetVelocityX())
navxTable.putNumber("YDisplacement", vmx.getAHRS().GetDisplacementY())
navxTable.putNumber("XDisplacement", vmx.getAHRS().GetDisplacementX())
visionTable.putNumber("CubeAngle", cubeAngle)
visionTable.putNumber("CubeDistance", cubeDistance)
smartDash.putNumber("DriveAngle", vmx.getAHRS().GetAngle())
smartDash.putNumber("CubeAngle", cubeAngle)
smartDash.putNumber("CubeDistance", cubeDistance)
#Show image
cv.imshow('My Webcam', img_contours)
#Put frame in video stream
cvsource.putFrame(img_contours)
#Write image to file
if writeVideo:
imgout.write(img_contours)
#Check for stop code from robot
if cv.waitKey(1) == 27:
break
#robotStop = visionTable.getNumber("RobotStop", 0)
#if robotStop == 1:
# break
#Close all open windows
cv.destroyAllWindows()
#Close video file
imgout.release()
#Close the log file
log_file.write('Run stopped on %s.' % datetime.datetime.now())
import sys
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
def valueChanged(table, key, value, isNew):
print("valueChanged: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
def connectionListener(connected, info):
print(info, "; Connected=%s" % connected)
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
sd = NetworkTables.getTable("SmartDashboard")
sd.addEntryListener(valueChanged)
def main():
# Allow Rio to boot and configure network
time.sleep(5.0)
# Setup logging
logging.basicConfig(level=logging.DEBUG)
# Setup NetworkTables
NetworkTables.initialize(server='10.8.34.2')
# Create table for values
evs = NetworkTables.getTable('EVS')
# sd = NetworkTables.getTable('SmartDashboard')
# Create sub-tables and append them to arrays: 3 for hatches, 3 for balls, and 6 for tape
hatchTables = []
ballTables = []
tapeTables = []
hatch0 = evs.getSubTable('Hatch0')
hatchTables.append(hatch0)
hatch1 = evs.getSubTable('Hatch1')
hatchTables.append(hatch1)
hatch2 = evs.getSubTable('Hatch2')
hatchTables.append(hatch2)
ball0 = evs.getSubTable('Ball0')
ballTables.append(ball0)
ball1 = evs.getSubTable('Ball1')
ballTables.append(ball1)
ball2 = evs.getSubTable('Ball2')
ballTables.append(ball2)
tape0 = evs.getSubTable('Tape0')
tapeTables.append(tape0)
tape1 = evs.getSubTable('Tape1')
tapeTables.append(tape1)
tape2 = evs.getSubTable('Tape2')
tapeTables.append(tape2)
tape3 = evs.getSubTable('Tape3')
tapeTables.append(tape3)
tape4 = evs.getSubTable('Tape4')
tapeTables.append(tape4)
tape5 = evs.getSubTable('Tape5')
tapeTables.append(tape5)
# Setup EdgeIQ
obj_detect = edgeiq.ObjectDetection("CAP1Sup/2019_FRC_GamePieces_Dev_v3")
obj_detect.load(engine=edgeiq.Engine.DNN_OPENVINO)
# Print out info
print("Loaded model:\n{}\n".format(obj_detect.model_id))
print("Engine: {}".format(obj_detect.engine))
print("Accelerator: {}\n".format(obj_detect.accelerator))
print("Labels:\n{}\n".format(obj_detect.labels))
# Get the FPS
fps = edgeiq.FPS()
#cross_frame_tracker = edgeiq.CorrelationTracker()
#tracker = edgeiq.CentroidTracker()
# sd.putString('DB/String 3', default_conf_thres)
try:
with edgeiq.WebcamVideoStream(cam=0) as video_stream:
# Allow Webcam to warm up
time.sleep(2.0)
fps.start()
for i in range(0, 2):
hatchTables[i].putBoolean('inUse', False)
ballTables[i].putBoolean('inUse', False)
tapeTables[i].putBoolean('inUse', False)
tapeTables[i + 3].putBoolean('inUse', False)
# loop detection
while True:
confidence_thres = default_conf_thres
frame = video_stream.read()
results = obj_detect.detect_objects(
frame, confidence_level=confidence_thres)
#objects = tracker.update(results.predictions)
'''
frame = edgeiq.markup_image(
frame, results.predictions, colors=obj_detect.colors)
'''
#Counters - they reset after every frame in the while loop
hatchCounter = 0
ballCounter = 0
tapeCounter = 0
# Update the EVS NetworkTable with new values
for prediction in results.predictions:
center_x, center_y = prediction.box.center
# Code goes here
numValues = [
center_x, center_y, prediction.box.end_x,
prediction.box.end_y, prediction.box.area,
(prediction.confidence * 100)
]
for entry in range(0, len(numValues) - 1):
numValues[entry] = round(numValues[entry], 3)
numValuesArray = np.asarray(numValues)
#
# IMPORTANT:
# Names of labels have not been decided yet
#
#
if prediction.label == "Hatch":
hatchTables[hatchCounter].putNumberArray(
'values', numValuesArray)
# Boolean asks to update
hatchTables[hatchCounter].putBoolean('inUse', True)
hatchCounter += 1
elif prediction.label == "Ball":
ballTables[ballCounter].putNumberArray(
'values', numValuesArray)
# Boolean asks to update
ballTables[ballCounter].putBoolean('inUse', True)
ballCounter += 1
elif prediction.label == "Tape":
tapeTables[tapeCounter].putNumberArray(
'values', numValuesArray)
# Boolean asks to update
tapeTables[tapeCounter].putBoolean('inUse', True)
tapeCounter += 1
# Sets the value after the last value to false. The Rio will stop when it finds a False
hatchTables[hatchCounter].putBoolean('inUse', False)
ballTables[ballCounter].putBoolean('inUse', False)
tapeTables[tapeCounter].putBoolean('inUse', False)
evs.putBoolean('checked', False)
# Generate text to display on streamer
text = ["Model: {}".format(obj_detect.model_id)]
text.append("Inference time: {:1.3f} s".format(
results.duration))
text.append("Objects:")
# Format and display values on localhost streamer
for prediction in results.predictions:
text.append("{}: {:2.2f}%".format(
prediction.label, prediction.confidence * 100))
'''
streamer.send_data(frame, text)
'''
fps.update()
'''
if streamer.check_exit():
break
'''
finally:
fps.stop()
print("elapsed time: {:.2f}".format(fps.get_elapsed_seconds()))
print("approx. FPS: {:.2f}".format(fps.compute_fps()))
print("Program Ending")
yres = 256
# blur radius (odd number)
blur_const = 3
# horizontal fov of camera for calculating angle
horizontal_fov = 64.4
# roborio socket
rio_ip = "10.37.86.2"
rio_port = 3000
rio_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# initialize NetworkTables
NetworkTables.initialize(server=args["serverip"])
nwt = NetworkTables.getTable(args["table"])
# push ip to NetworkTables
if not args["environment"]:
ip = ni.ifaddresses('eth0')[ni.AF_INET][0]['addr']
nwt.putString("rpi.ip", ip)
# Control Window
if args["gui"]:
cv2.namedWindow('Control', cv2.WINDOW_NORMAL)
# read and parse data file if present - otherwise use default values
try:
with open(dataFile, 'r') as f:
data = f.read()
import cv2
import numpy
import time
from networktables import NetworkTables
NetworkTables.initialize(server='10.57.16.2')
table = NetworkTables.getTable('SmartDashboard')
class VideoRecorder():
def __init__(self):
self.video = cv2.VideoCapture(0)
self.color_data = "thing"
self.hexagon_status = ""
self.width = self.video.get(3)
self.height = self.video.get(4)
# Divides videos width and height into 3, to create the quadrants
self.q_width = self.width/3
self.q_height = self.height/3
#green_color_range
self.green_upper = numpy.array([43,223,109], numpy.uint8)
self.green_lower = numpy.array([13,92,42], numpy.uint8)
def robotInit(self):
# Pull in smart dashboard info...
self.sd = NetworkTables.getTable("SmartDashboard")
# Start a timer....
self.timer = wpilib.Timer()
"""Robot initialization function. The Low level is to use the brushless function on the controllers."""
if wpilib.RobotBase.isSimulation():
self.frontLeftMotor = wpilib.Spark(self.frontLeftChannel)
self.rearLeftMotor = wpilib.Spark(self.rearLeftChannel)
self.frontRightMotor = wpilib.Spark(self.frontRightChannel)
self.rearRightMotor = wpilib.Spark(self.rearRightChannel)
else:
self.frontLeftMotor = rev.CANSparkMax(
self.frontLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearLeftMotor = rev.CANSparkMax(
self.rearLeftChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.frontRightMotor = rev.CANSparkMax(
self.frontRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
self.rearRightMotor = rev.CANSparkMax(
self.rearRightChannel,
rev.CANSparkMaxLowLevel.MotorType.kBrushless)
# The channel on the driver station that the joystick is connected to
joystickChannel = 0
m_frontLeftLocation = Translation2d(0.381, 0.381)
m_frontRightLocation = Translation2d(0.381, -0.381)
m_backLeftLocation = Translation2d(-0.381, 0.381)
m_backRightLocation = Translation2d(-0.381, -0.381)
# Creating my kinematics object using the wheel locations.
self.m_kinematics = MecanumDriveKinematics(m_frontLeftLocation,
m_frontRightLocation,
m_backLeftLocation,
m_backRightLocation)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
# Example chassis speeds: 1 meter per second forward, 3 meters
# per second to the left, and rotation at 1.5 radians per second
# counterclockwise.
preChassis = ChassisSpeeds()
preChassis.vx = 1.0
preChassis.vy = 0.0
preChassis.omega = 0.0
# Convert to wheel speeds
wheelSpeeds = MecanumDriveKinematics.toWheelSpeeds(preChassis)
# Get the individual wheel speeds
frontLeft = wheelSpeeds.frontLeftMetersPerSecond
frontRight = wheelSpeeds.frontRightMetersPerSecond
backLeft = wheelSpeeds.rearLeftMetersPerSecond
backRight = wheelSpeeds.rearRightMetersPerSecond
def set_starting_positions(self, positions):
table = NetworkTables.getTable("autonomous/" + self.MODE_NAME +
"/starting_position")
return table.putStringArray("options", positions)
camera.setConfigJson(json.dumps(config.config))
return cs, camera
if __name__ == "__main__":
if len(sys.argv) >= 2:
configFile = sys.argv[1]
# read configuration
if not readConfig():
sys.exit(1)
# start NetworkTables
ntinst = NetworkTablesInstance.getDefault()
#Name of network table - this is how it communicates with robot. IMPORTANT
networkTable = NetworkTables.getTable('ChickenVision')
if server:
print("Setting up NetworkTables server")
ntinst.startServer()
else:
print("Setting up NetworkTables client for team {}".format(team))
ntinst.startClientTeam(team)
# start cameras
cameras = []
streams = []
for cameraConfig in cameraConfigs:
cs, cameraCapture = startCamera(cameraConfig)
streams.append(cs)
cameras.append(cameraCapture)
from networktables import NetworkTables as NT
import time
NT.startServer(persistFilename='networktables.ini',
listenAddress='192.168.137.1',
port=1735)
blingSelect = NT.getTable("Bling").getEntry("blingSelect")
blingSelect.setString("idle")
NT.getTable("FMSInfo").getEntry("IsRedAlliance").setBoolean(True)
print("Server started and values set")
while True:
blingSelect.setString(input("blingSelect: "))
class grip:
"""
An OpenCV pipeline generated by GRIP.
"""
NetworkTables.initialize(server='localhost')
vision = NetworkTables.getTable("Root/Vision")
def __init__(self):
"""initializes all values to presets or None if need to be set
"""
self.__hsv_threshold_hue = [0.0, 180.0]
self.__hsv_threshold_saturation = [1.3326218036190145, 255.0]
self.__hsv_threshold_value = [166.47993895223533, 255.0]
self.hsv_threshold_output = True
self.__find_blobs_input = self.hsv_threshold_output
self.__find_blobs_min_area = 0.0
self.__find_blobs_circularity = [0.0, 0.9077575574989633]
self.__find_blobs_dark_blobs = False
self.find_blobs_output = True
self.__distance_transform_input = self.hsv_threshold_output
self.__distance_transform_type = cv2.DIST_L2
self.__distance_transform_mask_size = 3
self.distance_transform_output = True
self.__find_contours_input = self.distance_transform_output
self.__find_contours_external_only = True
self.find_contours_output = None
def process(self):
"""
Runs the pipeline and sets all outputs to new values.
"""
# Step HSV_Threshold0:
self.__hsv_threshold_input = ("http://frcvision.local:1181.mjpeg")
(self.hsv_threshold_output) = self.__hsv_threshold(
self.__hsv_threshold_input, self.__hsv_threshold_hue,
self.__hsv_threshold_saturation, self.__hsv_threshold_value)
# Step Find_Blobs0:
self.__find_blobs_input = self.hsv_threshold_output
(self.find_blobs_output) = self.__find_blobs(
self.__find_blobs_input, self.__find_blobs_min_area,
self.__find_blobs_circularity, self.__find_blobs_dark_blobs)
vision.key("Vision/Blobs")
vision.key("Vision/Blobs/area")
vision.setDouble(self.__find_blobs_min_area)
# Step Distance_Transform0:
self.__distance_transform_input = self.hsv_threshold_output
(self.distance_transform_output) = self.__distance_transform(
self.__distance_transform_input, self.__distance_transform_type,
self.__distance_transform_mask_size)
# Step Find_Contours0:
self.__find_contours_input = self.distance_transform_output
(self.find_contours_output) = self.__find_contours(
self.__find_contours_input, self.__find_contours_external_only)
@staticmethod
def __hsv_threshold(input, hue, sat, val):
"""Segment an image based on hue, saturation, and value ranges.
Args:
input: A BGR numpy.ndarray.
hue: A list of two numbers the are the min and max hue.
sat: A list of two numbers the are the min and max saturation.
lum: A list of two numbers the are the min and max value.
Returns:
A black and white numpy.ndarray.
"""
out = cv2.cvtColor(input, cv2.COLOR_BGR2HSV)
return cv2.inRange(out, (hue[0], sat[0], val[0]),
(hue[1], sat[1], val[1]))
@staticmethod
def __find_blobs(input, min_area, circularity, dark_blobs):
"""Detects groups of pixels in an image.
Args:
input: A numpy.ndarray.
min_area: The minimum blob size to be found.
circularity: The min and max circularity as a list of two numbers.
dark_blobs: A boolean. If true looks for black. Otherwise it looks for white.
Returns:
A list of KeyPoint.
"""
params = cv2.SimpleBlobDetector_Params()
params.filterByColor = 1
params.blobColor = (0 if dark_blobs else 255)
params.minThreshold = 10
params.maxThreshold = 220
params.filterByArea = True
params.minArea = min_area
params.filterByCircularity = True
params.minCircularity = circularity[0]
params.maxCircularity = circularity[1]
params.filterByConvexity = False
params.filterByInertia = False
detector = cv2.SimpleBlobDetector_create(params)
return detector.detect(input)
@staticmethod
def __distance_transform(input, type, mask_size):
"""Sets the values of pixels in a binary image to their distance to the nearest black pixel.
Args:
input: A numpy.array.
type: Opencv enum.
mask_size: The size of the mask. Either 0, 3, or 5.
Returns:
A black and white numpy.ndarray.
"""
h, w = input.shape[:2]
dst = numpy.zeros((h, w), numpy.float32)
cv2.distanceTransform(input, type, mask_size, dst=dst)
return numpy.uint8(dst)
@staticmethod
def __find_contours(input, external_only):
"""Sets the values of pixels in a binary image to their distance to the nearest black pixel.
Args:
input: A numpy.ndarray.
external_only: A boolean. If true only external contours are found.
Return:
A list of numpy.ndarray where each one represents a contour.
"""
if (external_only):
mode = cv2.RETR_EXTERNAL
else:
mode = cv2.RETR_LIST
method = cv2.CHAIN_APPROX_SIMPLE
im2, contours, hierarchy = cv2.findContours(input,
mode=mode,
method=method)
return contours
bh = 'Blue High'
bl = 'Blue Low'
gh = 'Green High'
gl = 'Green Low'
rh = 'Red High'
rl = 'Red Low'
wnd = 'Colorbars'
# Begin Creating trackbars for each BGR value
cv2.createTrackbar(bl, wnd, 0, 255, nothing)
cv2.createTrackbar(bh, wnd, 0, 255, nothing)
cv2.createTrackbar(gl, wnd, 0, 255, nothing)
cv2.createTrackbar(gh, wnd, 0, 255, nothing)
cv2.createTrackbar(rl, wnd, 0, 255, nothing)
cv2.createTrackbar(rh, wnd, 0, 255, nothing)
NetworkTables.initialize(server=ip)
datatable = NetworkTables.getTable("datatable")
subprocess.call(["sudo", "sh_files/ledbatch.sh"
]) # thats for test. put in comment after testing.
# capture frames from the camera
for image in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
# grab the raw NumPy array representing the image - this array
# will be 3D, representing the width, height, and # of channels
frame = image.array
# frame = frame[...,::-1]
# frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
if (display == 1):
cv2.imshow("Video Capture", frame)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#This initializes Networktables which will be used for communication between our robot and the GUI.
self.sd = NetworkTables.getTable('SmartDashboard')
#This initializes the camera.
wpilib.CameraServer.launch()
#This sets some counters for the state machines. (FOR Previous version without the encoders)
#self.getCubeCounter = 0
#self.dropCubeCounter = 0
#self.elevatorDownCounter = 0
#self.elevatorUpCounter = 0
#self.cubeTravelUp = 50
#self.cubeTravelStop = 1
#This sets some flags for the state machines.
self.prepareCubeFlag = 0 #This is the flag for the state machine which makes the robot prepared to grab the cube.
self.grabCubeFlag = 0 #This is the flag for the state machine which makes the robot grab the cube.
self.deliverCubeFlag = 0 #This is the flag for the state machine which makes the robot deliver the cube.
self.dstate = 0
self.pstate = 0
self.gstate = 0
#This sets the Drive Factor, which adjusts controller responsiveness.
self.driveFactor = 1
self.auto = self.sd.getNumber("auto", 0)
self.autoState = 0
#This initializes the Encoders - left and right, attached to gearbox
self.EC1 = wpilib.Encoder(0, 1)
self.EC2 = wpilib.Encoder(2, 3)
#Encoder for the elevator and shoulder
self.EC3 = wpilib.Encoder(4,
5) #This sets the encoder for the elevator.
self.EC4 = wpilib.Encoder(6, 7) #This sets the encoder for the arm.
#This sets the Pneumatics.
self.leftGearShift = wpilib.Solenoid(5, 0)
self.rightGearShift = wpilib.Solenoid(5, 1)
self.goldenArrowhead = wpilib.Solenoid(5,
2) # Reference to Guyanese flag
#This controls the function of the arm.
# Include limit switches for the elevator and shoulder mechanisms
# 2018-2-16 Warning! The Switch's channel should be modified according to the robot! - Fixed
#self.SW0 = wpilib.DigitalInput(0) #Lower Elevator Switch
#self.SW1 = wpilib.DigitalInput(1) #Upper Elevator Switch
#self.SW2 = wpilib.DigitalInput(2) #Lower shoulder switch
#self.SW3 = wpilib.DigitalInput(3) #Upper shoulder switch
# Left Motor Group Setup
self.M0 = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.M1 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.M0.setInverted(True) #This inverts the motors.
self.M1.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M0, self.M1)
# Right Motor Group Setup
self.M2 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.M3 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.right = wpilib.SpeedControllerGroup(self.M2, self.M3)
# Drive setup
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setMaxOutput(self.driveFactor)
# Misc Setting
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
# E = Elevator
self.E1 = wpilib.VictorSP(0) #This initializes the left elevator.
self.E2 = wpilib.VictorSP(1) #This initializes the right elevator.
# Shoulder
self.S1 = wpilib.VictorSP(2) #This initializes the left shoulder.
self.S2 = wpilib.VictorSP(3) #This initializes the right shoulder.
#Servo
self.SV1 = wpilib.Servo(4) #This initializes a servo.
#self.SV2 = wpilib.Servo(5)
#self.SV1.set(0.0)
#self.SV2.set(0.0)
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(
0) #This initializes a gyro to detect the direction of the robot.
self.gyro.reset() #This resets the gyro.
ap.add_argument("-c",
"--camera",
type=int,
default=camera_id,
help="Camera Port")
args = vars(ap.parse_args())
# Create the Mjpg server URL from data above
# server = "http://"
# server += args["ip"]
# server += "/stream.mjpg"
server = "http://localhost:2015/getframe.php"
# Enable logging to stdout & Initialize NetworkTables
logging.basicConfig(level=logging.DEBUG) # Set logging mode
NetworkTables.initialize(server=args["ip"]) # Init NT with RoboRIO IP address
NetworkTables.initialize() # Finish NT init
sd = NetworkTables.getTable("SmartDashboard") # Get SmartDashboard table
# Set max buffer size based on argument
pts = deque(maxlen=args["buffer"])
# If NOT using Mjpg stream, Init camera
if (args["camera"] != 99):
vs = VideoStream(src=args["camera"]).start() # Init camera
# Create Maps
pixel_map = interp1d([0, 600],
[min_range, max_range]) # Map pixel value to motor speed
deadzone_map = range((300 - deadzone + 1),
(300 + deadzone + 1)) # Handle deadzones
def deinit_networktables():
if HookInstance.persist.network.nt_enabled:
NetworkTables.shutdown()
#
# This is a NetworkTables server (eg, the robot or simulator side).
#
# On a real robot, you probably would create an instance of the
# wpilib.SmartDashboard object and use that instead -- but it's really
# just a passthru to the underlying NetworkTable object.
#
# When running, this will continue incrementing the value 'robotTime',
# and the value should be visible to networktables clients such as
# SmartDashboard. To view using the SmartDashboard, you can launch it
# like so:
#
# SmartDashboard.jar ip 127.0.0.1
#
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize(server='127.0.0.1')
sd = NetworkTables.getTable("SmartDashboard")
i = 0
while True:
sd.putNumber('test', i)
time.sleep(1)
i += 1
import sys
import time
from networktables import NetworkTables
from networktables.util import ntproperty
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
class SomeClient(object):
'''Demonstrates an object with magic networktables properties'''
robotTime = ntproperty('/SmartDashboard/robotTime', 0, writeDefault=False)
dsTime = ntproperty('/SmartDashboard/dsTime', 0)
c = SomeClient()
i = 0
while True:
try:
if self._exposure != self.net_table.getEntry("Exposure").value:
self.exposure = self.net_table.getEntry("Exposure").value
except:
pass
with self.capture_lock:
_, img = self.cap.read()
self._frame = img
self.new_frame = True
if __name__ == '__main__':
from networktables import NetworkTables
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize(server='localhost')
VisionTable = NetworkTables.getTable("BucketVision")
VisionTable.putString("BucketVisionState", "Starting")
FrontCameraTable = VisionTable.getSubTable('FrontCamera')
print("Starting Capture")
camera = Cv2Capture(network_table=FrontCameraTable)
camera.start()
print("Getting Frames")
while True:
if camera.new_frame:
cv2.imshow('my webcam', camera.frame)
if cv2.waitKey(1) == 27:
break # esc to quit
try:
ip = socket.gethostbyname('roboRIO-329-FRC.local')
print('Connected to robot')
break
except:
print('Waiting for NWT and Roborio connection')
time.sleep(.5)
pass
return (ip)
ip = net()
nt.initialize(server=ip)
sd = nt.getTable("SmartDashboard")
print('Connected to SmartDashboard')
def _node_name(n):
if n.startswith("^"):
return n[1:]
else:
return n.split(":")[0]
print('starting graph!')
input_graph = tf.Graph()
with tf.Session(graph=input_graph):
score = tf.placeholder(tf.float32,
red = (0, 0, 255)
# Default HSV values
# TODO Find real default values with the robot
min_h = 0
min_s = 42
min_v = 136
max_h = 190
max_s = 255
max_v = 255
# Logger initialize
logging.basicConfig(level=logging.DEBUG)
# Networktables initialize
NetworkTables.initialize(network_table_ip)
smart_dashboard_table = NetworkTables.getTable(smart_dashboard_table_name)
# cv2 initialization
cap = cv2.VideoCapture(camera_port)
# Reads the HSV values from the file and sets the HSV values accordingly
def read_values_from_file():
global lower
global upper
try:
file = open(saved_values_textfile_name, 'r')
min_h = float(file.readline())
min_s = float(file.readline())
min_v = float(file.readline())
def setup(self):
self.nt = NetworkTables.getTable("/components/indexer")
# values, which prints out all changes. Note that the keys are full paths, and
# not just individual key values.
#
import sys
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
def valueChanged(key, value, isNew):
print("valueChanged: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
NetworkTables.addEntryListener(valueChanged)
while True:
time.sleep(1)
from networktables import NetworkTables
from VR_data import TrackerData
import Constants
cond = threading.Condition()
notified = [False]
def connection_listener(connected, info):
print(info, '; Connected=%s' % connected)
with cond:
notified[0] = True
cond.notify()
NetworkTables.initialize(server=Constants.networktables_IP)
try:
NetworkTables.addconnection_listener(connection_listener,
immediateNotify=True)
except:
print("Unable to connect to network tables")
table = NetworkTables.getTable('SmartDashboard')
def get_target_location():
return TrackerData(table.getNumber("Target Location X", 0), 0,
table.getNumber("Target Location Z", 0), 0,
table.getNumber("Target Location Heading", 0), 0)
y = rho * np.sin(phi)
return(x, y)
import threading
from networktables import NetworkTables
cond = threading.Condition()
notified = [False]
def connectionListener(connected, info):
print(info, '; Connected=%s' % connected)
with cond:
notified[0] = True
cond.notify()
NetworkTables.initialize(server='10.25.30.2')
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
with cond:
print("Waiting")
if not notified[0]:
cond.wait()
# Insert your processing code here
print("Connected!")
NetworkTables.getTable("datatable").putNumber('test', 1);
# pixy2 Python SWIG get line features example #
print ("Pixy2 Python SWIG Example -- Get Line Features")
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from collections import deque
from networktables import NetworkTables
import numpy as np
import argparse
import cv2
import time
import imutils
x = 0 #programın ileride hata vermemesi için x 0 olarak tanımlıyorum
y = 0 # programın ileride hata vermemesi için y 0 olarak tanımlıyorum
NetworkTables.initialize(
server='roborio-6025-frc.local') # Roborio ile iletişim kuruyoruz
table = NetworkTables.getTable("Vision") # table oluşturuyoruz
#sari rengin algilanmasi
colorLower = (24, 100, 100)
colorUpper = (44, 255, 255)
#converter.py ile convert ettiğiniz rengi buraya giriniz
camera = cv2.VideoCapture(0) # kameradan
while True: #yazılımımız çalıştığı sürece aşağıdaki işlemleri tekrarla
(grabbed, frame) = camera.read(
) # grabbed ve frame değişkenini camera.read olarak tanımlıyoruz.
frame = imutils.resize(frame,
width=600) # görüntü genişliğini 600p yapıyoruz
frame = imutils.rotate(frame, angle=0) # görüntüyü sabitliyoruz
hsv = cv2.cvtColor(
frame, cv2.COLOR_BGR2HSV) # görüntüyü hsv formatına çeviriyoruz
import time
from networktables import NetworkTables
from networktables.util import ntproperty
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
'''if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]'''
NetworkTables.initialize(server='192.168.1.132')
class SomeClient(object):
"""Demonstrates an object with magic networktables properties"""
robotTime = ntproperty("/SmartDashboard/robotTime", 0, writeDefault=False)
dsTime = ntproperty("/ADifferentTable/dsTime", 0)
c = SomeClient()
i = 0
# just a passthru to the underlying NetworkTable object.
#
# When running, this will continue incrementing the value 'robotTime',
# and the value should be visible to networktables clients such as
# SmartDashboard. To view using the SmartDashboard, you can launch it
# like so:
#
# SmartDashboard.jar ip 127.0.0.1
#
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
sd = NetworkTables.getTable("SmartDashboard")
i = 0
while True:
try:
print('dsTime:', sd.getNumber('dsTime'))
except KeyError:
print('dsTime: N/A')
sd.putNumber('robotTime', i)
time.sleep(1)
i += 1
def __init__(self):
self.reset()
self.limelight_table = NetworkTables.getTable("limelight")
# On a real robot, you probably would create an instance of the
# wpilib.SmartDashboard object and use that instead -- but it's really
# just a passthru to the underlying NetworkTable object.
#
# When running, this will continue incrementing the value 'robotTime',
# and the value should be visible to networktables clients such as
# SmartDashboard. To view using the SmartDashboard, you can launch it
# like so:
#
# SmartDashboard.jar ip 127.0.0.1
#
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize()
sd = NetworkTables.getTable("SmartDashboard")
i = 0
while True:
print("dsTime:", sd.getNumber("dsTime", "N/A"))
sd.putNumber("robotTime", i)
time.sleep(1)
i += 1
import cv2
from grip import GripPipeline
from networktables import NetworkTables
import numpy as np
import os, time
NetworkTables.initialize(server='10.42.37.2')
table = NetworkTables.getTable('GRIP/vision')
idNum = 0
def extra_processing(pipeline):
global idNum
global newLogName
buff = []
xsum = 0
# Find the bounding boxes of the contours to get x, y, width, and height
for contour in pipeline.filter_contours_output:
x, y, w, h = cv2.boundingRect(contour)
buff.append(x + w / 2.0)
buff.append(h)
xsum += (x + w / 2.0) + (h)
idNum += 1
xsum += idNum
buff.append(idNum)
buff.append(xsum)
try:
#!/usr/bin/env python3
import sys
import time
from networktables import NetworkTables
import logging
if len(sys.argv) != 4:
print("Usage: {} <red> <grn> <blu>".format(sys.argv[0]))
print("where <red> <grn> <blu> are values between 0 and 255")
quit()
# To see messages from networktables, you must setup logging
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize(server="localhost")
nt = NetworkTables.getTable("Peariscope")
time.sleep(1)
nt.putNumber("led_red", int(round(float(sys.argv[1]))))
nt.putNumber("led_grn", int(round(float(sys.argv[2]))))
nt.putNumber("led_blu", int(round(float(sys.argv[3]))))
time.sleep(1)
def starting_position(self) -> str:
table = NetworkTables.getTable("autonomous/" + self.MODE_NAME +
"/starting_position")
return table.getString("selected", None)
# You need to tell it the IP address of the NetworkTables server (the
# robot or simulator).
#
# When running, this will create an automatically updated value, and print
# out the value.
#
import sys
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
sd = NetworkTables.getTable("SmartDashboard")
auto_value = sd.getAutoUpdateValue('robotTime', 0)
while True:
print('robotTime:', auto_value.value)
time.sleep(1)
if __name__ == '__main__':
#{NON-THREADING RELATED}
from networktables import NetworkTables
rioURL = '10.42.56.2'
NetworkTables.initialize(server = rioURL)
NetworkTables.setNetworkIdentity('TX2')
NetworkTables.setUpdateRate(.020)
table = NetworkTables.getTable('ZED')
robot_data = NetworkTables.getTable('Faraday')
from cv2 import VideoCapture
portL, portR = (False, False)
for port in range(8):
cam = VideoCapture(port)
found, frame = cam.read()
if found and frame.shape[1]//frame.shape[0] is 1:
if not portL: portL = port
else: portR = port
print('Found ports: ' + str((portL, portR)))
#{THREADING RELATED}
#{import overarching packages}
from queue import Queue
from CustomThread import CustomThread
#{import task-specific classes}
from Cameras import USB, ZED
from Stitching import ThreadableStitcher
from Servers import Web, NT
#{declare queues}
last_id = cam_id
success, im = cam.read()
finally:
cam.release()
print "Thread's done!"
if __name__ == "__main__":
print "Starting"
call(
"v4l2-ctl --device=/dev/video0 -c exposure_auto=1 -c exposure_absolute=5",
shell=True)
call(
"v4l2-ctl --device=/dev/video1 -c exposure_auto=1 -c exposure_absolute=5",
shell=True)
NetworkTables.initialize("10.22.12.2") # The ip of the roboRIO
t = Thread(target=update_image)
t.start()
pipeline = GripPipeline()
networkTableImageProcessing = NetworkTables.getTable("ImageProcessing")
contour_count = 2
try:
while im == None or not NetworkTables.isConnected():
pass
print "NT connection: %r" % NetworkTables.isConnected()
[
networkTableImageProcessing.delete(s)
for s in networkTableImageProcessing.getKeys()
]
while True:
print "Processing..."
# On startup, in command prompt run source .profile -> workon cv -> ./startUSBtest
from networktables import NetworkTables
import numpy as np
import cv2
import time
import math
def nothing(bep):
bep+1
NetworkTables.initialize(server='10.30.24.2')
table = NetworkTables.getTable('Pi')
cap = cv2.VideoCapture(0)
cap.set(3, 320)
cap.set(4, 180)
cap.set(5, 15)
cv2.namedWindow("Sliders")
cv2.createTrackbar("Minimum Hue", "Sliders", 40, 179, nothing)
cv2.createTrackbar("Minimum Saturation", "Sliders", 255, 255, nothing)
cv2.createTrackbar("Minimum Value", "Sliders", 48, 255, nothing)
cv2.createTrackbar("Maximum Hue", "Sliders", 90, 179, nothing)
cv2.createTrackbar("Maximum Saturation", "Sliders", 255, 255, nothing)
cv2.createTrackbar("Maximum Value", "Sliders", 145, 255, nothing)
# mask2 = np.zeros([320,160,3], dtype=np.int8, order='C')
from networktables import NetworkTables
# making connection to robot
cond = threading.Condition()
notified = [False]
def connectionListener(connected, info):
print(info, '; Connected=%s' % connected)
with cond:
notified[0] = True
cond.notify()
#format ex: team num is 1234 -> server = "10.12.34.2"
NetworkTables.initialize(server='10.76.73.2')
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
with cond:
print("Waiting")
if not notified[0]:
cond.wait()
# Insert your processing code here
print("Connected!")
table = NetworkTables.getTable('SmartDashboard')
#variables setup
text = 'Ball detected'
tb = 0
from rplidar import RPLidar
from networktables import NetworkTables
PORT_NAME = '/dev/ttyUSB0'
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.ERROR)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
val = 42
mmToIn = 25.4
robotSpeed = 0
NetworkTables.initialize("10.10.73.2")
sd = NetworkTables.getTable("LidarSendTable")
print("1073 Lidar Prototype ")
def run(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement = 2540
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
# import our classes
# Address for NetworkTable server
#networkTableServerAddress = '127.0.0.1' # Loopback
#networkTableServerAddress = '10.41.83.2' # Rio On competition field
#networkTableServerAddress = '10.38.14.2' # Rio On practice field
networkTableServerAddress = '192.168.0.103' # At 'home'
# And so it begins
print("Starting Network Example!")
# To see messages from networktables, you must setup logging
logging.basicConfig(level=logging.DEBUG)
try:
NetworkTables.initialize(server=networkTableServerAddress)
except ValueError as e:
print(e)
print("\n\n[WARNING]: NetworkTable Not Connected!\n\n")
bvTable = NetworkTables.getTable("ExampleTable")
bvTable.putString("ExampleTable","Starting")
bvTable.putString("ExampleTable","ONLINE")
runTime = 0
bvTable.putNumber("ExampleTime",runTime)
nextTime = time.time() + 1
try:
from rope import Rope
from blueboiler import BlueBoiler
from redboiler import RedBoiler
from boiler import Boiler
from gearlift import GearLift
from smokestack import SmokeStack
# And so it begins
print("Starting BUCKET VISION!")
# To see messages from networktables, you must setup logging
logging.basicConfig(level=logging.DEBUG)
try:
NetworkTables.setIPAddress(roboRioAddress)
NetworkTables.setClientMode()
NetworkTables.initialize()
except ValueError as e:
print(e)
print("\n\n[WARNING]: BucketVision NetworkTable Not Connected!\n\n")
bvTable = NetworkTables.getTable("BucketVision")
bvTable.putString("BucketVisionState", "Starting")
# Auto updating listener should be good for avoiding the need to poll for value explicitly
# A ChooserControl is also another option
# Make the cameraMode an auto updating listener from the network table
camMode = bvTable.getAutoUpdateValue('CurrentCam',
# values, which prints out all changes. Note that the keys are full paths, and
# not just individual key values.
#
import sys
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
def valueChanged(key, value, isNew):
print("valueChanged: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
NetworkTables.addGlobalListener(valueChanged)
while True:
time.sleep(1)
default_drive_speed = 0.2
min_drive_speed = 0.1
#
#
# Main routine starts here
#
# Let's start up the camera 1st
if (_dev):
# Use Portnum for Webcam testing
camera = cv2.VideoCapture(_portnum)
else:
# Use the camera stream from the ROMI/PI for Live Run
camera = cv2.VideoCapture("http://10.45.46.2:1181/stream.mjpg")
#
# Setup and Established NetworkTable connection
NetworkTables.initialize('127.0.0.1')
#NetworkTables.startClient('127.0.0.1')
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
# Wait until we got the NetworkTable Connection completed here
with cond:
print("Waiting for connection")
if not notified[0]:
cond.wait()
# Put the DriveByVision Table into x
x = NetworkTables.getTable('DriveByVision')
f = x.getNumber('Forward Speed', defaultValue=0.0)
lr = x.getNumber('Left-Right', defaultValue=0.0)
x.putBoolean("Stop", _stop)
x.putBoolean("QuickTurn", _quickturn)
