Last active
June 12, 2020 15:09
-
-
Save ixLikro/c396328b0deb139af3176cbc5244f91e to your computer and use it in GitHub Desktop.
ros node, a opencv color filter. The biggest contour of one color will be published to a ros topic as bounding rect.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| import cv2 | |
| import numpy as np | |
| import rospy | |
| from std_msgs.msg import String | |
| # open a debug window and draw the bounding rects | |
| GUI_MODE = False | |
| # print the HSV-Value of the pixel 100, 100, this is handy if you want to fine tune the colors | |
| COLOR_DEBUG_MODE = False | |
| #Publisher Init | |
| pos_ball_pub = rospy.Publisher('pos_ball', String, queue_size=10) | |
| pos_goalkeeper_pub = rospy.Publisher('pos_goalkeeper', String, queue_size=10) | |
| rospy.init_node('position__publisher', anonymous=True) | |
| #rospy.init_node('pos_goalkeeper_publisher', anonymous=True) | |
| # All colors are in HSV | |
| colors = [ | |
| {'name': "Orange", 'lower': np.array([23, 60, 60]), 'upper': np.array([50, 255, 255]), 'pub': pos_ball_pub}, | |
| {'name': "Blue", 'lower': np.array([100, 60, 60]), 'upper': np.array([120, 255, 255]), 'pub': pos_goalkeeper_pub}, | |
| ] | |
| font = cv2.FONT_HERSHEY_SIMPLEX | |
| cap = cv2.VideoCapture(0) | |
| # searches for the given color and calculated the contour | |
| # uses the biggest contour of the given color to calculate the bounding rect | |
| # returns a tuple with the values of the bounding rect (x,y,w,h) | |
| # or 4 times -1 if no bounding react was found | |
| def findBiggestBoundingRect(hsvFrame, color): | |
| # preparing the masks to overlay | |
| mask = cv2.inRange(hsvFrame, color['lower'], color['upper']) | |
| # calculates contours (basically outlines, that are even approximated as a chain) | |
| ignore, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) | |
| # find biggest contour | |
| if contours: | |
| contour_sizes = [(cv2.contourArea(contour), contour) for contour in contours] | |
| biggest_contour = max(contour_sizes, key=lambda x: x[0])[1] | |
| # calculate bounding react and draw it | |
| return cv2.boundingRect(biggest_contour) | |
| else: | |
| return -1, -1, -1, -1 | |
| while not rospy.is_shutdown(): | |
| _, frame = cap.read() | |
| # It converts the BGR color space of image to HSV color space | |
| hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) | |
| for color in colors: | |
| x, y, w, h = findBiggestBoundingRect(hsv, color) | |
| if w > 10 and h > 10: | |
| # valid | |
| str = "{},{},{},{}".format(x,y,w,h) | |
| rospy.loginfo(str) | |
| color["pub"].publish(str) | |
| #print("color: : "+ color["name"]+", x: "+str(x)+", y: "+str(y)+", width: "+str(w)+", height: "+str(h)) | |
| if GUI_MODE: | |
| cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2) | |
| cv2.putText(frame, color['name'], (x, y - 5), font, 0.7, (0, 255, 0), 2, cv2.LINE_AA) | |
| else: | |
| x, y, w, h = -1, -1, -1, -1 | |
| if COLOR_DEBUG_MODE: | |
| print(hsv[100][100]) | |
| if GUI_MODE: cv2.circle(frame, (100,100), 5, (255, 0, 0)) | |
| if GUI_MODE: | |
| cv2.imshow('frame', frame) | |
| # exit if esc was pressed | |
| k = cv2.waitKey(250) | |
| if k == 27: | |
| break | |
| cv2.destroyAllWindows() | |
| cap.release() | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment