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)