def circlesCoordinatesCallback(image): """Pronalazi krugove i vraca koordinate njihovog sredista.""" rosImg = ig.RospyImage(image) npImg = rosImg.createNumpyImage() circles = npImg.findCircles() print circles
def linesStreamCallback(image): """Pronadje sve linije na slici i oznaci ih.""" npimg = ig.RospyImage(image).createNumpyImage() lines = npimg.findLines() npimg.markLines(lines) cv2.imshow(WINDOW_NAME_LINES, npimg.getNumpyArray()) cv2.waitKey(1)
def rectangleCallback(image): """Pronalazi cetverokute na slici.""" npimg = ig.RospyImage(image).createNumpyImage() rectangles = npimg.findPolygon(4) npimg.markPolygons(rectangles) cv2.imshow(WINDOW_NAME_RECTANGLES, npimg.getNumpyArray()) cv2.waitKey(1)
def triangleCallback(image): """Pronalazi i oznacuje trokute na slici.""" rosImg = ig.RospyImage(image) npimg = rosImg.createNumpyImage() triangles = npimg.findPolygon(3) img = npimg.markPolygons(triangles) cv2.imshow(WINDOW_NAME_TRIANGLES, img) cv2.waitKey(1)
def yellowHSV(image): """Sluzilo za testiranje HSV boja.""" npimg = ig.RospyImage(image).createNumpyImage() rectangles = npimg.findColorPolygons(ig.YELL_MIN, ig.YELL_MAX, 4) if rectangles is not None and len(rectangles) > 0: npimg.markPolygons([rectangles[0]]) cv2.imshow(WINDOW_NAME_RECTANGLES, npimg.getNumpyArray()) cv2.waitKey(1)
def linesCallback(image): """Pronadje sve linije na slici. Ispise neke podatke vezane za njih.""" npimg = ig.RospyImage(image).createNumpyImage() lines = npimg.findLines() vectors = None if lines is not None: vectors = map(lambda l: l.vector().normalize(), lines) print vectors
def streamCallback(image): """ Funkcija se koristi kao callback za subscriber-a na /ardrone/front/image_raw. Prikazuje stream u prozoru. """ rosImg = ig.RospyImage(image) npImg = rosImg.createNumpyImage() cv2.imshow(WINDOW_NAME_REGULAR, npImg.getNumpyArray()) cv2.waitKey(1)
def refLineCallback(image): """Pronadje liniju (zid labirinta) po kojoj usmjerava letjelicu.""" npimg = ig.RospyImage(image).createNumpyImage() lines = npimg.findLines() if lines is not None: maxLen = max(map(lambda l: l.length(), lines)) line = filter(lambda l: abs(maxLen - l.length()) < 0.001, lines) npimg.markLines(line) cv2.imshow(WINDOW_NAME_LINES, npimg.getNumpyArray()) cv2.waitKey(1)
def colorCallback(image):
"""Ispise hsv od centralnog pixela."""
rosImg = ig.RospyImage(image)
npimg = rosImg.createNumpyImage()
width, height = ig.CAMERA_SIZE
#height /= 3
width += 250
print npimg.getPixelAt(width / 2, height / 2)
cv2.circle(npimg.getNumpyArray(), (width/2, height/2), 2, (0,0,0), 2)
cv2.imshow("boja", npimg.getNumpyArray())
cv2.waitKey(1)
def circlesCallback(image): """ Funkcija se koristi kao callback za subscriber-a na /ardrone/front/image_raw. Prikazuje stream u prozoru i oznacava krugove. """ rosImg = ig.RospyImage(image) npimg = rosImg.createNumpyImage() circles = npimg.findCircles() cimg = npimg.markCircles(circles) cv2.imshow(WINDOW_NAME_CIRCLES, cimg) cv2.waitKey(1)
def colorCirclesCallback(image): """ Pronadje i oznaci crveni krug na streamu. Ispisuje koordinate sredista oznacenog kruga. Napomena: pronalazi samo jedan krug. """ rosImg = ig.RospyImage(image) npimg = rosImg.createNumpyImage() circles = npimg.findCircles() circles = npimg.filterColoredCircles(circles, ig.RED) npimg.markCircles(circles) cv2.imshow(WINDOW_NAME_CIRCLES, npimg.getNumpyArray()) cv2.waitKey(1)
def colorRectangleCallback(image): """Pronalazi pravokutnik odredjene boje.""" npimg = ig.RospyImage(image).createNumpyImage() rectangles = npimg.findPolygon(4) colored = npimg.filterColoredPolygons(rectangles, ig.YELLOW) if colored != None and len(colored) > 1: rect = colored[0] else: cv2.imshow(WINDOW_NAME_RECTANGLES, npimg.getNumpyArray()) cv2.waitKey(1) return; npimg.markPolygons([rect]) center = rect.findCenter() print center cv2.imshow(WINDOW_NAME_RECTANGLES, npimg.getNumpyArray()) cv2.waitKey(1)
def colorTriangleCallback(image): """Pronalazi trokute odredjene boje.""" rosImg = ig.RospyImage(image) npimg = rosImg.createNumpyImage() triangles = npimg.findPolygon(3) coloredTriangles = npimg.filterColoredPolygons(triangles, ig.RED) if coloredTriangles != None and len(coloredTriangles) > 1: triangle = coloredTriangles[0] else: cv2.imshow(WINDOW_NAME_TRIANGLES, npimg.getNumpyArray()) cv2.waitKey(1) return npimg.markPolygons([triangle]) center = triangle.findCenter() direction = triangle.findDirection() print center, direction cv2.imshow(WINDOW_NAME_TRIANGLES, npimg.getNumpyArray()) cv2.waitKey(1)
def callback(image):
"""Pronalazi trokute odredjene boje."""
global data
rosImg = ig.RospyImage(image)
npimg = rosImg.createNumpyImage()
npimgCopy = rosImg.createNumpyImage()
markers = {}
angle = None
triangles = npimg.findPolygon(3)
coloredTriangles = npimg.filterColoredPolygons(triangles, ig.RED1)
coloredTriangles += npimg.filterColoredPolygons(triangles, ig.RED2)
if coloredTriangles is not None and len(coloredTriangles) > 0:
triangle = coloredTriangles[0]
#npimgCopy.markPolygons([triangle])
center = triangle.findCenter()
data.position.position.x = center[0]
data.position.position.y = center[1]
data.pubPoseTrokut.publish(data.position)
lines = npimg.findLines()
if lines is not None:
minAng = min(map(lambda l: abs(l.angle2()), lines))
line = filter(lambda l: abs(minAng - abs(l.angle2())) < 0.1, lines)
lineLen = map(lambda l: (l.length(), l), line)
lineLen = sorted(lineLen, reverse=True)
line = [lineLen[0][1]]
npimgCopy.markLines(line)
direction = line[0].angle2()
angle = direction
data.north.orientation.z = direction
data.pubPoseNorth.publish(data.north)
rectangles = npimg.findPolygon(4)
if rectangles is not None and len(rectangles) > 0:
yellow = npimg.filterColoredPolygons(rectangles, ig.YELLOW)
orange = npimg.filterColoredPolygons(rectangles, ig.ORANGE)
blue = npimg.filterColoredPolygons(rectangles, ig.BLUE)
purple = npimg.filterColoredPolygons(rectangles, ig.PURPLE)
if yellow is not None and len(yellow) > 0:
rect = yellow[0]
npimgCopy.markPolygons([rect])
center = rect.findCenter()
data.yellowMarker.position.x = center[0]
data.yellowMarker.position.y = center[1]
data.pubPoseYM.publish(data.yellowMarker)
markers['yellow'] = center
if orange is not None and len(orange) > 0:
rect = orange[0]
npimgCopy.markPolygons([rect])
center = rect.findCenter()
data.orangeMarker.position.x = center[0]
data.orangeMarker.position.y = center[1]
data.pubPoseOM.publish(data.orangeMarker)
markers['orange'] = center
if blue is not None and len(blue) > 0:
rect = blue[0]
npimgCopy.markPolygons([rect])
center = rect.findCenter()
data.blueMarker.position.x = center[0]
data.blueMarker.position.y = center[1]
data.pubPoseBM.publish(data.blueMarker)
markers['blue'] = center
if purple is not None and len(purple) > 0:
rect = purple[0]
npimgCopy.markPolygons([rect])
center = rect.findCenter()
data.purpleMarker.position.x = center[0]
data.purpleMarker.position.y = center[1]
data.pubPosePM.publish(data.purpleMarker)
markers['purple'] = center
circles = npimg.findCircles(20)
greenCircles = npimg.filterColoredCircles(circles, ig.GREEN)
if greenCircles is not None and len(greenCircles) > 0:
circ = greenCircles[0]
npimgCopy.markCircles([circ])
center = circ.getPosition()
data.exitMarker.position.x = center[0]
data.exitMarker.position.y = center[1]
data.pubPoseExit.publish(data.exitMarker)
markers['exit'] = center
"""
blueCircles = npimg.filterColoredCircles(circles, ig.BLUE)
if blueCircles is not None and len(blueCircles) > 0:
circ = blueCircles[0]
npimgCopy.markCircles([circ])
center = circ.getPosition()
data.purpleMarker.position.x = center[0]
data.purpleMarker.position.y = center[0]
data.pubPosePM.publish(data.purpleMarker)
markers['purple'] = center
"""
loc = ms.locateQuad(markers, angle)
if loc is not None:
data.globalCoords.position.x = loc[0]
data.globalCoords.position.y = loc[1]
data.pubPoseGlobal.publish(data.globalCoords)
print "%6.2f %6.2f" % (loc[0], loc[1])
X = 320 / 2
Y = 240 / 2
cv2.circle(npimgCopy.getNumpyArray(), (X, Y), 2, (0, 0, 0), 2)
cv2.imshow('stream', npimgCopy.getNumpyArray())
cv2.waitKey(1)
