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)
