diff --git a/scripts/live_recognition_with_head.py b/scripts/live_recognition_with_head.py index 8af6b9c..b3322a2 100644 --- a/scripts/live_recognition_with_head.py +++ b/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.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 - 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) + motionProxy.setStiffnesses("Head", 0.7) + names = ["HeadYaw", "HeadPitch"] + fractionMaxSpeed = 0.05 + angles=[0, 0] + motionProxy.setAngles(names,angles,fractionMaxSpeed) + move_to(0, 0, angle[1]) + move_to(0.3, 0, 0) if __name__ == '__main__': - video = NaoImageReader(nao_ip, port=nao_port, cam_id=cam_id, res=res, - fps=fps) + 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: - continue + 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 0y>200: - set_angle("down") - if 0x>220: - set_angle("right") - ''' finally: - video.close() + video_top.close() + video_low.close() diff --git a/scripts/live_recognition_with_head_with_body.py b/scripts/live_recognition_with_head_with_body.py index 6b4843a..8cc6473 100644 --- a/scripts/live_recognition_with_head_with_body.py +++ b/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(): diff --git a/scripts/move_robot.py b/scripts/move_robot.py index fd69125..a1f5600 100644 --- a/scripts/move_robot.py +++ b/scripts/move_robot.py @@ -69,33 +69,16 @@ def main(robotIP, PORT=9559): # First call of move API # 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 - motionProxy.post.moveTo(3, 0, 0) - - # here we wait until the move process is over - motionProxy.waitUntilMoveIsFinished() + if ax: + motionProxy.post.moveTo(0, mag, 0) + motionProxy.waitUntilMoveIsFinished() + else: + motionProxy.post.moveTo(mag, 0, 0) ''' # Second call of move API