Ball search basics

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)

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
+res = (1, (240, 320))  # NAOQi code and acutal resolution
-fps = 1
+fps = 30
-cam_id = 0  # 0 := top, 1 := bottom
+cam_id = 1  # 0 := top, 1 := bottom
 
 # Recognition stuff
-red_lower = (0, 17, 225)  # HSV coded red interval
+red_lower = (0, 185, 170)  # HSV coded red interval
-red_upper = (42, 255, 255)
+red_upper = (2, 255, 255)
-min_radius = 10
+min_radius = 5
-resized_width = 600  # Maybe we need it maybe don't (None if don't)
+resized_width = None  # Maybe we need it maybe don't (None if don't)
 
-
+def get_frame_nao(cam_proxy, subscriber, width, height):
-def get_frame_nao(cam_proxy, subscriber):
     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:

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()