vtable = NetworkTable.getTable("vision")
NetworkTable.addConnectionListener(connectionListener, immediateNotify=True)
if SHOW_IMAGE :
cv2.startWindowThread()
cv2.namedWindow("image")
cv2.imshow("image", camera.get_image())
# main
while True:
st = time.time()
img = camera.capture()
if USE_VISION:
img,a1,a2,c1,c2,ct = finder.findPoints(img, SHOW_IMAGE)
fovh = 30.686 # degrees
x = 8.0 * 0.0254 # distance between tapes (meters)
w = 1.0 # meters
h = w * CAMERA_HEIGHT / CAMERA_WIDTH # meters
dc = 0.5 * w / math.tan(math.radians(fovh)) # meters
# xp = w * abs((c1[0] - CAMERA_WIDTH/2.0) - (c2[0] - CAMERA_WIDTH/2.0)) / CAMERA_WIDTH # meters
cx = w * (ct[0] - CAMERA_WIDTH/2.0) / CAMERA_WIDTH # meters
cy = h * (ct[1] - CAMERA_HEIGHT/2.0) / CAMERA_HEIGHT # meters
#d = (dc * HEIGHT_DIFF / cx) * math.sqrt((dc*dc + cy*cy + cx*cx) / (dc*dc + 2.0*cy*cy))
d = dc * HEIGHT_DIFF / cy
import rospy
import time
lower_threshold = (17, 166, 36)
upper_threshold = (23, 255, 51)
if __name__ == "__main__":
try:
rospy.init_node("color_detection")
rate = rospy.Rate(15) # 15 FPS
cap = Camera(port=5600)
cd = ColorDetection(lower_threshold, upper_threshold)
print("Wait for camera capture..")
frame = cap.capture()
while frame is None and not rospy.is_shutdown():
rate.sleep()
frame = cap.capture()
print("Frame captured!")
fps = 15.0
t = time.time()
while not rospy.is_shutdown():
frame = cap.capture()
mask = cd.update(frame)
if cd.has_centroid:
cv.circle(mask, cd.centroid, 5, 127, -1)
cv.putText(mask, "fps: %.1f" % fps, (240, 230), cv.FONT_HERSHEY_SIMPLEX, 0.5, 127, 2)
import cv2
import time
from vision.camera import Camera
import vision.finder as finder
camera = Camera(0, 100)
#cv2.imshow("video", camera.get_image());
while 1:
start_time = time.time()
im = camera.capture()
# edg = cv2.Canny(im, 50, 250)
p, a = finder.findPoints(im)
#print(a)
#cv2.imshow("video", p)
if cv2.waitKey(1) > 0:
break
#print("frame time: %.3f" % (time.time() - start_time))
