def turn90andFindtheHole(robotIP, PORT, distance): """ 该函数将绕着给定的距离进行90度绕圈找洞,直到找到洞返回 :param robotIP: IP :param PORT: 9559 :param distance: 距离值,一般由turnHeadandGetDistance()函数得到 :return: True表示动作执行结束 """ motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] hole = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 0), 'hole', 0)[0][1] print hole if hole != [0, 0]: tts.say('do not need to move') return True motionProxy.moveTo(distance, -distance, pi / 2, smallTurnStep) tts.say('finish') hole = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 0), 'hole', 0)[0][1] while hole == [0, 0]: motionProxy.moveTo(distance, -distance, pi / 2, smallTurnStep) hole = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 0), 'hole', 0)[0][1] return True
def turnHeadandGetDistance(robotIP, PORT=9559): """ 为了减少误差,转动头部使目标位于视野中心,然后调用函数求出距离 :param robotIP: 机器人IP :param PORT: 9559 :return: 返回distance的数组,为【x直线距离,y直线距离,斜边距离】 """ motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) rotation1 = motionProxy.getAngles('HeadYaw', True) rotation2 = motionProxy.getAngles('HeadPitch', True) print rotation1, rotation2 img = getImagfromvedio(robotIP, PORT, 1) # 获取旋转所需角度 anglelist = getangle(ImagProgressHSV(img, 'ball', 1)[0][0]) rot1 = str(rotation1) rot2 = str(rotation2) rot1 = rot1[1:10] rot2 = rot2[1:10] anglelist[0] = float(anglelist[0]) + float(rot1) anglelist[1] = float(anglelist[1]) + float(rot2) alpha = float(anglelist[0]) beta = float(anglelist[1]) print alpha, beta # 转动头部使目标位于视野中央 motionProxy.setStiffnesses("Head", 1.0) names = "HeadYaw" angleLists = alpha timeLists = 1.0 isAbsolute = True motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute) print 'Yaw finish' tts.say('Yaw finish') names = "HeadPitch" angleLists = beta motionProxy.angleInterpolation(names, angleLists, timeLists, isAbsolute) print 'Pitch finish' tts.say('Pitch finish') # 得到目前头部角度进行距离测算 time.sleep(1.0) img = getImagfromvedio(robotIP, PORT, 1) cv.imshow('img', img) data = ImagProgressHSV(img, 'ball', 1)[0][0] distance = getdistancefromcam(robotIP, PORT, data) # theta = motionProxy.getAngles('HeadPitch', True) # distance = getdistance(theta) # distance = distance.real # cv.imshow('rs',img) print distance # time.sleep(4.0) # motionProxy.rest() # cv.waitKey(0) return distance
def walkuntil(robotIP, PORT): y = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[0][0][0] print y motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] # 单步移动 while y < 180: motionProxy.moveTo(0.017, 0, 0, smallTurnStep) cv2.waitKey(1) return walkuntil(robotIP, PORT) tts.say('walk until finish') return True
def walkToballv2(robotIP, PORT): motionProxy = ALProxy("ALMotion", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] # 单步移动 data = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[0] ball = data[0] hole = data[1] if 140 < ball < 180: motionProxy.moveTo(0, 0, 0, smallTurnStep) if ball[0] < hole[0]: while not (140 < ball < 180): motionProxy.moveTo(0, 0, pi / 6, smallTurnStep) elif ball[0] > hole[0]: while not (140 < ball < 180): motionProxy.moveTo(0, 0, -pi / 6, smallTurnStep)
def walkToBall(robotIP, PORT): """ 该函数由球心在图片中的x位置决定是否进行旋转直到球位于视野中心,然后判断球的y坐标的位置,走到相应的距离,执行找洞函数 :param robotIP: IP :param PORT: 9559 :return: True """ motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] while True: ball = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[0][0] motionProxy.moveTo(0, 0, pi / 12, smallTurnStep) if 140 < ball[0] < 180: break elif 140 < ball[0]: motionProxy.moveTo(0, 0, pi / 12, smallTurnStep) else: motionProxy.moveTo(0, 0, -pi / 12, smallTurnStep) y = ball[1] # TODO 此处的24个距离值需要具体给定 distance = [ 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05 ] y = y // 10 flag = -(float(ball[0]) - 160) / abs(float(ball[0]) - 160) motionProxy.moveTo(distance[y], 0, 0, smallTurnStep) # distance = turnHeadandGetDistance(robotIP, PORT)[2] # rotationYaw = motionProxy.getAngles('HeadYaw', True) # distance = distance / 100 # rotationYaw = float(str(rotationYaw)[1:10]) # rotationYaw = rotationYaw * almath.TO_RAD # # motionProxy.moveTo(0, 0, rotationYaw, smallTurnStep) # postureProxy.goToPosture("StandInit", 0.5) # # if not 80 < ImagProgressHSV(getImag(robotIP, PORT, 0, str(random.randint(1, 1000))), 'ball', 0)[0][0] < 240: # # rotationYaw = motionProxy.getAngles('HeadYaw', True) # # motionProxy.moveTo(0, 0, rotationYaw, smallTurnStep) # motionProxy.moveTo(distance, 0, 0, smallTurnStep) turn90andFindtheHole(robotIP, PORT, 0.2) tts.say('walk to ball finish') return True
def areajudgement(robotIP, PORT): """ 该函数将进行判断处理过的二值图中黑色部分的多少,从而向前微调和向后微调 :param robotIP: IP :param PORT: 9559 :return: True表示动作完成,可以进行下一步击球 """ while True: result = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[1] count = 0 # postureProxy.goToPosture("StandInit", 0.5) for row in range(240): for col in range(320): pv = result[row, col] if pv == 255: count += 1 print count return count
def searchForBall(robotIP, PORT): """ 此函数在流程开始前进行找球,采用原地转圈的形式,一旦找到球,停止动作 :param robotIP: IP :param PORT: 9559 :return: True,在找到球的时候返回 """ motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) tts.say('search for ball') smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] # TODO 此处是否需要改为视频流需要依据实际效果进行调整 try: data = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 0)[0][0] tts.say('find the ball') return True except TypeError: tts.say('can not find the ball') motionProxy.moveTo(0, 0, math.pi / 3, smallTurnStep) return searchForBall(robotIP, PORT)
camera_pitch = head_pitch + cameraDirection y = float(data[1]) x = float(data[0]) img_pitch = (y - 120) / 240 * 47.64 / np.pi img_yaw = (160 - x) / 320 * 60.97 / np.pi ball_pitch = camera_pitch + img_pitch ball_yaw = img_yaw + head_yaw print("ball yaw = ", ball_yaw / np.pi * 180) dis_x = (cameraHeight - 30) / np.tan(ball_pitch) + np.sqrt(cameraX**2 + cameraY**2) dis = dis_x dis_y = dis_x * np.sin(ball_yaw) dis_x = dis_x * np.cos(ball_yaw) distance = [abs(dis_x), abs(dis_y), abs(dis)] return distance if __name__ == '__main__': robotIP = '169.254.202.17' PORT = 9559 motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] # 单步移动 motionProxy.wakeUp() postureProxy.goToPosture("StandInit", 0.5) time.sleep(1) data = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[0][0] print getdistancefromcam(robotIP, PORT, data) cv.waitKey(0)
def mircoadjust(robotIP, PORT): """ 该函数为方向微调函数,在找到球和洞之后进行左右平移直到球和洞处于同一条直线上并且处于视野的中心线上 :param robotIP: IP :param PORT: 9559 :return: True表示动作以完成,可以进行前后微调 """ motionProxy = ALProxy("ALMotion", robotIP, PORT) tts = ALProxy("ALTextToSpeech", robotIP, PORT) data = ImagProgressHSV(getImagfromvedio(robotIP, PORT, 1), 'ball', 1)[0] # imag = getImag(robotIP, PORT, 0, str(random.randint(1, 1000))) ball = data[0] hole = data[1] legName = ["LLeg", "RLeg"] X = 0 Y = 0.05 Theta = 0 footSteps = [[X, Y, Theta]] fractionMaxSpeed = [0.5] clearExisting = False print abs(float(ball[0]) - float(hole[0])) print (abs(float(ball[0]) - 160) < 5) xdistance = (abs(float(ball[0]) - float(hole[0])) <= 5) and (abs(float(ball[0]) - 160) <= 5) smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]] if xdistance: # 球和洞在中心直线上 tts.say('micro judge finish') cv.waitKey(1) return True if abs(float(ball[0]) - float(hole[0])) >= 100: # 球和洞在一条直线上,但是不在中心 if float(ball[0]) < 160: tts.say('max value') tts.say('walk to left') motionProxy.moveTo(0, 0.3, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif float(ball[0]) >= 160: tts.say('max value') tts.say('walk to right') motionProxy.moveTo(0, -0.3, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif abs(float(ball[0]) - float(hole[0])) >= 50: # 球和洞不在同一条直线上 if float(ball[0]) < 160: tts.say('second max value') tts.say('walk to left') motionProxy.moveTo(0, 0.1, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif float(ball[0]) >= 160: tts.say('second max value') tts.say('walk to right') motionProxy.moveTo(0, -0.1, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif abs(float(ball[0]) - float(hole[0])) >= 10: if float(ball[0]) < 160: tts.say('min value') tts.say('walk to left') motionProxy.moveTo(0, 0.05, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif float(ball[0]) >= 160: tts.say('min value') tts.say('walk to right') motionProxy.moveTo(0, -0.05, 0, smallTurnStep) cv.waitKey(1) return mircoadjust(robotIP, PORT) elif abs(float(ball[0]) - float(hole[0])) < 5: tts.say('micro judge finish') return True
cv.rectangle(Image, (x, y), (x + w, y + h), (0, 255, 0), 2) result = Image datahole = [x + w / 2, y + h] except TypeError: print 'NULL NULL NULL NULL NULL NULL NULL' # print data cv.imshow('test', result) try: data = [databall, datahole] return data, MedirImagball except UnboundLocalError: print 'NULL NULL NULL NULL NULL NULL NULL' return [[0, 0], [0, 0]], MedirImagball if __name__ == "__main__": robotIP = '169.254.252.60' PORT = 9559 motionProxy = ALProxy("ALMotion", robotIP, PORT) postureProxy = ALProxy("ALRobotPosture", robotIP, PORT) motionProxy.wakeUp() postureProxy.goToPosture("StandInit", 0.5) name = str(random.randint(1, 1000)) while True: try: print ImagProgressHSV(getImagfromvedio(robotIP, 9559, 1), 'ball', 1)[0][0] except TypeError: print 'wrong' cv.waitKey(1)