diff --git a/scripts/live_recognition_with_head.py b/scripts/live_recognition_with_head.py new file mode 100644 index 0000000..cbe81a6 --- /dev/null +++ b/scripts/live_recognition_with_head.py @@ -0,0 +1,233 @@ +from __future__ import print_function +from __future__ import division + +import cv2 +import numpy as np +#import imutils +from naoqi import ALProxy +from collections import deque + + +# Nao configuration +nao_ip = '192.168.0.10' +nao_port = 9559 +#res = (3, (960, 1280)) # NAOQi code and acutal resolution +res=(1,(240,320)) +#res=(2,(480,640)) + +fps = 30 +cam_id = 0 # 0 := top, 1 := bottom + +# Recognition stuff +red_lower = (0, 185, 170) # HSV coded red interval +red_upper = (6, 255, 255) +min_radius = 5 +resized_width = None # Maybe we need it maybe don't (None if don't) + +global current_value +current_value=0 + +def get_angle(): + robotIP="192.168.0.10" + PORT = 9559 + motionProxy = ALProxy("ALMotion", robotIP, PORT) + names=["HeadPitch","HeadYaw"] + useSensors=False + angle=motionProxy.getAngles(names,useSensors) + #print("angle_is"+str(angles)) + return angle + +def set_angle(direction): +#def main(robotIP,x,y): + robotIP="192.168.0.10" + PORT = 9559 + motionProxy = ALProxy("ALMotion", robotIP, PORT) + # activiert gelenke + motionProxy.setStiffnesses("Head", 1.0) + + + #names = "HeadYaw" + #useSensors = False + + #commandAngles = motionProxy.getAngles(names, useSensors) + + #type(commandAngles) + #type(float(commandAngles)) + #current_angle=float(commandAngles) + #print(current_angle) + #next_angle=float(commandAngles)-0.2 + #print("next_angle"+str(next_angle)) + #angles = [0,next_angle] + + + + #print("set_angle") + # Example showing how to set angles, using a fraction of max speed + names = ["HeadYaw", "HeadPitch"] + #global current_value + a=get_angle() + #print(a[0]) +# print(a) + + + #current_value=current_value-0.2 + if direction=="up": + angles = [a[1],a[0]-0.2] + elif direction=="down": + angles = [a[1], a[0]+0.2] + elif direction=="right": + angles= [a[1]-0.2,a[0]] + elif direction=="left": + angles=[a[1]+0.2,a[0]] + fractionMaxSpeed = 0.5 + + motionProxy.setAngles(names, angles, fractionMaxSpeed) + + + +def get_frame_nao(cam_proxy, subscriber, width, height): + result = cam_proxy.getImageRemote(subscriber) + cam_proxy.releaseImage(subscriber) + if result == None: + raise RuntimeError('cannot capture') + elif result[6] == None: + raise ValueError('no image data string') + else: + return np.frombuffer(result[6], dtype=np.uint8).reshape( + height, width, 3 + ) + # i = 0 + # for y in range(res[1][0]): + # for x in range(res[1][1]): # columnwise + # image.itemset((y, x, 0), values[i + 0]) + # image.itemset((y, x, 1), values[i + 1]) + # image.itemset((y, x, 2), values[i + 2]) + # i += 3 + # return image + + +def find_colored_ball(frame, hsv_lower, hsv_upper, min_radius,motionProxy): + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + + # construct a mask for the color "green", then perform a series of + # dilations and erosions to remove any small blobs left in the mask + 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 + cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, + cv2.CHAIN_APPROX_SIMPLE)[-2] + + # only proceed if at least one contour was found + if len(cnts) == 0: + return None + + # find the largest contour in the mask, then use it to compute + # the minimum enclosing circle and centroid + c = max(cnts, key=cv2.contourArea) + ((x, y), radius) = cv2.minEnclosingCircle(c) + + if radius < min_radius: + return None + + M = cv2.moments(c) + center = (int(M["m10"] / M["m00"]),int(M["m01"] // M["m00"])) + #print("center is: "+str(center[0])) + x=center[0] + y=center[1] + print(str(y)) + #print(type(get_angle())) + a=get_angle() + print(a[0]) + print(a[1]) + + #print(get_angle()) +# print("angle_is"+str(get_angle())) + if 0y>200: + set_angle("down") + elif 0x>220: + set_angle("right") + + return center, int(radius) + + +def draw_ball_markers(frame, center, radius, history): + # draw the enclosing circle and ball's centroid on the frame, + if center is not None and radius is not None: + cv2.circle(frame, center, radius, (255, 255, 0), 1) + cv2.circle(frame, center, 5, (0, 255, 0), -1) + + # loop over the set of tracked points + for i in range(1, len(history)): + # if either of the tracked points are None, ignore them + if history[i - 1] is None or history[i] is None: + continue + # otherwise, compute the thickness of the line and + # draw the connecting lines + thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) + cv2.line(frame, history[i - 1], history[i], (0, 255, 0), thickness) + + # check if head has to be moved according to the ball + #y_history=history[len(history) - 1] + #y_history=int(y_history) + #print(str(type(y_history))) + #wert=y_history[0] + #print("y="+str(wert)) + + return frame + + +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 + ) + history = deque(maxlen=64) + motionProxy = ALProxy("ALMotion", nao_ip, nao_port) + + try: + while True: + 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: + frame = imutils.resize(frame, width=resized_width) + # blurred = cv2.GaussianBlur(frame, (11, 11), 0) + + try: + center, radius = find_colored_ball( + frame, red_lower, red_upper, min_radius,motionProxy + ) + history.appendleft(center) + draw_ball_markers(frame, center, radius, history) + except TypeError: # No red ball found and function returned None + history.appendleft(None) + draw_ball_markers(frame, None, None, history) + + # show the frame to screen + cv2.imshow("Frame", frame) + cv2.waitKey(1) + + finally: + vd_proxy.unsubscribe(cam_subscriber) + cv2.destroyAllWindows() + + +if __name__ == '__main__': + nao_demo()