improvements in color calibration and tracking

.gitignore

@@ -1,2 +1,3 @@
 *.sw*
 *.pyc
+*.jpg

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
+}

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)

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)

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,7 +25,9 @@ class BallFinder(object):
         self.history = deque(maxlen=64)
         self.last_center = None
         self.last_radius = None
+        self.viz = viz
 
+        if self.viz:
             cv2.namedWindow('ball_mask')
             cv2.namedWindow('Frame')
 
@@ -36,6 +39,7 @@ 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)
+        if self.viz:
             cv2.imshow('ball_mask', mask)
 
         # find contours in the mask and initialize the current
@@ -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,6 +77,7 @@ class BallFinder(object):
         self.draw_ball_markers(frame)
 
         # show the frame to screen
+        if self.viz:
             cv2.imshow("Frame", frame)
             return cv2.waitKey(2)
 

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"
@@ -50,41 +47,48 @@ def set_angle_new(x,y):
     # 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))
+    # 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)
+    # 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]
+    # 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:
+    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()
@@ -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<y<100:

scripts/live_recognition_with_head_with_body.py

@@ -89,40 +89,40 @@ def move_to(old,y):
 
     # First call of move API
     # with post prefix to not be bloquing here.
-    motionProxy.post.moveTo(0, 0.0, 0)
+    # motionProxy.post.moveTo(0, 0.0, 0)
 
     # wait that the move process start running
-    time.sleep(0.1)
+    # time.sleep(0.1)
 
     # get robotPosition and nextRobotPosition
-    useSensors = False
-    robotPosition     = almath.Pose2D(motionProxy.getRobotPosition(useSensors))
-    nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
+    # useSensors = False
+    # robotPosition     = almath.Pose2D(motionProxy.getRobotPosition(useSensors))
+    # nextRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())
 
     # get the first foot steps vector
     # (footPosition, unChangeable and changeable steps)
 
-    footSteps1 = []
+    # footSteps1 = []
     #try:
-    footSteps1 = motionProxy.getFootSteps()
+    # footSteps1 = motionProxy.getFootSteps()
     #except Exception, errorMsg:
     #    print str(errorMsg)
     #    PLOT_ALLOW = False
 
     # Second call of move API
-    motionProxy.post.moveTo(0, 0.0, 1.2*y)
+    motionProxy.post.moveTo(0, 0.0, y)
 
     # get the second foot steps vector
-    footSteps2 = []
+    # footSteps2 = []
     #try:
-    footSteps2 = motionProxy.getFootSteps()
+    # footSteps2 = motionProxy.getFootSteps()
     #except Exception, errorMsg:
         #print str(errorMsg)
         #PLOT_ALLOW = False
-    motionProxy.setStiffnesses("Head", 0.5)
+    motionProxy.setStiffnesses("Head", 0.7)
     names  = ["HeadYaw", "HeadPitch"]
-    fractionMaxSpeed  = 0.5
-    angles=[0,old]
+    fractionMaxSpeed  = 0.05
+    angles=[0, 0]
     motionProxy.setAngles(names,angles,fractionMaxSpeed)