robot now runs after the ball

scripts/live_recognition_with_head.py

@@ -53,30 +53,8 @@ def set_angle_new(x,y):
     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=[-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:
     global counter
     if abs(x_diff) > 0.1:
         motionProxy.changeAngles(names, angles, fractionMaxSpeed)
@@ -84,106 +62,45 @@ def set_angle_new(x,y):
     else:
         counter += 1
         print(counter)
-    if counter == 10:
+    if counter == 4:
+        counter = 0
         print('Going to rotate')
         angle = get_angle()
         if abs(angle[1]) > 0.1:
-            move_to(0, angle[1])
+            motionProxy.setStiffnesses("Head", 0.7)
-            #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)
-            #tts.setParameter("pitchShift", 1)
-            #tts.setParameter("speed", 50)
-            #tts.setVoice("Kenny22Enhanced")
-
-            #tts.say("Ball found")
-            global counter
-            counter=counter+1
-	    #print(counter)
-	    #print("Ball found")
-            if counter==5:
-		    global counter
-                    counter=0
-	            print(get_angle())
-
-     '''
-
-def set_angle(direction):
-#def main(robotIP,x,y):
-    robotIP="192.168.0.11"
-    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
+            fractionMaxSpeed  = 0.05
-    a=get_angle()
+            angles=[0, 0]
-    #print(a[0])
+            motionProxy.setAngles(names,angles,fractionMaxSpeed)
-#    print(a)
+            move_to(0, 0, angle[1])
-       
+        move_to(0.3, 0, 0)
-    
-    #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)
 
 
 if __name__ == '__main__':
-    video = NaoImageReader(nao_ip, port=nao_port, cam_id=cam_id, res=res,
+    video_top = NaoImageReader(nao_ip, port=nao_port, cam_id=0, res=res,
+                               fps=fps)
+    video_low = NaoImageReader(nao_ip, port=nao_port, cam_id=1, 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())
+                (x, y), radius = finder.find_colored_ball(video_top.get_frame())
+                x, y = video_top.to_relative(x, y)
+                print('Top camera')
+            except TypeError:
+                try:
+                    (x, y), radius = finder.find_colored_ball(
+                        video_low.get_frame()
+                    )
+                    x, y = video_low.to_relative(x, y)
+                    print('Low camera')
                 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:
-                set_angle("up")
-            elif 240>y>200:
-                set_angle("down")
-            if 0<x<100:  # maybe do simultaneous x-y setting
-                set_angle("left")
-            elif 320>x>220:
-                set_angle("right")
-  	    '''
     finally:
-        video.close()
+        video_top.close()
+        video_low.close()

scripts/live_recognition_with_head_with_body.py

@@ -32,7 +32,7 @@ current_value = 0
 global counter
 counter=0
 
-def move_to(old,y):
+def move_to(front, side, angle):
     motionProxy  = ALProxy("ALMotion", nao_ip, nao_port)
     postureProxy = ALProxy("ALRobotPosture", nao_ip, nao_port)
 
@@ -110,7 +110,8 @@ def move_to(old,y):
     #    PLOT_ALLOW = False
 
     # Second call of move API
-    motionProxy.post.moveTo(0, 0.0, y)
+    motionProxy.post.moveTo(front, side, angle)
+    # motionProxy.waitUntilMoveIsFinished()
 
     # get the second foot steps vector
     # footSteps2 = []
@@ -119,12 +120,6 @@ def move_to(old,y):
     #except Exception, errorMsg:
         #print str(errorMsg)
         #PLOT_ALLOW = False
-    motionProxy.setStiffnesses("Head", 0.7)
-    names  = ["HeadYaw", "HeadPitch"]
-    fractionMaxSpeed  = 0.05
-    angles=[0, 0]
-    motionProxy.setAngles(names,angles,fractionMaxSpeed)
-
 
 
 def get_angle():

scripts/move_robot.py

@@ -70,32 +70,15 @@ def main(robotIP, PORT=9559):
     # with post prefix to not be bloquing here.
     motionProxy.post.moveTo(0, 0.0, 0)
 
-    # wait that the move process start running
-    time.sleep(0.1)
-
-    # get robotPosition and nextRobotPosition
-    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 = []
-    try:
-        footSteps1 = motionProxy.getFootSteps()
-    except Exception, errorMsg:
-        print str(errorMsg)
-        PLOT_ALLOW = False
-
     while True:
-        cmd = int(raw_input(">"))
+        ax = int(raw_input("Axis: "))
+        mag = float(raw_input('How much: '))
 
-    # Second call of move API
+        if ax:
-        motionProxy.post.moveTo(3, 0, 0)
+            motionProxy.post.moveTo(0, mag, 0)
-
-    # here we wait until the move process is over
             motionProxy.waitUntilMoveIsFinished()
+        else:
+            motionProxy.post.moveTo(mag, 0, 0)
    
     ''' 
      # Second call of move API