From 6d6799d656f50abeaf09cc8a68b87a7d2270513b Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sun, 27 May 2018 18:47:16 +0200 Subject: [PATCH] Ball search basics --- scripts/colorpicker.py | 116 ++++++++++++++++++++++++++++++ scripts/live_recognition.py | 27 ++++--- scripts/reacting_to_events.py | 129 ++++++++++++++++++++++++++++++++++ 3 files changed, 261 insertions(+), 11 deletions(-) create mode 100644 scripts/colorpicker.py create mode 100644 scripts/reacting_to_events.py diff --git a/scripts/colorpicker.py b/scripts/colorpicker.py new file mode 100644 index 0000000..58377d3 --- /dev/null +++ b/scripts/colorpicker.py @@ -0,0 +1,116 @@ +from __future__ import print_function +from live_recognition import get_frame_nao +import cv2 as cv +import imutils +from naoqi import ALProxy + +max_value = 255 +max_value_H = 360 // 2 +low_H = 0 +low_S = 0 +low_V = 0 +high_H = max_value_H +high_S = max_value +high_V = max_value +window_capture_name = 'Video Capture' +window_detection_name = 'Object Detection' +low_H_name = 'Low H' +low_S_name = 'Low S' +low_V_name = 'Low V' +high_H_name = 'High H' +high_S_name = 'High S' +high_V_name = 'High V' + +def do_print(): + print('(%s %s %s): (%s %s %s)' % + (low_H, low_S, low_V, high_H, high_S, high_V)) + +def on_low_H_thresh_trackbar(val): + global low_H + low_H = min(high_H-1, val) + cv.setTrackbarPos(low_H_name, window_detection_name, low_H) + do_print() + +def on_high_H_thresh_trackbar(val): + global high_H + high_H = max(val, low_H+1) + cv.setTrackbarPos(high_H_name, window_detection_name, high_H) + do_print() + +def on_low_S_thresh_trackbar(val): + global low_S + low_S = min(high_S-1, val) + cv.setTrackbarPos(low_S_name, window_detection_name, low_S) + do_print() + +def on_high_S_thresh_trackbar(val): + global high_S + high_S = max(val, low_S+1) + cv.setTrackbarPos(high_S_name, window_detection_name, high_S) + do_print() + +def on_low_V_thresh_trackbar(val): + global low_V + low_V = min(high_V-1, val) + cv.setTrackbarPos(low_V_name, window_detection_name, low_V) + do_print() + +def on_high_V_thresh_trackbar(val): + global high_V + high_V = max(val, low_V+1) + cv.setTrackbarPos(high_V_name, window_detection_name, high_V) + do_print() + + +cap = cv.VideoCapture(0) + +cv.namedWindow(window_capture_name) +cv.namedWindow(window_detection_name) + +cv.createTrackbar( + low_H_name, window_detection_name, low_H, + max_value_H, on_low_H_thresh_trackbar +) +cv.createTrackbar( + high_H_name, window_detection_name , high_H, max_value_H, + on_high_H_thresh_trackbar +) +cv.createTrackbar( + low_S_name, window_detection_name , low_S, max_value, + on_low_S_thresh_trackbar +) +cv.createTrackbar( + high_S_name, window_detection_name , high_S, max_value, + on_high_S_thresh_trackbar +) +cv.createTrackbar( + low_V_name, window_detection_name , low_V, max_value, + on_low_V_thresh_trackbar +) +cv.createTrackbar( + high_V_name, window_detection_name , high_V, max_value, + on_high_V_thresh_trackbar +) + +vd_proxy = ALProxy('ALVideoDevice', '192.168.0.11', 9559) +cam_subscriber = vd_proxy.subscribeCamera( + "ball_finder", 0, 1, 13, 20 +) + +try: + while True: + frame = get_frame_nao(vd_proxy, cam_subscriber, 320, 240) + + frame_HSV = cv.cvtColor(frame, cv.COLOR_BGR2HSV) + frame_threshold = cv.inRange( + frame_HSV, (low_H, low_S, low_V), (high_H, high_S, high_V) + ) + + cv.imshow(window_capture_name, frame) + cv.imshow(window_detection_name, frame_threshold) + + key = cv.waitKey(1) + if key == ord('q') or key == 27: + break +finally: + vd_proxy.unsubscribe(cam_subscriber) diff --git a/scripts/live_recognition.py b/scripts/live_recognition.py index d69c785..a4e77b7 100644 --- a/scripts/live_recognition.py +++ b/scripts/live_recognition.py @@ -11,18 +11,17 @@ from collections import deque # Nao configuration nao_ip = '192.168.0.11' nao_port = 9559 -res = (2, (480, 640)) # NAOQi code and acutal resolution -fps = 1 -cam_id = 0 # 0 := top, 1 := bottom +res = (1, (240, 320)) # NAOQi code and acutal resolution +fps = 30 +cam_id = 1 # 0 := top, 1 := bottom # Recognition stuff -red_lower = (0, 17, 225) # HSV coded red interval -red_upper = (42, 255, 255) -min_radius = 10 -resized_width = 600 # Maybe we need it maybe don't (None if don't) +red_lower = (0, 185, 170) # HSV coded red interval +red_upper = (2, 255, 255) +min_radius = 5 +resized_width = None # Maybe we need it maybe don't (None if don't) - -def get_frame_nao(cam_proxy, subscriber): +def get_frame_nao(cam_proxy, subscriber, width, height): result = cam_proxy.getImageRemote(subscriber) cam_proxy.releaseImage(subscriber) if result == None: @@ -31,7 +30,7 @@ def get_frame_nao(cam_proxy, subscriber): raise ValueError('no image data string') else: return np.frombuffer(result[6], dtype=np.uint8).reshape( - res[1][0], res[1][1], 3 + height, width, 3 ) # i = 0 # for y in range(res[1][0]): @@ -51,6 +50,8 @@ def find_colored_ball(frame, hsv_lower, hsv_upper, min_radius): mask = cv2.inRange(hsv, hsv_lower, hsv_upper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) + cv2.imshow('ball_mask', mask) + cv2.waitKey(1) # find contours in the mask and initialize the current # (x, y) center of the ball @@ -94,6 +95,9 @@ def draw_ball_markers(frame, center, radius, history): def nao_demo(): + cv2.namedWindow('ball_mask') + cv2.namedWindow('Frame') + vd_proxy = ALProxy('ALVideoDevice', nao_ip, nao_port) cam_subscriber = vd_proxy.subscribeCamera( "ball_finder", cam_id, res[0], 13, fps @@ -102,7 +106,8 @@ def nao_demo(): try: while True: - frame = get_frame_nao(vd_proxy, cam_subscriber) + frame = get_frame_nao(vd_proxy, cam_subscriber, res[1][1], + res[1][0]) # maybe resize the frame, maybe blur it if resized_width is not None: diff --git a/scripts/reacting_to_events.py b/scripts/reacting_to_events.py new file mode 100644 index 0000000..c59bd8b --- /dev/null +++ b/scripts/reacting_to_events.py @@ -0,0 +1,129 @@ +from naoqi import ALProxy +from naoqi import ALBroker +from naoqi import ALModule +import time + +NAO_IP = "192.168.0.11" + +# Global variable to store the BallSearcher module instance +BallSearcher = None +memory = None +ball_proxy = None +ball_found = False + + +class BallSearcherModule(ALModule): + """ A simple module able to react to facedetection events""" + + def __init__(self, name): + ALModule.__init__(self, name) + # No need for IP and port here because + # we have our Python broker connected to NAOqi broker + + # Create a proxy to ALTextToSpeech for later use + self.tts = ALProxy("ALTextToSpeech") + self.tts.setParameter('speed', 100) + self.mp = ALProxy('ALMotion') + self.mp.setStiffnesses("Head", 1.0) + + # Subscribe to the BallDetected event: + global memory + global ball_proxy + global ball_found + ball_proxy = ALProxy('ALRedBallDetection') + ball_proxy.subscribe('detector') + memory = ALProxy("ALMemory") + memory.subscribeToEvent("redBallDetected", + "BallSearcher", + "onBallDetected") + + def searchForBall(self): + names = ["HeadYaw", "HeadPitch"] + sleep_period = 0.8 + fractionMaxSpeed = 0.5 + i=0 + while i<2: + time.sleep(sleep_period) + if ball_found: + return + print i + angles = [i,0] + self.mp.setAngles(names, angles, fractionMaxSpeed) + i=float(i)+3.14/4 + + # go back to middle position + time.sleep(sleep_period) + if ball_found: + return + print 'go back' + angles = [0,0] + self.mp.setAngles(names, angles, fractionMaxSpeed) + + # go into the right direction + i=0 + while i > -2: + time.sleep(sleep_period) + if ball_found: + return + print i + angles = [i,0] + self.mp.setAngles(names, angles, fractionMaxSpeed) + i=i-3.14/4 + + # go back to middle position + time.sleep(sleep_period) + if ball_found: + return + print "get back" + angles = [0,0] + self.mp.setAngles(names, angles, fractionMaxSpeed) + + def onBallDetected(self, *_args): + """ This will be called each time a ball is detected.""" + + # Unsubscribe to the event when talking, to avoid repetitions + memory.unsubscribeToEvent("redBallDetected", "BallSearcher") + # time.sleep(0.1) + global ball_found + ball_found = True + print 'gotcha' + self.tts.say("Hello, ball") + + # Subscribe again to the event + # memory.subscribeToEvent("redBallDetected", + # "BallSearcher", + # "onBallDetected") + + +def main(): + """ Main entry point.""" + + # We need this broker to be able to construct + # NAOqi modules and subscribe to other modules + # The broker must stay alive until the program exists + myBroker = ALBroker( + "myBroker", + "0.0.0.0", + 0, + '192.168.0.11', + 9559 + ) + + + # Warning: BallSearcher must be a global variable + # The name given to the constructor must be the name of the + # variable + global BallSearcher + BallSearcher = BallSearcherModule("BallSearcher") + BallSearcher.searchForBall() + + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print "Interrupted by user, shutting down" + myBroker.shutdown() + + +if __name__ == "__main__": + main()