diff --git a/.gitignore b/.gitignore index 579f56f..c87d4d8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.sw* *.pyc +*.jpg diff --git a/scripts/ball_hsv.json b/scripts/ball_hsv.json new file mode 100644 index 0000000..77165ff --- /dev/null +++ b/scripts/ball_hsv.json @@ -0,0 +1,8 @@ +{ + "low_s": 175, + "low_v": 100, + "high_h": 6, + "high_v": 255, + "low_h": 0, + "high_s": 255 +} \ No newline at end of file diff --git a/scripts/colorpicker.py b/scripts/colorpicker.py index 39eb37b..857788c 100644 --- a/scripts/colorpicker.py +++ b/scripts/colorpicker.py @@ -57,6 +57,7 @@ class Colorpicker(object): tuple(map(self.settings.get, ('low_h', 'low_s', 'low_v'))), tuple(map(self.settings.get, ('high_h', 'high_s', 'high_v'))) ) + frame_threshold = cv2.resize(frame_threshold, (640, 480)) cv2.imshow(self.WINDOW_CAPTURE_NAME, frame) cv2.imshow(self.WINDOW_DETECTION_NAME, frame_threshold) return cv2.waitKey(1) @@ -107,9 +108,19 @@ if __name__ == '__main__': choices=['upper', 'lower'], help='choose a camera from nao' ) + parser.add_argument( + '--nao-res', + choices=[1, 2, 3], + type=int, + default=1 + ) args = parser.parse_args() cp = Colorpicker() + camera_ids = { + 'upper': 0, + 'lower': 1 + } if args.input_config: cp.load(args.input_config) if args.video_file: @@ -119,7 +130,8 @@ if __name__ == '__main__': elif args.nao_ip: rdr = NaoImageReader( args.nao_ip, - cam_id=args.nao_cam if args.nao_cam else 0 + cam_id=camera_ids[args.nao_cam] if args.nao_cam else 0, + res=args.nao_res ) else: rdr = VideoReader(0) diff --git a/scripts/imagereaders.py b/scripts/imagereaders.py index 0fce45f..8ca0941 100644 --- a/scripts/imagereaders.py +++ b/scripts/imagereaders.py @@ -1,3 +1,5 @@ +from __future__ import division + import numpy as np import cv2 try: @@ -21,6 +23,14 @@ class NaoImageReader(object): "video_streamer", cam_id, res, 13, fps ) + def to_angles(self, x, y): + return self.vd.getAngularPositionFromImagePosition( + self.cam_id, x / self.res[1], y / self.res[0] + ) + + def to_relative(self, x, y): + return x / self.res[1], y / self.res[0] + def get_frame(self): result = self.vd.getImageRemote(self.sub) self.vd.releaseImage(self.sub) diff --git a/scripts/live_recognition.py b/scripts/live_recognition.py index b1a1297..c1f995e 100644 --- a/scripts/live_recognition.py +++ b/scripts/live_recognition.py @@ -4,7 +4,7 @@ from __future__ import division import json import cv2 import numpy as np -import imutils +# import imutils from imagereaders import NaoImageReader, VideoReader from collections import deque @@ -15,7 +15,8 @@ red_upper = (2, 255, 255) class BallFinder(object): - def __init__(self, hsv_lower, hsv_upper, min_radius, width): + def __init__(self, hsv_lower, hsv_upper, min_radius, width, + viz=False): self.hsv_lower = hsv_lower self.hsv_upper = hsv_upper @@ -24,9 +25,11 @@ class BallFinder(object): self.history = deque(maxlen=64) self.last_center = None self.last_radius = None + self.viz = viz - cv2.namedWindow('ball_mask') - cv2.namedWindow('Frame') + if self.viz: + cv2.namedWindow('ball_mask') + cv2.namedWindow('Frame') def find_colored_ball(self, frame): hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) @@ -36,7 +39,8 @@ class BallFinder(object): mask = cv2.inRange(hsv, self.hsv_lower, self.hsv_upper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) - cv2.imshow('ball_mask', mask) + if self.viz: + cv2.imshow('ball_mask', mask) # find contours in the mask and initialize the current # (x, y) center of the ball @@ -45,6 +49,7 @@ class BallFinder(object): # only proceed if at least one contour was found if len(cnts) == 0: + print('Nothin there') return None # find the largest contour in the mask, then use it to compute @@ -61,8 +66,8 @@ class BallFinder(object): def next_frame(self, frame): # maybe resize the frame, maybe blur it - if self.width is not None: - frame = imutils.resize(frame, width=self.width) + # if self.width is not None: + # frame = imutils.resize(frame, width=self.width) try: self.last_center, self.last_radius = self.find_colored_ball(frame) except TypeError: # No red ball found and function returned None @@ -72,8 +77,9 @@ class BallFinder(object): self.draw_ball_markers(frame) # show the frame to screen - cv2.imshow("Frame", frame) - return cv2.waitKey(2) + if self.viz: + cv2.imshow("Frame", frame) + return cv2.waitKey(2) def draw_ball_markers(self, frame): # draw the enclosing circle and ball's centroid on the frame, diff --git a/scripts/live_recognition_with_head.py b/scripts/live_recognition_with_head.py index e48f504..8af6b9c 100644 --- a/scripts/live_recognition_with_head.py +++ b/scripts/live_recognition_with_head.py @@ -1,13 +1,11 @@ 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 from imagereaders import NaoImageReader from live_recognition import BallFinder +from live_recognition_with_head_with_body import move_to # Nao configuration @@ -28,8 +26,7 @@ min_radius = 5 resized_width = None # Maybe we need it maybe don't (None if don't) current_value = 0 -global counter -counter=0 +counter = 0 def get_angle(): robotIP="192.168.0.11" @@ -46,50 +43,57 @@ def set_angle_new(x,y): robotIP="192.168.0.11" PORT = 9559 motionProxy = ALProxy("ALMotion", robotIP, PORT) - + # activiert gelenke motionProxy.setStiffnesses("Head", 0.5) names = ["HeadYaw", "HeadPitch"] - fractionMaxSpeed = 0.025 - x_mid=320/2 - y_mid=240/2 - #y_mid=120 - x_diff=x-x_mid - y_diff=y-y_mid + fractionMaxSpeed = 0.3 + x_diff=x-0.5 + y_diff=y-0.5 print("x_diff="+str(x_diff)) print("y_diff="+str(y_diff)) - - #videoProxy=ALProxy('ALVideoDevice', robotIP, PORT) - #ball_angles=videoProxy.getAngularPositionFromImagePosition(cam_id,[x/320,y/240]) - #print(ball_angles) - #ball_angle_x=ball_angles[0] - #ball_angle_y=ball_angles[1] - #print("ball_angle_x="+str(ball_angle_x)) - #print("ball_angle_y="+str(ball_angle_y)) - #ball_angle_x_diff=ball_angle_x+ - #ball_angle_y_diff=ball_angle_y-99 - #print(ball_angle_x_diff*3.14/180) - #print(ball_angle_y_diff) - -# print(ball_angles-[-169,99]) - #[-169.53343200683594, 99.27782440185547] (x_mid,y_mid) - #if abs(ball_angle_x)>0.2 and abs(ball_angle_y)>0.01: - #angles=[ball_angle_x,0] - #angles=[0.25*ball_angle_x,0.25*ball_angle_y] - #angles=[0.25*ball_angle_x,0.25*ball_angle_y] - #angles=[2*(-1 if x_diff > 0 else 1),2*(-1 if y_diff > 0 else 1)] - angles=[2*(-1 if x_diff > 0 else 1),0] + # videoProxy=ALProxy('ALVideoDevice', robotIP, PORT) + # ball_angles=videoProxy.getAngularPositionFromImagePosition(cam_id,[x/320,y/240]) + # print(ball_angles) + # ball_angle_x=ball_angles[0] + # ball_angle_y=ball_angles[1] + # print("ball_angle_x="+str(ball_angle_x)) + # print("ball_angle_y="+str(ball_angle_y)) + + # ball_angle_x_diff=ball_angle_x+ + # ball_angle_y_diff=ball_angle_y-99 + # print(ball_angle_x_diff*3.14/180) + # print(ball_angle_y_diff) + + # print(ball_angles-[-169,99]) + # [-169.53343200683594, 99.27782440185547] (x_mid,y_mid) + # if abs(ball_angle_x)>0.2 and abs(ball_angle_y)>0.01: + # angles=[ball_angle_x,0] + # angles=[0.25*ball_angle_x,0.25*ball_angle_y] + # angles=[0.25*ball_angle_x,0.25*ball_angle_y] + # angles=[2*(-1 if x_diff > 0 else 1),2*(-1 if y_diff > 0 else 1)] + angles=[-x_diff / 2, 0] #if abs(ball_angle_x)>0.1 or abs(ball_angle_y)>0.1: #if abs(x_diff)>50 or abs(y_diff)>50: - if abs(x_diff)>50: - motionProxy.changeAngles(names, angles, fractionMaxSpeed) + global counter + if abs(x_diff) > 0.1: + motionProxy.changeAngles(names, angles, fractionMaxSpeed) + counter = 0 + else: + counter += 1 + print(counter) + if counter == 10: + print('Going to rotate') + angle = get_angle() + if abs(angle[1]) > 0.1: + move_to(0, angle[1]) #motionProxy.setAngles(names,angles,fractionMaxSpeed) #else: # a=get_angle() # motionProxy.setAngles(names,a,fractionMaxSpeed) - + ''' elif abs(ball_angle_x)<0.1 and abs(ball_angle_y)<0.1: #tts = ALProxy("ALTextToSpeech", "192.168.0.11", 9559) @@ -160,12 +164,16 @@ if __name__ == '__main__': video = NaoImageReader(nao_ip, port=nao_port, cam_id=cam_id, res=res, fps=fps) finder = BallFinder(red_lower, red_upper, 5, None) + finder.load_hsv_config('ball_hsv.json') try: while True: try: (x, y), radius = finder.find_colored_ball(video.get_frame()) except TypeError: continue + print(x, y) + x, y = video.to_relative(x, y) + print(x, y) set_angle_new(x,y) ''' if 0