自学教程：python实现Nao机器人的单目测距

#!/usr/bin/python2.7#-*- encoding: UTF-8 -*-import vision_definitions#----------------------单目测距--------------------------------#***********************************************#@函数名：   DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)#@参数：     (cxnum,rynum)是通过图像识别得到球心的像素点坐标#           (colsum,rowsum)是图片总大小：640*480#            cameraID=0，取上摄像头；cameraID=1，取下摄像头#@返回值：   无#@功能说明： 采用机器人的下摄像头进行测量球离机器人的相关角度与距离def DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID):    distx=-(cxnum-colsum/2)    disty=rynum-rowsu/2    print distx,disty    Picture_angle=disty*47.64/480    if cameraID ==0:        h=0.62        Camera_angle=12    else:        h=0.57        Camera_angle=38    #Head_angle[0]机器人仰俯角度    Total_angle=math.pi*(Picture_angle+Camera_angle)/180+Head_angle[0]    d1=h/math.tan(Total_angle)    alpha=math.pi*(distx*60.92/640)/180    d2=d1/math.cos(alpha)    #Head_angle[1]机器人左右角度    Forward_Distance=d2*math.cos(alpha+Head_angle[1])    Sideward_Distance=-d2*math.sin(alpha+Head_angle[1])#***********************************************#@函数名：   GetNaoImage(IP,PORT,cameraID)#@参数：     略#@返回值：   无#@功能说明： 采调用机器人内置摄像头控制模块，对当前场景进行拍摄并保持。#           由于球距离机器人约小于0.6m时，机器人额头摄像头无法看到，#           所以需要变换摄像头，cameraID=0，取上摄像头；#           cameraID=1，取下摄像头def Get NaoImage(IP,PORT,cameraID):    camProxy=ALProxy("ALVideoDevice",IP,PORT)    resolition=2 #VGA格式640*480    colorSpace=11#RGB    #选择并启用摄像头    camProxy.setParam(vision_definitions.kCameraSelectID,cameraID)    videoClient=camProxy.subscribe("python_client",resolition,colorSpace,5)    #获取摄像机图像。    #image [6]包含以ASCII字符数组形式传递的图像数据。    naoImage=camProxy.getImageRemote(videoClient)    camProxy.unsubscribe(videoClient)    #获取图像大小和像素阵列。    imageWidth=naoImage[0]    imageHeight=naoImage[1]    array=naoImage[6]    #从我们的像素阵列创建一个PIL图像。    im=Image.fromstring("RGB",(imageWidth,imageHeight),array)    #保存图像。    im.save("temp.jpg","JPEG")#***********************************************#@函数名：   findColorPattern(img,pattern)#@参数：     略#@返回值：   无#@功能说明：  将RGB图像转化为二值图：此方法用的是cv,可以尝试用cv2代码会更加简洁def  findColorPattern(img,pattern):    channels=[None,None,None]    channels[0]=cv.CreateImage(cv.GetSize(img),8,1)    channels[1]=cv.CreateImage(cv.GetSize(img),8,1)    channels[2]=cv.CreateImage(cv.GetSize(img),8,1)    ch0=cv.CreateImage(cv.GetSize(img),8,1)    ch1=cv.CreateImage(cv.GetSize(img),8,1)    ch2=cv.CreateImage(cv.GetSize(img),8,1)    cv.Split(img,ch0,ch1,ch2,None)    dest=[None,None,None,None]    dest[0]=cv.CreateImage(cv.GetSize(img),8,1)    dest[1]=cv.CreateImage(cv.GetSize(img),8,1)    dest[2]=cv.CreateImage(cv.GetSize(img),8,1)    dest[3]=cv.CreateImage(cv.GetSize(img),8,1)    cv.Smooth(ch0,channels[0],cv.CV_GAUSSIAN,3,3,0)    cv.Smooth(ch1,channels[1],cv.CV_GAUSSIAN,3,3,0)    cv.Smooth(ch2,channels[2],cv.CV_GAUSSIAN,3,3,0)    for i in range(3):        k=2-i        lower=pattern[k]-75#设置阈值        upper=pattern[k]+75        cv.InRangeS(channels[i],lower,upper,dest[i])    cv.And(dest[0],dest[1],dest[3])    temp=cv.CreateImage(cv.GetSize(img),8,1)    cv.And(dest[2],dest[3],temp)    '''    cv.NameWindow("result",cv.CV_WINDOW_AUTOSIZE)    cv.ShowImage("result",temp)    cv.WaitKey(0)    '''    return temp#***********************************************#@函数名：   xyProject(matrix,imgaesize)#@参数：     matrix#           imgaesize#@返回值：   无#@功能说明： 利用二值图，计算球的像素坐标。其原理是：遍历各行各列#           像素的数值的和，最大的组合即为球心坐标def xyProject(matrix,imagesize):    #声明一个数据类型为8位型单通道的imagessize[1]*1/1*imagessize[0]矩阵(初始值为 0)。    colmask=cv.CreateMat(imagessize[1],1,cv.CV_8UC1)    rowmask=cv.CreateMat(1,imagessize[0],cv.CV_8UC1)    cv.Set(colmask,1)    cv.Set(rowmask,1)    colsum=[]    for i in range(imagesize[0]):        col=cv.GetCol(matrix,i)        #计算向量点积        a=cv.DotProduct(colmask,col)        colsum.append(a)    rowsum=[]    for i in range(imagesize[1]):        row=cv.GetRow(matrix,i)        a=cv.DotProduct(rowmask,row)        rowsum.append(a)    return(colsum,rowsum)#得到各行各列“1”值的和def crMax(colsum,rowsum):    cx=max(colsum)    ry=max(rowsum)    for i in range(len(colsum)):        if colsum[i]==cx:            cxnum=i    for i in range(len(rowsum)):        if rowsum[i]==ry:            rynum=i    return(cxnum,rynum)#***********************************************#@函数名：  GetHeadAngles(robotIP,PORT)#@参数：    略#@返回值：   无#@功能说明：def GetHeadAngles(robotIP,PORT):    motionProxy=ALProxy("ALMotion",robotIP,PORT)    names=["HeadPitch","HeadYaw"]    useSensors=1    sensorAngles=motionProxy.getAngles(names,useSensors)    return sensorAngles#***********************************************#@函数名：  SetHeadAngles(robotIP,PORT,angles)#@参数：    略#@返回值：   无#@功能说明：def SetHeadAngles(robotIP,PORT,angles):    motionProxy=ALProxy("ALMotion",robotIP,PORT)    motionProxy.setStiffnesses("Head",1.0)    names=["HeadPitch","HeadYaw"]    fractionMaxSpeed=0.2    motionProxy.setAngles(names,angles,fractionMaxSpeed)    time.sleep(2.0)    motionProxy.setStiffnesses("Head",0.0)#***********************************************#@函数名：   Capture_Picture(IP,PORT,cameraID,angles,pattern_colors)#@参数：     angles#           pattern_colors#@返回值：   无#@功能说明： 将上面的一系列函数整合起来def Capture_Picture(IP,PORT,cameraID,angles,pattern_colors):    SetHeadAngles(IP,PORT,angles)    GetNaoImage(IP,PORT,cameraID)    image=cv.LoadImage("temp.jpg")    imagesize=cv.GetSize(image) #返回数值，两个元素分别为列数和行数    matrix=findColorPattern(image,pattern_colors)    (colsum,rowsum)=xyProject(matrix,imagesize)    (cxnum,rynum)=crMax(colsum,rowsum)    cv.SaveImage("result.jpg",matrix)    return (cxnum,rynum,colsum,rowsum) #***********************************************#@函数名：   Target_Detect_and_Distance(IP,PORT)#@参数：#@返回值：   无#@功能说明： 当上摄像头无法找到球时，切换到下摄像头，然后在左转右转。#       在这个过程中，如果发现目标，则计算距离并输出距离#       若始终未找到目标，则输出距离为0。def Target_Detect_and_Distance(IP,PORT):    pattern_colors=(255,150,50)    cameraID=0# 默认上摄像头    angles=[0,0]    (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)    if(cxnum,rynum)==(639,479):        cameraID=1        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)    if(cxnum,rynum)==(639,479):        cameraID=0        angles=[0.0.7]        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)    if(cxnum,rynum)==(639,479):        cameraID=0        angles=[0,-0.7]        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)    HeadAngles-GetHeadAngles(IP,PORT)    ###############    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)    if(cxnum,rynum)==(639,479):        (Forward_Distance,Sideward_Distance)=(0,0)    print "Forward_Distance=",Forward_Distance,"meters"    print "Sideward_Distance="+Sideward_Distance+"meters"#***********************************************#@函数名：   Target_Detect_and_Distance(IP,PORT)#@参数：#@返回值：   无#@功能说明： 当找到球后，可能会存在一定的误差。#           因此需要判断球位于机器人前方的哪一侧，再来确定用哪只脚踢球def Final_See(robotIP,PORT):    pattern_colors=(255,150,50)    angles=[0.5,0]    SetHeadAngles(robotIP,PORT,angles)    cameraID=1    GetNaoImage(robotIP,PORT,cameraID)    image=cv.LoadImage("temp.jpg")    imagesize=cv.GetNaoImage(image)    matrix=findColorPattern(image,pattern_colors)    (colsum,rowsum)=xyProject(matrix,imgaesize)    (cxnum,rynum)=crMax(colsum,rowsum)    cv.SaveImage("result.jpg",matrix)    HeadAngles=GetHeadAngles(robotIP,PORT)    #########################    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)    if cxnum<len(colsum)/2:        side=0#左脚    else:        side=1#右脚    print "side=",side    print "last distance=",Forward_Distance    return (side,Forward_Distance)