python实现Nao机器人的单目测距

 本文实例为大家分享了python实现nao机器人单目测距的具体代码，供大家参考，具体内容如下

此代码适于用做对nao机器人做视觉识别和测距实验,只提供关键代码部分,尝试利用cv2去优化代码会更加简洁哟！

此代码的主要功能：

1.初始姿态下，通过更换摄像头和转头去寻找目标
2.通过颜色阈值识别目标，计算目标与nao的距离和角度

可以扩展功能：

1.在运动过程中对方向和距离进行多次测量和校正，提高准确度
2.找到目标后，通过对目标的测量，选择使用哪个脚去踢目标

#!/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)

以上就是本文的全部内容，希望对大家的学习有所帮助，也希望大家多多支持。