def endPosture(ip, port=9559): posture = ALProxy("ALRobotPosture", ip, port) posture.goToPosture("Crouch", 0.5)
def yellowStickDetection(yellow_thresholdMin, yellow_thresholdMax, robotIP="127.0.0.1", port=9559): MOTION = ALProxy("ALMotion", robotIP, port) CAMERA = ALProxy("ALVideoDevice", robotIP, port) TTS = ALProxy("ALTextToSpeech", robotIP, port) #初始化机器人姿势 standWithStick(0.1, robotIP, port) MOTION.angleInterpolation(["HeadPitch", "HeadYaw"], 0, 0.5, True) imageContours = [] angleFlag = 0 stickHeight = 0.46 robotHeight = 0.478 + 0.05 imageWidth = 640 imageHeight = 480 for i in range(0, 5): #检测正前方 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) #获取照片轮廓特征点 time.sleep(2) imageContours = pictureHandle(0, yellow_thresholdMin, yellow_thresholdMax, robotIP, port) if (imageContours != []): angleFlag = 0 break #检测左前方45° MOTION.angleInterpolation("HeadYaw", 45 * math.pi / 180.0, 0.5, True) time.sleep(2) imageContours = pictureHandle(0, yellow_thresholdMin, yellow_thresholdMax, robotIP, port) if (imageContours != []): angleFlag = 1 break #检测右前方45° MOTION.angleInterpolation("HeadYaw", -45 * math.pi / 180.0, 0.5, True) time.sleep(2) imageContours = pictureHandle(0, yellow_thresholdMin, yellow_thresholdMax, robotIP, port) if (imageContours != []): angleFlag = -1 break TTS.say("没有检测到黄杆") #检测5次都没有检测到,就返回False if (i == 4): return False #让机器人看向正前方 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) TTS.say("哈哈,我看到了") cnt = imageContours[0] #获取图像的距 M = cv2.moments(cnt) #黄杆重心 cx = int(M['m10'] / M['m00']) cy = int(M['m01'] / M['m00']) #计算黄杆重心和图片中心的距离,正:黄杆在中心左边 负:黄杆在中心右边 disX = imageWidth / 2 - cx disY = cy - imageHeight / 2 #计算黄杆重心和图片中心的偏转角度,机器人水平视角为60.97° stickAngleH = 60.97 / 640 * disX stickAngleV = 47.64 / 480 * disY + 1.2 if (angleFlag == 1): stickAngleH = stickAngleH + 45 elif (angleFlag == -1): stickAngleH = stickAngleH - 45 else: stickAngleH = stickAngleH distStick = (robotHeight - stickHeight / 2) / (math.tan( stickAngleV * math.pi / 180.0)) #--------------------------测试代码-----------------------------------# print "distStick = ", distStick print "stickAngleH = ", stickAngleH #imgTest = cv2.imread("camImage.png",1) #cv2.circle(imgTest,(cx,cy), 5, (0,0,255), -1) #cv2.namedWindow("image",cv2.WINDOW_NORMAL) #cv2.namedWindow("image2",cv2.WINDOW_NORMAL) #cv2.namedWindow("image3",cv2.WINDOW_NORMAL) #cv2.imshow("image",dilation) #cv2.imshow("image",imgTest) #cv2.imshow("image3",th1) #cv2.waitKey(0) #--------------------------测试结束-----------------------------------# return [distStick, stickAngleH * math.pi / 180]
import time from naoqi import ALProxy def main(robotIP): PORT = 9559 try: motionProxy = ALProxy("ALMotion", robotIP, PORT) except Exception,e: print "Could not create proxy to ALMotion" print "Error was: ",e sys.exit(1) try: postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) except Exception, e: print "Could not create proxy to ALRobotPosture" print "Error was: ", e # Send NAO to Pose Init postureProxy.goToPosture("StandInit", 0.5) ##################################### # A small example using getFootSteps ##################################### # First call of move API # with post prefix to not be bloquing here. motionProxy.post.moveTo(0.3, 0.0, 0.5)
# -*- encoding: UTF-8 -*- import sys import time import random import color import motion from naoqi import ALProxy tts = ALProxy("ALTextToSpeech",config.ipnao,9559) sr = ALProxy("ALSpeechRecognition",config.ipnao,9559) memProxy = ALProxy("ALMemory",config.ipnao,9559) motion = ALProxy("ALMotion",config.ipnao,9559) sr.setVocabulary(["oui","non","rouge","bleu","vert","jaune","orange","rose","blanc"],True) sr.setAudioExpression(True) sr.setVisualExpression(True) fini = False x = "WordRecognized" cpt = 3 def onWordRecognized(x,total): """Ici on regarde le mot reconnu si l'indice de fiabilité est supérieur à 0.5 """ retVal = memProxy.getData("WordRecognized") print x, retVal[0], retVal[1] print total if(retVal[0] != "" and retVal[1]>0.5): if(retVal[0] == "rouge"): return True elif retVal[0] == "bleu":
def redBallDetection(red_thresholdMin, red_thresholdMax, method=1, addAngle=0, robotIP="127.0.0.1", port=9559): MOTION = ALProxy("ALMotion", robotIP, port) CAMERA = ALProxy("ALVideoDevice", robotIP, port) TTS = ALProxy("ALTextToSpeech", robotIP, port) #初始化机器人姿势 MOTION.setMoveArmsEnabled(False, False) standWithStick(0.3, robotIP, port) MOTION.angleInterpolation("HeadPitch", 0, 0.5, True) MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) MOTION.angleInterpolation("HeadPitch", addAngle * math.pi / 180.0, 0.5, False) angleFlag = 0 cameraID = 1 val = [] robotHeight = 0.478 TTS.say("开始检测红球") if method == 0: #首先识别正前方,然后是左边45度,最后是右边45度,三个方向由标志位angleFlag进行标记 for i in range(0, 5): #先打开下方摄像头进行检测 CAMERA.setActiveCamera(1) cameraID = 1 val = searchBall(0, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = 0 break val = searchBall(45, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = 1 break val = searchBall(-45, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = -1 break #先打开上方摄像头进行检测 CAMERA.setActiveCamera(0) cameraID = 1 val = searchBall(0, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = -1 break val = searchBall(45, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = -1 break val = searchBall(-45, True, robotIP, port) time.sleep(1) if (val and isinstance(val, list) and len(val) >= 2): angleFlag = -1 break TTS.say("球在哪里呢") #如果5次都没有检测到就返回False if (i == 4): TTS.say("红球检测失败") return False #检测完红球后,最后看向正前方 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) time.sleep(0.5) TTS.say("哈哈!我看到了!") #看到球之后,获取球的水平垂直偏角 ballinfo = val[1] thetah = ballinfo[0] thetav = ballinfo[1] #根据红球方向标志位确定红球具体角度值 if (cameraID == 0): thetav = thetav + 39.7 * math.pi / 180.0 + addAngle * math.pi / 180.0 elif (cameraID == 1): thetav = thetav + 1.2 * math.pi / 180.0 + addAngle * math.pi / 180.0 robotHeight = robotHeight + 0.05 if (angleFlag == 1): theta = thetah + 45 * math.pi / 180.0 elif (angleFlag == -1): theta = thetah - 45 * math.pi / 180.0 else: theta = thetah distBall = robotHeight / (math.tan(thetav)) return [distBall, theta] elif method == 1: redBallContours = [] imageWidth = 640 imageHeight = 480 for i in range(0, 5): CAMERA.setActiveCamera(1) cameraID = 1 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = 0 break #检测左前方45° MOTION.angleInterpolation("HeadYaw", 45 * math.pi / 180, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = 1 break #检测右前方45° MOTION.angleInterpolation("HeadYaw", -45 * math.pi / 180, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = -1 break CAMERA.setActiveCamera(0) cameraID = 0 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = 0 break #检测左前方45° MOTION.angleInterpolation("HeadYaw", 45 * math.pi / 180, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = 1 break #检测右前方45° MOTION.angleInterpolation("HeadYaw", -45 * math.pi / 180, 0.5, True) redBallContours = pictureHandle(cameraID, red_thresholdMin, red_thresholdMax, robotIP, port) if (redBallContours != []): angleFlag = -1 break TTS.say("球在哪里呢") #检测5次都没有检测到,就返回False if (i == 4): TTS.say("红球检测失败") return False #让机器人看向正前方 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) TTS.say("哈哈,我看到了") cnt = redBallContours[0] #获取图像的距 M = cv2.moments(cnt) #红球重心 cx = int(M['m10'] / M['m00']) cy = int(M['m01'] / M['m00']) #计算红球重心和图片中心的距离,正:红球在中心左边 负:红球在中心右边 disX = imageWidth / 2 - cx disY = cy - imageHeight / 2 #计算红球重心和图片中心的偏转角度,机器人水平视角为60.97° ballAngleH = 60.97 / 640 * disX ballAngleV = 47.64 / 480 * disY if (cameraID == 0): ballAngleV = ballAngleV + 1.2 + addAngle elif (cameraID == 1): ballAngleV = ballAngleV + 39.7 + addAngle if (angleFlag == 1): ballAngleH = ballAngleH + 45 elif (angleFlag == -1): ballAngleH = ballAngleH - 45 else: ballAngleH = ballAngleH #机器人到球的距离 distBall = robotHeight / (math.tan(ballAngleV * math.pi / 180.0)) #---------------------------------测试代码--------------------------------------------# print "distBall = ", distBall print "ballAngleH = ", ballAngleH * math.pi / 180.0 #imgTest = cv2.imread("camImage.png",1) #cv2.circle(imgTest,(cx,cy), 5, (0,255,255), -1) #cv2.namedWindow("image",cv2.WINDOW_NORMAL) #cv2.namedWindow("image2",cv2.WINDOW_NORMAL) #cv2.namedWindow("image3",cv2.WINDOW_NORMAL) #cv2.imshow("image",dilation) #cv2.imshow("image",imgTest) #cv2.imshow("image3",th1) #cv2.waitKey(0) #---------------------------------测试结束--------------------------------------------# return [distBall, ballAngleH * math.pi / 180.0]
def new(robotIP, port=9559): tts = ALProxy("ALTextToSpeech", robotIP, port) motionProxy = ALProxy("ALMotion", robotIP, port) postureProxy = ALProxy("ALRobotPosture", robotIP, port) Low_Level(motionProxy, postureProxy, tts)
def exe(self, args=None, addr=None): # create proxy motion = ALProxy("ALMotion", Settings.naoHostName, Settings.naoPort) motion.stopMove()
def Scan(state): global tracker global cameraNo, bar, z, headMovement, posBar # construct the argument parser and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-o", "--output", type=str, default="barcodes.csv", help="path to output CSV file containing barcodes") args = vars(ap.parse_args()) try: # Proxy to camera and motion print("Starting video stream...") camera = ALProxy("ALVideoDevice", "Mario.local", 9559) headMovement = ALProxy("ALMotion", "Mario.local", 9559) tracker = ALProxy("ALTracker", "Mario.local", 9559) except: print("Error connecting to proxies") csv = open(args["output"], "w") found = set() targetName = "Face" faceWidth = 0.1 # default tracker.registerTarget(targetName, faceWidth) # Subscribe to camera if state == "unsure": cameraNo = 0 elif state == "deal" or state == "boneyard": cameraNo = 1 # mainDomino.cameraNo # mainDomino.cameraNo AL_kQVGA = 3 # 1280x960 AL_kBGRColorSpace = 13 try: captureDevice = camera.subscribeCamera("captureDevice", cameraNo, AL_kQVGA, AL_kBGRColorSpace, 10) except: print("Error subscribing to camera") # init = False # Create image width = 1280 height = 960 image = np.zeros((height, width, 3), np.uint8) bar = [] posBar = [] while True: # Position head at center moveHeadCenter() # Get the image try: result = camera.getImageRemote(captureDevice) except: print("Error getting image remotely") if result == None: print 'Error: Cannot capture.' elif result[6] == None: print 'Error: No image data string.' else: # Translate value to mat values = map(ord, list(result[6])) i = 0 for y in range(0, height): for x in range(0, width): 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 # if cameraNo == 1: # tiles = image[300:800, 0:1280] # else: tiles = image grey = cv2.cvtColor(tiles, cv2.COLOR_BGR2GRAY) # convert image to grey ret,thresh1 = cv2.threshold(grey,127,255,cv2.THRESH_BINARY) ret3,th3 = cv2.threshold(grey,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) barcodes = pyzbar.decode(th3) # find and decode barcodes # Loop over detected barcodes for barcode in barcodes: (x, y, w, h) = barcode.rect # Convert data into string barcodeData = barcode.data.decode("utf-8") barcodeType = barcode.type # Put barcode data, x and y coords on the image text = "{}".format(barcodeData) cv2.putText(image, text, (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) if barcodeData not in found: found.add(barcodeData) # Put new found barcodes into an array barcode = map(int, barcodeData) # Put the barcode x,y position into array posXYZ = [x, y, z] # Add info to barcode position array, so we know where the barcode was found if len(barcode) > 2: break else: bar.insert(0, barcode) posBar.insert(0, posXYZ) print bar print posBar #moveHeadLeft() # moveHeadRight() if state == "deal": # If we haven't seen all tiles that have been dealt, move head from side to side. # if len(bar) < 7: # moveHeadLeft() # moveHeadRight() # If we have seen them all, then stop video stream. if len(bar) == 7: # tracker.track(targetName) camera.releaseImage(captureDevice) camera.unsubscribe(captureDevice) # camera.stop() csv.close() cv2.destroyAllWindows() #camera.stop() return bar, posBar elif state == "boneyard" or state == "unsure": if len(bar) == 1: # tracker.track(targetName) camera.releaseImage(captureDevice) camera.unsubscribe(captureDevice) csv.close() cv2.destroyAllWindows() #camera.stop() # camera.stop() return bar, posBar # Show video stream cv2.imshow("Barcode Scanner", th3) #cv2.imshow("Barcode Scanner", image) #cv2.imshow("new image", new_image) # cv2.waitKey(1) key = cv2.waitKey(1) & 0xFF # if the `q` key was pressed, break from the loop if key == ord("q"): break # close the output CSV file do a bit of cleanup # print("[INFO] cleaning up...") csv.close() cv2.destroyAllWindows() camera.stop()
from naoqi import ALProxy view = ALProxy("ALTabletService", "192.168.1.19", 9559) view.showWebview() view.loadUrl("http://bot-front.eu-gb.mybluemix.net")
# report.txt for details. # How to run : Fix ip variable, then: python run.py from time import time, sleep import random, sys from naoqi import ALModule, ALProxy, ALBroker # our modules from gestureRecognition import * # global variables ip = "192.168.1.102" port = 9559 # proxies postureProxy = ALProxy("ALRobotPosture", ip, port) motionProxy = ALProxy("ALMotion", ip, port) tts = ALProxy("ALTextToSpeech", ip, port) memory = ALProxy("ALMemory", ip, port) aup = ALProxy("ALAudioPlayer", ip, port) LED = ALProxy("ALLeds", ip, port) ################################################################################ # Communication functions ################################################################################ # blink eyes based on a probability to keep it random def blinkEyes(): LED.off("FaceLeds") sleep(0.15) LED.on("FaceLeds")
#!/usr/bin/env python # -*- coding: utf-8 -*- from naoqi import ALProxy # プロキシのインスタンス生成 photo = ALProxy("ALPhotoCapture", "192.168.10.74", 9559) # フォーマットの指定(“bmp”, “dib”, “jpeg”, “jpg”, “jpe”, “png”, “pbm”, “pgm”, “ppm”, “sr”, “ras”, “tiff”, “tif”が指定可能) photo.setPictureFormat("jpeg") # 解像度の指定(0 = kQQVGA, 1 = kQVGA, 2 = kVGA) photo.setResolution(4) # 保存先を指定して画像をキャプチャ photo.takePicture("/home/nao/recordings/cameras/", "image1")
frame_window = 10 emotion_offsets = (20, 40) # loading models face_detection = load_detection_model(detection_model_path) emotion_classifier = load_model(emotion_model_path, compile=False) # getting input model shapes for inference emotion_target_size = emotion_classifier.input_shape[1:3] # starting lists for calculating modes emotion_window = [] emotion_window1 = [] # starting video streaming camProxy = ALProxy("ALVideoDevice", "127.0.0.1", 55955) cameraIndex = 0 resolution = vision_definitions.kVGA colorSpace = vision_definitions.kRGBColorSpace resolution = 2 colorSpace = 3 cv2.namedWindow('window_frame') video_capture = cv2.VideoCapture(0) if video_capture.isOpened(): frame = video_capture.read() else: rval = False while True: rval, frame = video_capture.read() gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
def main(robotIP, port): names = list() times = list() keys = list() names.append("HeadPitch") times.append([ 0.833333, 1.16667, 1.5, 1.83333, 2.16667, 2.83333, 3.16667, 3.5, 3.83333, 4.16667, 4.5, 5.16667, 5.5 ]) keys.append([[-0.00730551, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.00730551, [3, -0.222222, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.00730551, [3, -0.222222, 0], [3, 0.111111, 0]], [-0.00730551, [3, -0.111111, 0], [3, 0, 0]]]) names.append("HeadYaw") times.append([ 0.833333, 1.16667, 1.5, 1.83333, 2.16667, 2.83333, 3.16667, 3.5, 3.83333, 4.16667, 4.5, 5.16667, 5.5 ]) keys.append( [[0, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.441568, [3, -0.111111, 0.104138], [3, 0.111111, -0.104138]], [-0.624828, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.403339, [3, -0.111111, -0.104138], [3, 0.111111, 0.104138]], [0, [3, -0.111111, -0.114241], [3, 0.222222, 0.228481]], [0.624828, [3, -0.222222, 0], [3, 0.111111, 0]], [0, [3, -0.111111, 0.177733], [3, 0.111111, -0.177733]], [-0.441568, [3, -0.111111, 0.104138], [3, 0.111111, -0.104138]], [-0.624828, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.403339, [3, -0.111111, -0.104138], [3, 0.111111, 0.104138]], [0, [3, -0.111111, -0.114241], [3, 0.222222, 0.228481]], [0.624828, [3, -0.222222, 0], [3, 0.111111, 0]], [0.00643452, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LAnklePitch") times.append([5.5]) keys.append([[-0.340758, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LAnkleRoll") times.append([5.5]) keys.append([[-0.00211309, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LElbowRoll") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append([[-1.12892, [3, -0.277778, 0], [3, 0.111111, 0]], [-1.12944, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.10481, [3, -0.111111, 0], [3, 0.222222, 0]], [-1.4848, [3, -0.222222, 0], [3, 0.111111, 0]], [-1.39704, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.39704, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.12892, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.12944, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.10481, [3, -0.111111, 0], [3, 0.222222, 0]], [-1.4848, [3, -0.222222, 0], [3, 0.111111, 0]], [-1.39704, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.39704, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.01329, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LElbowYaw") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[-0.0381204, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.0482739, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.000871307, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.464231, [3, -0.222222, 0.00273673], [3, 0.111111, -0.00136837]], [-0.4656, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.4656, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.0381204, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.0482739, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.000871307, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.464231, [3, -0.222222, 0.00273673], [3, 0.111111, -0.00136837]], [-0.4656, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.4656, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.38861, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LHand") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.29, [3, -0.277778, 0], [3, 0.111111, 0]], [0.29, [3, -0.111111, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, -0.024233], [3, 0.222222, 0.048466]], [0.740441, [3, -0.222222, -0.112393], [3, 0.111111, 0.0561963]], [0.82, [3, -0.111111, 0], [3, 0.111111, 0]], [0.82, [3, -0.111111, 0], [3, 0.111111, 0]], [0.29, [3, -0.111111, 0], [3, 0.111111, 0]], [0.29, [3, -0.111111, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, -0.024233], [3, 0.222222, 0.048466]], [0.740441, [3, -0.222222, -0.112393], [3, 0.111111, 0.0561963]], [0.82, [3, -0.111111, 0], [3, 0.111111, 0]], [0.82, [3, -0.111111, 0], [3, 0.111111, 0]], [0.259986, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LHipPitch") times.append([5.5]) keys.append([[-0.45, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LHipRoll") times.append([5.5]) keys.append([[0.00848469, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LHipYawPitch") times.append([5.5]) keys.append([[-0.00730551, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LKneePitch") times.append([5.5]) keys.append([[0.709886, [3, -1.83333, 0], [3, 0, 0]]]) names.append("LShoulderPitch") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.896904, [3, -0.277778, 0], [3, 0.111111, 0]], [0.901058, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0.0593063], [3, 0.222222, -0.118613]], [-0.0317612, [3, -0.222222, 0], [3, 0.111111, 0]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [0.896904, [3, -0.111111, -0.00415387], [3, 0.111111, 0.00415387]], [0.901058, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0.0593063], [3, 0.222222, -0.118613]], [-0.0317612, [3, -0.222222, 0], [3, 0.111111, 0]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [1.39796, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LShoulderRoll") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.323134, [3, -0.277778, 0], [3, 0.111111, 0]], [0.322036, [3, -0.111111, 0], [3, 0.111111, 0]], [0.331618, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.243711, [3, -0.222222, 0], [3, 0.111111, 0]], [0.172022, [3, -0.111111, -0.162022], [3, 0.111111, 0.162022]], [0.728418, [3, -0.111111, 0], [3, 0.111111, 0]], [0.323134, [3, -0.111111, 0.00109782], [3, 0.111111, -0.00109782]], [0.322036, [3, -0.111111, 0], [3, 0.111111, 0]], [0.331618, [3, -0.111111, 0], [3, 0.222222, 0]], [-0.243711, [3, -0.222222, 0], [3, 0.111111, 0]], [0.172022, [3, -0.111111, -0.162022], [3, 0.111111, 0.162022]], [0.728418, [3, -0.111111, 0], [3, 0.111111, 0]], [0.300643, [3, -0.111111, 0], [3, 0, 0]]]) names.append("LWristYaw") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[-0.28573, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.277897, [3, -0.111111, -0.00783302], [3, 0.111111, 0.00783302]], [0.305228, [3, -0.111111, -0.15925], [3, 0.222222, 0.3185]], [1.15535, [3, -0.222222, 0], [3, 0.111111, 0]], [1.15175, [3, -0.111111, 0], [3, 0.111111, 0]], [1.15175, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.28573, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.277897, [3, -0.111111, -0.00783302], [3, 0.111111, 0.00783302]], [0.305228, [3, -0.111111, -0.15925], [3, 0.222222, 0.3185]], [1.15535, [3, -0.222222, 0], [3, 0.111111, 0]], [1.15175, [3, -0.111111, 0], [3, 0.111111, 0]], [1.15175, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.00264126, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RAnklePitch") times.append([5.5]) keys.append([[-0.342235, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RAnkleRoll") times.append([5.5]) keys.append([[-0.00532818, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RElbowRoll") times.append([ 0.833333, 1.16667, 1.5, 1.83333, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.16667, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[1.46317, [3, -0.277778, 0], [3, 0.111111, 0]], [1.48296, [3, -0.111111, 0], [3, 0.111111, 0]], [1.39704, [3, -0.111111, 0.0859244], [3, 0.111111, -0.0859244]], [0.666716, [3, -0.111111, 0], [3, 0.111111, 0]], [1.11667, [3, -0.111111, 0], [3, 0.111111, 0]], [1.10481, [3, -0.111111, 0], [3, 0.111111, 0]], [1.10481, [3, -0.111111, 0], [3, 0.111111, 0]], [1.46317, [3, -0.111111, -0.0197868], [3, 0.111111, 0.0197868]], [1.48296, [3, -0.111111, 0], [3, 0.111111, 0]], [1.39704, [3, -0.111111, 0.0859244], [3, 0.111111, -0.0859244]], [0.666716, [3, -0.111111, 0], [3, 0.111111, 0]], [1.11667, [3, -0.111111, 0], [3, 0.111111, 0]], [1.10481, [3, -0.111111, 0], [3, 0.111111, 0]], [1.10481, [3, -0.111111, 0], [3, 0.111111, 0]], [1.01181, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RElbowYaw") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.47155, [3, -0.277778, 0], [3, 0.111111, 0]], [0.470375, [3, -0.111111, 0.000991634], [3, 0.111111, -0.000991634]], [0.4656, [3, -0.111111, 0.00477521], [3, 0.222222, -0.00955043]], [-0.00480866, [3, -0.222222, 0], [3, 0.111111, 0]], [0.000871307, [3, -0.111111, 0], [3, 0.111111, 0]], [0.000871307, [3, -0.111111, 0], [3, 0.111111, 0]], [0.47155, [3, -0.111111, 0], [3, 0.111111, 0]], [0.470375, [3, -0.111111, 0.000991634], [3, 0.111111, -0.000991634]], [0.4656, [3, -0.111111, 0.00477521], [3, 0.222222, -0.00955043]], [-0.00480866, [3, -0.222222, 0], [3, 0.111111, 0]], [0.000871307, [3, -0.111111, 0], [3, 0.111111, 0]], [0.000871307, [3, -0.111111, 0], [3, 0.111111, 0]], [1.38165, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RHand") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.751334, [3, -0.277778, 0], [3, 0.111111, 0]], [0.753008, [3, -0.111111, -0.001674], [3, 0.111111, 0.001674]], [0.82, [3, -0.111111, 0], [3, 0.222222, 0]], [0.308975, [3, -0.222222, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, 0], [3, 0.111111, 0]], [0.751334, [3, -0.111111, -0.001674], [3, 0.111111, 0.001674]], [0.753008, [3, -0.111111, -0.001674], [3, 0.111111, 0.001674]], [0.82, [3, -0.111111, 0], [3, 0.222222, 0]], [0.308975, [3, -0.222222, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, 0], [3, 0.111111, 0]], [0.314233, [3, -0.111111, 0], [3, 0.111111, 0]], [0.256119, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RHipPitch") times.append([5.5]) keys.append([[-0.45526, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RHipRoll") times.append([5.5]) keys.append([[-0.00467092, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RHipYawPitch") times.append([5.5]) keys.append([[-0.00847434, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RKneePitch") times.append([5.5]) keys.append([[0.708487, [3, -1.83333, 0], [3, 0, 0]]]) names.append("RShoulderPitch") times.append([ 0.833333, 1.16667, 1.5, 1.83333, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.16667, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[-0.0943158, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.01006, [3, -0.111111, -0.0190615], [3, 0.111111, 0.0190615]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.10472, [3, -0.111111, 0], [3, 0.111111, 0]], [0.850918, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.0943158, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.01006, [3, -0.111111, -0.0190615], [3, 0.111111, 0.0190615]], [0.0200533, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.10472, [3, -0.111111, 0], [3, 0.111111, 0]], [0.850918, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0], [3, 0.111111, 0]], [0.841751, [3, -0.111111, 0], [3, 0.111111, 0]], [1.3964, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RShoulderRoll") times.append([ 0.833333, 1.16667, 1.5, 1.83333, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.16667, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[0.246735, [3, -0.277778, 0], [3, 0.111111, 0]], [-0.0620962, [3, -0.111111, 0.162526], [3, 0.111111, -0.162526]], [-0.728418, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.267035, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.336307, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.331618, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.331618, [3, -0.111111, 0], [3, 0.111111, 0]], [0.246735, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.0620962, [3, -0.111111, 0.162526], [3, 0.111111, -0.162526]], [-0.728418, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.267035, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.336307, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.331618, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.331618, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.294826, [3, -0.111111, 0], [3, 0, 0]]]) names.append("RWristYaw") times.append([ 0.833333, 1.16667, 1.5, 2.16667, 2.5, 2.83333, 3.16667, 3.5, 3.83333, 4.5, 4.83333, 5.16667, 5.5 ]) keys.append( [[-1.17584, [3, -0.277778, 0], [3, 0.111111, 0]], [-1.17422, [3, -0.111111, -0.00161095], [3, 0.111111, 0.00161095]], [-1.15175, [3, -0.111111, -0.0224728], [3, 0.222222, 0.0449456]], [-0.302801, [3, -0.222222, 0], [3, 0.111111, 0]], [-0.305228, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.305228, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.17584, [3, -0.111111, 0], [3, 0.111111, 0]], [-1.17422, [3, -0.111111, -0.00161095], [3, 0.111111, 0.00161095]], [-1.15175, [3, -0.111111, -0.0224728], [3, 0.222222, 0.0449456]], [-0.302801, [3, -0.222222, 0], [3, 0.111111, 0]], [-0.305228, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.305228, [3, -0.111111, 0], [3, 0.111111, 0]], [-0.000403613, [3, -0.111111, 0], [3, 0, 0]]]) try: motion = ALProxy("ALMotion", robotIP, port) motion.angleInterpolationBezier(names, times, keys) posture = ALProxy("ALRobotPosture", robotIP, port) #posture.goToPosture("StandInit", 1.0) except BaseException, err: print err
type="string", dest="IP", default="127.0.0.1") parser.add_option("-p", "--broker-port", action="store", type="int", dest="PORT", default=9559) parser.add_option("-n", "--no-arms", action="store_true", dest="NOARMS") (options, args) = parser.parse_args() print("----- Started") try: navigation_proxy = ALProxy("ALNavigation", options.IP, options.PORT) motion_proxy = ALProxy("ALMotion", options.IP, options.PORT) except Exception, e: print("Could not create proxies to ALNavigation or ALMotion") print(str(e)) exit(1) if options.NOARMS: print("----- Moving with arms disabled") motion_proxy.setMoveArmsEnabled(False, False) # get an event e = threading.Event() # spawn looging thread thread = LoggingThread(options.IP, options.PORT, "log.m")
import vision_definitions import time import Image, cv2 import numpy as np import sys IP = "172.20.25.152" # NAOqi's IP address. PORT = 9559 if (len(sys.argv) >= 2): print("On prend l'IP entree") IP = sys.argv[1] # Create proxy on ALVideoDevice print("Creating ALVideoDevice proxy to ", IP) camProxy = ALProxy("ALVideoDevice", IP, PORT) # Register a Generic Video Module camProxy = ALProxy("ALVideoDevice", IP, PORT) camProxy.setParam(18, 0) # "kCameraSelectID", 0 : camera top, 1 : camera bottom resolution = 0 # 0 : QQVGA, 1 : QVGA, 2 : VGA colorSpace = 11 # RGB mini = 25 maxi = 33 seuil = 4 try : videoClient = camProxy.subscribeCamera("python_client",1, resolution, colorSpace, 5) print(videoClient) except RuntimeError:
# Init proxies. try: motionProxy = ALProxy("ALMotion", robotIP, port) except Exception, e: print "Could not create proxy to ALMotion" print "Error was: ", e try: postureProxy = ALProxy("ALRobotPosture", robotIP, port) except Exception, e: print "Could not create proxy to ALRobotPosture" print "Error was: ", e try: ttsProxy = ALProxy("ALTextToSpeech", robotIP, port) except Exception, e: print "Could not create proxy to ALTextToSpeech" print "Error was: ", e # Set NAO in Stiffness On
import sys from naoqi import ALProxy try: proxy = ALProxy("ALLeds", IP, PORT) except Exception,e: print "Could not create proxy to ALLeds" print "Error was: ",e sys.exit(1) while(true) proxy.rotateEyes(const int& rgb, const float& timeForRotation, const float& totalDuration); """ Say 'hello, you' each time a human face is detected """ import sys import time from naoqi import ALProxy from naoqi import ALBroker from naoqi import ALModule from optparse import OptionParser NAO_IP = "nao.local" # Global variable to store the HumanGreeter module instance
def main(robotIP, port): names = list() times = list() keys = list() names.append("HeadPitch") times.append([0.64, 1.16, 1.64, 2.16, 2.64, 3.16, 3.64, 5]) keys.append([ -0.0782759, -0.50166, -0.0782759, -0.50166, -0.0782759, -0.50166, -0.656595, -0.0245859 ]) names.append("HeadYaw") times.append([0.64, 1.16, 1.64, 2.16, 2.64, 3.16, 3.64, 5]) keys.append([ -0.0859459, -0.0706059, -0.0859459, -0.0706059, -0.0859459, -0.0706059, -0.0782759, 0.00609404 ]) names.append("LAnklePitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([-0.0910584, -0.298148, -0.298148, -0.293215, -0.104485]) names.append("LAnkleRoll") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([-0.0329368, 0.0192192, 0.0192192, 0.0667732, 0.0092244]) names.append("LElbowRoll") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append( [0, -0.612024, 0, -0.612024, 0, -0.612024, -0.523053, -0.326699]) names.append("LElbowYaw") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ -0.420357, -0.374338, -0.420357, -0.374338, -0.420357, -0.374338, -0.819198, -0.759372 ]) names.append("LHand") times.append([1.12, 3.12, 4.12, 5]) keys.append([0.796751, 0.796751, 0.295298, 0.918933]) names.append("LHipPitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.340577, 0.25774, 0.25774, -0.13343, 0.0596046]) names.append("LHipRoll") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.090152, 0.0671419, 0.0671419, -0.0555781, -0.0324061]) names.append("LHipYawPitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([-0.360449, -0.384992, -0.384992, -0.483168, 0.0183645]) names.append("LKneePitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.0714636, 0.405876, 0.405876, 0.671257, 0.0702441]) names.append("LShoulderPitch") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ -1.05237, -0.823801, -1.05237, -0.823801, -1.05237, -0.823801, -0.510863, 1.56771 ]) names.append("LShoulderRoll") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ 0.432547, 0.0352401, 0.432547, 0.0352401, 0.432547, 0.0352401, 0.507713, 0.329768 ]) names.append("LWristYaw") times.append([1.12, 3.12, 4.12, 5]) keys.append([-1.30394, -1.30394, 0.676451, -1.02629]) names.append("RAnklePitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([-0.0787807, -0.292008, -0.292008, -0.256563, -0.0951351]) names.append("RAnkleRoll") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.165806, 0.0680678, 0.0558505, -0.0933421, -0.00302827]) names.append("RElbowRoll") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ 4.19617e-05, 0.656595, 4.19617e-05, 0.656595, 4.19617e-05, 0.656595, 0.598302, 0.291501 ]) names.append("RElbowYaw") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ 0.233125, 0.170232, 0.233125, 0.170232, 0.233125, 0.170232, 0.984786, 0.77923 ]) names.append("RHand") times.append([1.12, 3.12, 4.12, 5]) keys.append([0.849115, 0.849115, 0.340389, 0.918205]) names.append("RHipPitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.355635, 0.26973, 0.26973, -0.185867, 0.0367591]) names.append("RHipRoll") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([-0.119116, -0.0853684, -0.0853684, 0.0496235, 0.0107584]) names.append("RKneePitch") times.append([0.72, 1.72, 3.24, 3.72, 5]) keys.append([0.0476684, 0.388217, 0.388217, 0.699619, 0.0839559]) names.append("RShoulderPitch") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ -1.11978, -0.926494, -1.11978, -0.926494, -1.11978, -0.926494, -0.404934, 1.56779 ]) names.append("RShoulderRoll") times.append([0.68, 1.2, 1.68, 2.2, 2.68, 3.2, 3.68, 5]) keys.append([ -0.454107, -0.0245859, -0.454107, -0.0245859, -0.454107, -0.0245859, -0.604437, -0.320648 ]) names.append("RWristYaw") times.append([1.12, 3.12, 4.12, 5]) keys.append([1.31153, 1.31153, -0.421891, 0.967912]) try: motion = ALProxy("ALMotion", robotIP, port) motion.angleInterpolation(names, keys, times, True) except BaseException, err: print err
#-*- coding: utf-8 -*- from naoqi import ALProxy # ----------> Connect to robot <---------- robot_ip = "192.168.1.100" robot_port = 9559 # default port : 9559 motion = ALProxy("ALMotion", robot_ip, robot_port) sensorList = motion.getSensorNames() for sensor in sensorList: print sensor
def main(robotIP, port, unit_time): names = list() times = list() keys = list() names.append("HeadPitch") times.append([0.64, 1.12, 1.6, 2.12, 2.64, 3.12, 3.88]) keys.append([[-0.066004, [3, -0.213333, 0], [3, 0.16, 0]], [0.217786, [3, -0.16, 0], [3, 0.16, 0]], [-0.066004, [3, -0.16, 0.0949853], [3, 0.173333, -0.102901]], [-0.375872, [3, -0.173333, 0], [3, 0.173333, 0]], [0.00456004, [3, -0.173333, 0], [3, 0.16, 0]], [-0.365133, [3, -0.16, 0], [3, 0.253333, 0]], [-0.144238, [3, -0.253333, 0], [3, 0, 0]]]) names.append("HeadYaw") times.append([0.64, 1.12, 1.6, 2.12, 2.64, 3.12, 3.88]) keys.append([[-0.0629359, [3, -0.213333, 0], [3, 0.16, 0]], [-0.918907, [3, -0.16, 0], [3, 0.16, 0]], [-0.0629359, [3, -0.16, 0], [3, 0.173333, 0]], [-1.02936, [3, -0.173333, 0], [3, 0.173333, 0]], [-0.016916, [3, -0.173333, 0], [3, 0.16, 0]], [-1.02015, [3, -0.16, 0], [3, 0.253333, 0]], [-0.032256, [3, -0.253333, 0], [3, 0, 0]]]) names.append("LAnklePitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[-0.0158923, [3, -0.4, 0], [3, 0.333333, 0]], [-0.493928, [3, -0.333333, 0], [3, 0.173333, 0]], [-0.430072, [3, -0.173333, 0], [3, 0.16, 0]], [-0.493928, [3, -0.16, 0], [3, 0.226667, 0]], [0.093532, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LAnkleRoll") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[-0.0973648, [3, -0.4, 0], [3, 0.333333, 0]], [0.0729092, [3, -0.333333, -0.0528154], [3, 0.173333, 0.027464]], [0.143473, [3, -0.173333, 0], [3, 0.16, 0]], [0.0729092, [3, -0.16, 0.0358642], [3, 0.226667, -0.0508076]], [-0.116542, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LElbowRoll") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[-1.44345, [3, -0.226667, 0], [3, 0.16, 0]], [-0.800706, [3, -0.16, 0], [3, 0.16, 0]], [-0.969447, [3, -0.16, 0], [3, 0.173333, 0]], [-0.6335, [3, -0.173333, -0.139594], [3, 0.173333, 0.139594]], [-0.131882, [3, -0.173333, 0], [3, 0.16, 0]], [-0.6335, [3, -0.16, 0], [3, 0.24, 0]], [-0.427944, [3, -0.24, 0], [3, 0, 0]]]) names.append("LElbowYaw") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[-0.744032, [3, -0.226667, 0], [3, 0.16, 0]], [-2.05101, [3, -0.16, 0], [3, 0.16, 0]], [-0.0245859, [3, -0.16, 0], [3, 0.173333, 0]], [-0.483252, [3, -0.173333, 0], [3, 0.173333, 0]], [-0.481718, [3, -0.173333, 0], [3, 0.16, 0]], [-0.483252, [3, -0.16, 0.00153411], [3, 0.24, -0.00230117]], [-1.17509, [3, -0.24, 0], [3, 0, 0]]]) names.append("LHand") times.append([0.6, 1.08, 1.56, 2.08, 3.08, 3.88]) keys.append([[0.613843, [3, -0.2, 0], [3, 0.16, 0]], [0.613843, [3, -0.16, 0], [3, 0.16, 0]], [0.613843, [3, -0.16, 0], [3, 0.173333, 0]], [0.614934, [3, -0.173333, 0], [3, 0.333333, 0]], [0.613479, [3, -0.333333, 0.00145501], [3, 0.266667, -0.00116401]], [0.3048, [3, -0.266667, 0], [3, 0, 0]]]) names.append("LHipPitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.274614, [3, -0.4, 0], [3, 0.333333, 0]], [0.34978, [3, -0.333333, -0.0289308], [3, 0.173333, 0.015044]], [0.406538, [3, -0.173333, 0], [3, 0.16, 0]], [0.34978, [3, -0.16, 0.0370256], [3, 0.226667, -0.052453]], [0.138102, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LHipRoll") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.134638, [3, -0.4, 0], [3, 0.333333, 0]], [0.014986, [3, -0.333333, 0], [3, 0.173333, 0]], [0.0870839, [3, -0.173333, 0], [3, 0.16, 0]], [0.014986, [3, -0.16, 0], [3, 0.226667, 0]], [0.11816, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LHipYawPitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[-0.289883, [3, -0.4, 0], [3, 0.333333, 0]], [-0.54146, [3, -0.333333, 0.14694], [3, 0.173333, -0.0764089]], [-0.959931, [3, -0.173333, 0], [3, 0.16, 0]], [-0.54146, [3, -0.16, -0.108712], [3, 0.226667, 0.154009]], [-0.171766, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LKneePitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.0346476, [3, -0.4, 0], [3, 0.333333, 0]], [0.692733, [3, -0.333333, 0], [3, 0.173333, 0]], [0.67586, [3, -0.173333, 0], [3, 0.16, 0]], [0.692733, [3, -0.16, 0], [3, 0.226667, 0]], [-0.090548, [3, -0.226667, 0], [3, 0, 0]]]) names.append("LShoulderPitch") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[1.72417, [3, -0.226667, 0], [3, 0.16, 0]], [1.54623, [3, -0.16, 0.177943], [3, 0.16, -0.177943]], [0.651908, [3, -0.16, 0], [3, 0.173333, 0]], [1.64287, [3, -0.173333, 0], [3, 0.173333, 0]], [1.64287, [3, -0.173333, 0], [3, 0.16, 0]], [1.64287, [3, -0.16, 0], [3, 0.24, 0]], [1.48027, [3, -0.24, 0], [3, 0, 0]]]) names.append("LShoulderRoll") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[0.789967, [3, -0.226667, 0], [3, 0.16, 0]], [0.484702, [3, -0.16, 0.128089], [3, 0.16, -0.128089]], [0.021434, [3, -0.16, 0], [3, 0.173333, 0]], [0.911154, [3, -0.173333, -0.194562], [3, 0.173333, 0.194562]], [1.18881, [3, -0.173333, 0], [3, 0.16, 0]], [0.911154, [3, -0.16, 0.146855], [3, 0.24, -0.220282]], [0.0873961, [3, -0.24, 0], [3, 0, 0]]]) names.append("LWristYaw") times.append([0.6, 1.08, 1.56, 2.08, 3.08, 3.88]) keys.append([[-0.79312, [3, -0.2, 0], [3, 0.16, 0]], [-0.813062, [3, -0.16, 0], [3, 0.16, 0]], [-0.79312, [3, -0.16, 0], [3, 0.173333, 0]], [-1.42666, [3, -0.173333, 0], [3, 0.333333, 0]], [-1.15514, [3, -0.333333, -0.271517], [3, 0.266667, 0.217214]], [0.145688, [3, -0.266667, 0], [3, 0, 0]]]) names.append("RAnklePitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.00098731, [3, -0.4, 0], [3, 0.333333, 0]], [-0.0802851, [3, -0.333333, 0], [3, 0.173333, 0]], [0.258309, [3, -0.173333, 0], [3, 0.16, 0]], [-0.0802851, [3, -0.16, 0], [3, 0.226667, 0]], [0.107422, [3, -0.226667, 0], [3, 0, 0]]]) names.append("RAnkleRoll") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.179769, [3, -0.4, 0], [3, 0.333333, 0]], [0.261799, [3, -0.333333, -0.0271751], [3, 0.173333, 0.014131]], [0.303687, [3, -0.173333, 0], [3, 0.16, 0]], [0.303687, [3, -0.16, 0], [3, 0.226667, 0]], [0.07214, [3, -0.226667, 0], [3, 0, 0]]]) names.append("RElbowRoll") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[1.24718, [3, -0.226667, 0], [3, 0.16, 0]], [0.673468, [3, -0.16, 0], [3, 0.16, 0]], [0.833004, [3, -0.16, 0], [3, 0.173333, 0]], [0.398883, [3, -0.173333, 0], [3, 0.173333, 0]], [0.658129, [3, -0.173333, 0], [3, 0.16, 0]], [0.398883, [3, -0.16, 0.00613657], [3, 0.24, -0.00920485]], [0.389678, [3, -0.24, 0], [3, 0, 0]]]) names.append("RElbowYaw") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[0.785367, [3, -0.226667, 0], [3, 0.16, 0]], [2.08926, [3, -0.16, 0], [3, 0.16, 0]], [-0.273093, [3, -0.16, 0], [3, 0.173333, 0]], [0.516916, [3, -0.173333, -0.263081], [3, 0.173333, 0.263081]], [1.30539, [3, -0.173333, 0], [3, 0.16, 0]], [0.516916, [3, -0.16, 0], [3, 0.24, 0]], [1.17654, [3, -0.24, 0], [3, 0, 0]]]) names.append("RHand") times.append([0.6, 1.08, 1.56, 2.08, 3.08, 3.88]) keys.append([[0.585115, [3, -0.2, 0], [3, 0.16, 0]], [0.585115, [3, -0.16, 0], [3, 0.16, 0]], [0.585115, [3, -0.16, 0], [3, 0.173333, 0]], [0.587661, [3, -0.173333, -0.000189252], [3, 0.333333, 0.000363946]], [0.588025, [3, -0.333333, 0], [3, 0.266667, 0]], [0.3068, [3, -0.266667, 0], [3, 0, 0]]]) names.append("RHipPitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.277401, [3, -0.4, 0], [3, 0.333333, 0]], [0.0288929, [3, -0.333333, 0], [3, 0.173333, 0]], [0.355635, [3, -0.173333, 0], [3, 0.16, 0]], [0.0288929, [3, -0.16, 0], [3, 0.226667, 0]], [0.131882, [3, -0.226667, 0], [3, 0, 0]]]) names.append("RHipRoll") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[-0.159001, [3, -0.4, 0], [3, 0.333333, 0]], [-0.439722, [3, -0.333333, 0.107313], [3, 0.173333, -0.0558026]], [-0.648346, [3, -0.173333, 0], [3, 0.16, 0]], [-0.439722, [3, -0.16, -0.0803346], [3, 0.226667, 0.113807]], [-0.06592, [3, -0.226667, 0], [3, 0, 0]]]) names.append("RHipYawPitch") times.append([3.88]) keys.append([[-0.171766, [3, -1.29333, 0], [3, 0, 0]]]) names.append("RKneePitch") times.append([1.2, 2.2, 2.72, 3.2, 3.88]) keys.append([[0.0277265, [3, -0.4, 0], [3, 0.333333, 0]], [0.606045, [3, -0.333333, 0], [3, 0.173333, 0]], [0.142776, [3, -0.173333, 0], [3, 0.16, 0]], [0.606045, [3, -0.16, 0], [3, 0.226667, 0]], [-0.091998, [3, -0.226667, 0], [3, 0, 0]]]) names.append("RShoulderPitch") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[1.78715, [3, -0.226667, 0], [3, 0.16, 0]], [1.5095, [3, -0.16, 0.137548], [3, 0.16, -0.137548]], [0.96186, [3, -0.16, 0], [3, 0.173333, 0]], [1.17815, [3, -0.173333, -0.055224], [3, 0.173333, 0.055224]], [1.2932, [3, -0.173333, 0], [3, 0.16, 0]], [1.17815, [3, -0.16, 0], [3, 0.24, 0]], [1.49569, [3, -0.24, 0], [3, 0, 0]]]) names.append("RShoulderRoll") times.append([0.68, 1.16, 1.64, 2.16, 2.68, 3.16, 3.88]) keys.append([[-0.710284, [3, -0.226667, 0], [3, 0.16, 0]], [-0.382007, [3, -0.16, -0.116584], [3, 0.16, 0.116584]], [-0.01078, [3, -0.16, 0], [3, 0.173333, 0]], [-1.63068, [3, -0.173333, 0], [3, 0.173333, 0]], [-1.57546, [3, -0.173333, 0], [3, 0.16, 0]], [-1.63068, [3, -0.16, 0], [3, 0.24, 0]], [-0.104354, [3, -0.24, 0], [3, 0, 0]]]) names.append("RWristYaw") times.append([0.6, 1.08, 1.56, 2.08, 3.08, 3.88]) keys.append([[0.826783, [3, -0.2, 0], [3, 0.16, 0]], [0.849794, [3, -0.16, 0], [3, 0.16, 0]], [0.826783, [3, -0.16, 0], [3, 0.173333, 0]], [1.0937, [3, -0.173333, 0], [3, 0.333333, 0]], [0.27301, [3, -0.333333, 0.189193], [3, 0.266667, -0.151355]], [0.0720561, [3, -0.266667, 0], [3, 0, 0]]]) mul = unit_time/3.95 times_unit = [list(np.array(t, dtype=float)*mul) for t in times] try: motion = ALProxy("ALMotion", robotIP, port) motion.angleInterpolationBezier(names, times_unit, keys) except BaseException, err: print err
def main(IP, PORT): ##在planB中没有标定,只有阈值来确定目标物与机器人的相对位置 ##设置定时器计算程序运行总时间 t0 = time.time() ##这里阈值有两个,一个是棒棒的shB;一个是桶的shT;shW为阈值宽度 ##这两个阈值的作用: ##shB为机器人到达相对于目标物(棒棒)指定位置时目标物在底部摄像头拍摄到画面的位置 ##shT为机器人到达相对于目标物(桶)指定位置时目标物在底部摄像头拍摄到画面的位置 ##其中棒棒在定位过程中先使棒棒出现在底部摄像头视野内,再使目标在图像中处于阈值位置 ##桶的原理同上,但在定位过程中,头部向下转20度(Pitch) shB = 262 shT = 270 shW = 20 ##该程序中有三个标志位: ##flag 0代表顶部摄像头 1代表底部摄像头 ##flag2 0代表机器人没有低头发现目标 1代表低头20度后发现目标 ##flag3 机器人在丢失目标时,0代表因为太远而丢失目标 1代表因为太近而丢失目标 flag = 0 flag2 = 0 flag3 = 0 ##调用ALTextSpeech模块,机器人说hello,说明程序开始 tts = ALProxy("ALTextToSpeech", IP, PORT) tts.say("Hello!") ##第一步:找到棒棒+粗定位 ##转换姿态找到目标: for xunzhaocishu in range(4): ##第1步:找到目标 for t in range(4): pt.posinit() #调整姿态 #前看 angle0 = 0 flag = 0 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break flag = 1 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break #下看 pt.posinit2(0.0, 20) flag = 1 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: flag2 = 1 break flag = 0 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: flag2 = 1 break pt.posinit2(0.0, 0.0) ##左看 angle0 = 30 headm.headcheck(0, 60) flag = 0 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break flag = 1 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break ##右看 angle0 = -30 headm.headcheck(0, -60) flag = 1 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break flag = 0 point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: break ##向左转90度 mt.move(0, 0, 1.57079633) print '找到目标' ##找到目标,机器人说I found the bar! tts.say("I found the bar!") ##第2步:看到了目标向目标转动 ##开始时如果头偏了应将头偏回并将身子转过去,并重新计算距离角度 if flag2 == 1: if flag == 0: #如果是机器人低头且用顶部摄像头发现的先往前走20cm distance, angle, x, y = gp2.getPosition(point, flag) mt.move(0, 0, -angle) mt.move(20, 0, 0) if flag == 1: #如果是机器人低头且用底部摄像头发现的先往后走20cm distance, angle, x, y = gp2.getPosition(point, flag) mt.move(0, 0, -angle) mt.move(-20, 0, 0) if angle0 == 0: #如果是转头发现的先转过身去 distance, angle, x, y = gp1.getPosition(point, flag) elif angle0 != 0: if angle0 == 30: pt.posinit() #调整姿态 mt.move(0, 0, 0.5236) elif angle0 == -30: pt.posinit() #调整姿态 mt.move(0, 0, -0.5236) pt.posinit() #调整姿态 point1, detection1 = gp1.checkexist(IP, PORT, 1) point0, detection0 = gp1.checkexist(IP, PORT, 0) if detection1 == 1: flag = 1 point = point1 break elif detection0 == 1: flag = 0 point = point0 break error = aa.adjustment(IP, PORT, 0, flag) if error == 0: mt.move(10, 0, 0) flag = 1 #第3步:粗定位之后走到目标前,即底部摄像头可以捕捉到目标 st = 25 if flag == 0: ##如果顶部摄像头出现目标0->顶部 1->底部 detection = 0 flag = 1 while detection == 0: print '第一次调整距离' mt.move(st, 0, 0) pt.posinit() point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: distance, angle, x, y = gp1.getPosition(point, flag) if y > 80: print '底部摄像头出现目标' break if detection == 0: print '底部摄像头目标没有出现' error = aa.adjustment(IP, PORT, 0, 0) if error == 0 and detection == 0: error = aa.adjustment2(IP, PORT, 0, 0, 20) print '底部摄像头出现目标' ##第二步:走近棒棒+精定位(近,底部摄像头) #循环调整,精确定位棒的位置,使目标物到达底部摄像头阈值范围内 ad = 0 pt.posinit() detection = 0 while detection == 0: point, detection = gp1.checkexist(IP, PORT, flag) if detection == 1: distance, angle, x, y = gp1.getPosition(point, flag) break else: mt.move(10, 0, 0) flag3 = 0 while y > shB + shW / 2 or y < shB - shW / 2: aa.adjustment(IP, PORT, 0, flag) print '第二次调整距离' print y if y > 180: flag3 = 1 elif y <= 180: flag3 = 0 ##按接近程度选择步长 st = 5 if -40 < shB - y < 40: st = 2 ##根据目标在图像中位置移动 if y > shB + shW / 2: mt.move(-st, 0, 0) elif y < shB - shW / 2: mt.move(st, 0, 0) ##获得目标在图像中位置 pt.posinit() point, detection = gp1.checkexist(IP, PORT, flag) while detection == 0: print '丢失目标' if flag3 == 1: mt.move(-st, 0, 0) else: mt.move(st, 0, 0) point, detection = gp1.checkexist(IP, PORT, flag) distance, angle, x, y = gp1.getPosition(point, flag) ##第三步:捡棒棒 mt.move(0, 0, 0.1 - angle) gt.grab() tts.say("I got it!") mt.move(-20, 0, 0.0) ##第四步:判断是否成功,修补失误* ##第五步:找桶+粗定位(远,顶部摄像头,或近,底部摄像头) #转换姿态找到目标: #第1步:找到桶 flag2 = 0 for xunzhaocishu in range(4): for t in range(4): pt.posinit() #调整姿态 #前看 angle0 = 0 flag = 0 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break flag = 1 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break #下看 pt.posinit2(0.0, 20) flag = 1 point, detection = gp2.find_boxcontours2(IP, PORT, flag) if detection == 1: flag2 = 1 break flag = 0 point, detection = gp2.find_boxcontours2(IP, PORT, flag) if detection == 1: flag2 = 1 break pt.posinit2(0.0, 0) #左看 angle0 = 30 headm.headcheck(0, 30) flag = 0 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break flag = 1 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break #右看 angle0 = -30 headm.headcheck(0, -30) flag = 0 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break flag = 1 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: break #向左转90度 mt.move(0, 0, 1.57079633) print '找到目标' tts.say("I find the can!") #第2步:看到了目标向目标转动 if flag2 == 1: if flag == 0: #如果是低头发现的先往前走20cm distance, angle, x, y = gp2.getPosition(point, flag) mt.move(0, 0, -angle) mt.move(20, 0, 0) if flag == 1: distance, angle, x, y = gp2.getPosition(point, flag) mt.move(0, 0, -angle) #开始时如果头偏了应将头偏回并将身子转过去,并重新计算距离角度 if angle0 != 0: if angle0 == 30: headm.headcheck(0, 0) mt.move(0, 0, 0.5236) elif angle0 == -30: headm.headcheck(0, 0) mt.move(0, 0, -0.5236) if flag2 == 0 or (flag2 == 1 and flag == 0): ##1判断是哪个摄像头捕获目标2确认是否真的识别正确 pt.posinit() #调整姿态 point1, detection1 = gp2.find_boxcontours(IP, PORT, 1) point0, detection0 = gp2.find_boxcontours(IP, PORT, 0) if detection1 == 1: print '摄像头1' flag = 1 elif detection0 == 1: print '摄像头0' flag = 0 ##调整角度 error = aa.adjustment(IP, PORT, 1, flag) if error == 0: mt.move(-10, 0, 0) elif error == 1: break #第3步:向目标物移动,使之出现在底部摄像头内 #如果目标在上摄像头视野中,则向前走,步长为10cm,直到下摄像头视野中出现目标 st = 50 if flag == 0: flag = 1 detection == 0 while (detection == 0): print '第一次调整距离' mt.move(st, 0, 0) pt.posinit() #调整姿态 point, detection = gp2.find_boxcontours(IP, PORT, flag) if detection == 1: distance, angle, x, y = gp2.getPosition(point, flag) if y > 50: print '底部摄像头出现目标' break elif detection == 0: print '底部摄像头目标没有出现' error = aa.adjustment(IP, PORT, 1, 0) if error == 0 and detection == 0: mt.move(-10, 0, 0) print '底部摄像头出现目标' ##第六步:走近桶+精定位(近,底部摄像头,头向下转20°) #循环调整,走到使桶出现在阈值范围内的位置(**需进一步标定,求出shT,以及要走的步长和方向) #一边调整一边低头至20度,且底部摄像头出现桶 for i in range(1, 3): st = 10 error = aa.adjustment2(IP, PORT, 1, 1, 10 * t) if error == 0: error = aa.adjustment2(IP, PORT, 1, 0, 10 * t) if error == 0: mt.move(st, 0, 0) pt.posinit2(0.0, 20) point, detection = gp2.find_boxcontours(IP, PORT, flag) while detection == 0: mt.move(10, 0, 0) pt.posinit2(0.0, 20) point, detection = gp2.find_boxcontours(IP, PORT, flag) ad = 0 if detection == 1: distance, angle, x, y = gp2.getPosition(point, flag) flag3 = 0 while y > shT + shW / 2 or y < shT - shW / 2: aa.adjustment2(IP, PORT, 1, flag, 20) print '第二次调整距离' print y if y > 240: flag3 == 1 ##按接近程度选择步长 st2 = 4 if -40 < shT - y < 40: st2 = 2 ##根据目标在图像中位置移动 if y > shT + shW / 2: mt.move(-st2, 0, 0) elif y < shT - shW / 2: mt.move(st2, 0, 0) ##获得目标在图像中位置 pt.posinit2(0.0, 20) point, detection = gp2.find_boxcontours(IP, PORT, flag) while detection == 0: print '丢失目标' if flag3 == 1: mt.move(-st, 0, 0) else: mt.move(st, 0, 0) pt.posinit2(0.0, 20) point, detection = gp1.checkexist(IP, PORT, flag) distance, angle, x, y = gp1.getPosition(point, flag) ##第七步:判断是否可扔* ##第八步:扔棒棒 mt.move(0, 0, 0.1 - angle) gt.drop() t1 = time.time() # Time the image transfer. print "acquisition delay ", t1 - t0 mt.move(-10, 0, 0) pt.posinit() tts.say("I threw it!")
else: print("Given argument is gibberish, ignoring...") else: print("Given argument is gibberish, ignoring...") # Load pepper ip if load_ip() == 1: print( "Something went wrong while loading pepper's IP, using '127.0.0.1' instead..." ) else: print("Load succes (" + _IP + ")") # Setup sound try: audio = ALProxy("ALAudioPlayer", _IP, 9559) meowID = audio.loadFile("/home/nao/audio/cat_meow2.wav") audio.setVolume(meowID, 0.25) kgggID = audio.loadFile("/home/nao/audio/cat_screech.wav") audio.setVolume(kgggID, 0.25) except: print("Could not connect to Pepper (is her IP correct?)") led_thread = LedThread() connection = Connection() connection.start() # Begin a ros subscriber rospy.init_node("pepper_animation", anonymous=True) # Also start a publisher to push the thing #animationPublisher = rospy.Publisher("/animation", String, queue_size=5)
# -*- encoding: UTF-8 -*- from naoqi import ALProxy ip = "192.168.3.2" port = 9559 # motion motion = ALProxy("ALMotion", ip, port) #motion.moveToward(0.2,0.0,0.0) # left #motion.moveToward(-0.2,0.0,0.0) # right motion.moveToward(0.0, 0.1, 0.0) # left #motion.moveToward(0.0,-0.1,0.0) # right
def find_objects(): """ Process the recorded video and determine object information Returns an array of Objects """ broker = ALBroker("broker", "0.0.0.0", 0, "localhost", 9559) motion = ALProxy("ALMotion") posture = ALProxy("ALRobotPosture") camera = ALProxy("ALVideoDevice", "localhost", 9559) posture.goToPosture("Stand", 0.5) images = record_video(camera, motion) fourcc = cv2.cv.CV_FOURCC('M', 'J', 'P', 'G') out_video = cv2.VideoWriter('/home/nao/output.avi', fourcc, 20.0, (640, 480)) objects = [] old_objects = [] for pair in images: nao_image = pair[0] angle = pair[1] # Convert NAO Format to OpenCV format frame = np.reshape( np.frombuffer(nao_image[6], dtype='%iuint8' % nao_image[2]), (nao_image[1], nao_image[0], nao_image[2])) video_frame = frame.copy() cv2.putText(video_frame, str(angle), (20, 460), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 255, 255)) out_video.write(video_frame) old_found = [] new_contours = [] contours = find_contours(frame) for i in range(len(contours)): contour = contours[i] x, y, w, h = cv2.boundingRect(contour) cx = x + w / 2 cy = y + h / 2 a = camera.getAngularPositionFromImagePosition( 1, (float(cx) / 640, float(cy) / 480)) a[0] += angle best = (None, 99999) for j in range(len(old_objects)): if j in old_found: continue u = old_objects[j] dist = math.sqrt((cx - u.position[0])**2 + (cy - u.position[1])**2) if dist < best[1]: best = (u, dist) if best[1] < 150: old_found.append(best[0]) best[0].update(contour, (cx, cy), frame[y:(y + h), x:(x + w)], a) else: print best[1] new_contours.append(i) new_objects = list(old_found) for contour in new_contours: contour = contours[contour] x, y, w, h = cv2.boundingRect(contour) cx = x + w / 2 cy = y + h / 2 a = camera.getAngularPositionFromImagePosition( 1, (float(cx) / 640, float(cy) / 480)) a[0] += angle roi = frame[y:(y + h), x:(x + w)] obj = Object(contour, (cx, cy), roi, a) objects.append(obj) new_objects.append(obj) old_objects = new_objects out_video.release() return objects
def naoMarkDetection(robotIP="127.0.0.1", port=9559): MOTION = ALProxy("ALMotion", robotIP, port) CAMERA = ALProxy("ALVideoDevice", robotIP, port) TTS = ALProxy("ALTextToSpeech", robotIP, port) LANDMARK = ALProxy("ALLandMarkDetection", robotIP, port) MEMORY = ALProxy("ALMemory", robotIP, port) #初始化机器人姿势 standWithStick(0.1, robotIP, port) #设置机器人上方摄像头开启 CAMERA.setActiveCamera(0) #加载mark识别EVENT memvalue1 = "LandmarkDetected" alpha = 0 size = 0 period = 200 LANDMARK.subscribe("Test_LandMark", period, 0.0) MOTION.angleInterpolation("HeadPitch", 0, 0.5, True) time.sleep(1.5) for i in range(0, 5): #检测正前方 MOTION.angleInterpolation("HeadYaw", 0, 0.5, True) time.sleep(2.5) val = MEMORY.getData(memvalue1) if (val and isinstance(val, list) and len(val) >= 2): TTS.say("前方我看到了") markInfo = val[1] shapeInfo = markInfo[0] rshapeInfo = shapeInfo[0] alpha = rshapeInfo[1] size = rshapeInfo[4] angleFlag = 0 break #检测左边45度 MOTION.angleInterpolation("HeadYaw", 45 * math.pi / 180.0, 0.5, True) time.sleep(2.5) val = MEMORY.getData(memvalue1) if (val and isinstance(val, list) and len(val) >= 2): TTS.say("左边我看到了") markInfo = val[1] shapeInfo = markInfo[0] rshapeInfo = shapeInfo[0] alpha = rshapeInfo[1] size = rshapeInfo[4] angleFlag = 1 break #检测右边45度 MOTION.angleInterpolation("HeadYaw", -45 * math.pi / 180.0, 0.5, True) time.sleep(2.5) val = MEMORY.getData(memvalue1) if (val and isinstance(val, list) and len(val) >= 2): TTS.say("右边我看到了") markInfo = val[1] shapeInfo = markInfo[0] rshapeInfo = shapeInfo[0] alpha = rshapeInfo[1] size = rshapeInfo[4] angleFlag = -1 break TTS.say("没有检测到标志") #如果5次都没检测到mark就返回False if (i == 4): return False if (angleFlag == 1): alpha += math.pi / 4 elif (angleFlag == -1): alpha -= math.pi / 4 else: alpha = alpha #mark的半径,在赛场上应该改为0.12 r = 0.093 / 2 #mark与机器人摄像头的高度差,场上应该改为0 h = 0.05 #将机器人的头对准mark #MOTION.angleInterpolation("HeadYaw",alpha,0.5,True) #time.sleep(1.5) #val1 = MEMORY.getData(memvalue1) #markinfo1 = val1[1] #shapeinfo1 = markinfo1[0] #rshapeinfo1 = shapeinfo1[0] k = 0.0913 #利用等比例法测出机器人到mark的距离 #根据标志成像的大小和实际大小的比例 #size = rshapeInfo[4] d1 = k / size #返回nao mark距离和角度值 print "the disMark = ", d1 print "the markAngle = ", alpha return [d1, alpha]
def force_connect(self): self.proxy = ALProxy("ALPeoplePerception")
def pictureHandle(cameraID, thresholdMin, thresholdMax, robotIP="127.0.0.1", port=9559): CAMERA = ALProxy("ALVideoDevice", robotIP, port) CAMERA.setActiveCamera(cameraID) # VGA 设置分辨率为2:640*480 0:160*120 resolution = 2 # RGB 设置颜色空间为RGB colorSpace = 11 stickAngle = 0.0 videoClient = CAMERA.subscribe("python_client", resolution, colorSpace, 5) #设置曝光度模式 CAMERA.setCamerasParameter(videoClient, 22, 2) time.sleep(0.5) #获取照片 naoImage = CAMERA.getImageRemote(videoClient) CAMERA.unsubscribe(videoClient) imageWidth = naoImage[0] imageHeight = naoImage[1] array = naoImage[6] #装换为PIL图片格式 img = Image.fromstring("RGB", (imageWidth, imageHeight), array) img.save("camImage.png", "PNG") imgTest = cv2.imread("camImage.png", 1) #RGB转换为HSV颜色空间 hsv = cv2.cvtColor(imgTest, cv2.COLOR_BGR2HSV) #获取掩膜 mask = cv2.inRange(hsv, thresholdMin, thresholdMax) #原始图像与掩膜做位运算,提取色块 res = cv2.bitwise_and(imgTest, imgTest, mask=mask) #将hsv转化为rgb color = cv2.cvtColor(res, cv2.COLOR_HSV2RGB) #转换为灰度图 gray = cv2.cvtColor(color, cv2.COLOR_RGB2GRAY) #二值化 ret, th1 = cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY) #开运算,去除噪声(开运算=先腐蚀再膨胀) #kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(3,8)) #opening = cv2.morphologyEx(th1, cv2.MORPH_OPEN, kernel) kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (2, 2)) erosion = cv2.erode(th1, kernel, iterations=1) kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (4, 8)) dilation = cv2.dilate(erosion, kernel, iterations=1) #--------------------------------测试代码-----------------------------------# #cv2.namedWindow("image",cv2.WINDOW_NORMAL) #cv2.namedWindow("image1",cv2.WINDOW_NORMAL) #cv2.imshow("image",th1) #cv2.imshow("image1",dilation) #cv2.waitKey(0) #--------------------------------测试结束-----------------------------------# #获取轮廓 contours, hierarchy = cv2.findContours(dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) return contours
def resetPopulation(self): """Empties the tracked population. """ if not self.proxy: self.proxy = ALProxy("ALPeoplePerception") return self.proxy.resetPopulation()
keys.append([[0.166571, [3, -0.255556, 0], [3, 0.344444, 0]], [0.166208, [3, -0.344444, 0], [3, 0.188889, 0]], [0.166571, [3, -0.188889, 0], [3, 0.311111, 0]], [0.166208, [3, -0.311111, 0], [3, 0.233333, 0]], [0.25, [3, -0.233333, 0], [3, 0, 0]]]) names.append("RShoulderPitch") times.append([0.166667, 0.766667, 1.8, 2.36667, 2.83333, 3.3, 4]) keys.append([[0.0767419, [3, -0.0555556, 0], [3, 0.2, 0]], [-0.59515, [3, -0.2, 0.11552], [3, 0.344444, -0.19895]], [-0.866668, [3, -0.344444, 0], [3, 0.188889, 0]], [-0.613558, [3, -0.188889, -0.253109], [3, 0.155556, 0.208443]], [0.584497, [3, -0.155556, -0.249275], [3, 0.155556, 0.249275]], [0.882091, [3, -0.155556, -0.108169], [3, 0.233333, 0.162253]], [1.39576, [3, -0.233333, 0], [3, 0, 0]]]) names.append("RShoulderRoll") times.append([0.166667, 0.766667, 1.8, 2.36667, 2.83333, 3.3, 4]) keys.append([[-0.019984, [3, -0.0555556, 0], [3, 0.2, 0]], [-0.019984, [3, -0.2, 0], [3, 0.344444, 0]], [-0.615176, [3, -0.344444, 0.175025], [3, 0.188889, -0.0959815]], [-0.833004, [3, -0.188889, 0], [3, 0.155556, 0]], [-0.224006, [3, -0.155556, -0.00920487], [3, 0.155556, 0.00920487]], [-0.214801, [3, -0.155556, 0], [3, 0.233333, 0]], [-0.297251, [3, -0.233333, 0], [3, 0, 0]]]) names.append("RWristYaw") times.append([0.766667, 1.8, 2.36667, 3.3, 4]) keys.append([[-0.058334, [3, -0.255556, 0], [3, 0.344444, 0]], [-0.0521979, [3, -0.344444, 0], [3, 0.188889, 0]], [-0.067538, [3, -0.188889, 0], [3, 0.311111, 0]], [-0.038392, [3, -0.311111, -0.0121571], [3, 0.233333, 0.00911784]], [-0.00371307, [3, -0.233333, 0], [3, 0, 0]]]) try: motion = ALProxy("ALMotion",robotIP,port) motion.angleInterpolationBezier(names, times, keys) except BaseException, err: print err if __name__ == "__main__": robotIP = "127.0.0.1" port = 9559 if len(sys.argv) <= 1: print "(robotIP default: 127.0.0.1)" elif len(sys.argv) <= 2: robotIP = sys.argv[1] else: port = int(sys.argv[2])
def startPosture(ip, port=9559): posture = ALProxy("ALRobotPosture", ip, port) posture.goToPosture("StandInit", 0.5)