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


Posted in Python onSeptember 04, 2021

 本文实例为大家分享了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)

以上就是本文的全部内容,希望对大家的学习有所帮助,也希望大家多多支持三水点靠木。

Python 相关文章推荐
Python 解析XML文件
Apr 15 Python
跟老齐学Python之模块的加载
Oct 24 Python
Python中urllib2模块的8个使用细节分享
Jan 01 Python
python和bash统计CPU利用率的方法
Jul 10 Python
Python快速排序算法实例分析
Nov 29 Python
使用Python从零开始撸一个区块链
Mar 14 Python
Python输出\u编码将其转换成中文的实例
Dec 15 Python
对python修改xml文件的节点值方法详解
Dec 24 Python
Python 多维List创建的问题小结
Jan 18 Python
python 读取文件并把矩阵转成numpy的两种方法
Feb 12 Python
pandas读取csv文件提示不存在的解决方法及原因分析
Apr 21 Python
基于tensorflow for循环 while循环案例
Jun 30 Python
python读取mnist数据集方法案例详解
Sep 04 #Python
Pyqt5将多个类组合在一个界面显示的完整示例
Sep 04 #Python
一小时学会TensorFlow2之基本操作2实例代码
Python torch.flatten()函数案例详解
Aug 30 #Python
Python之基础函数案例详解
Aug 30 #Python
python中使用 unittest.TestCase单元测试的用例详解
Aug 30 #Python
python使用matplotlib绘制图片时x轴的刻度处理
You might like
使用 php4 加速 web 传输
2006/10/09 PHP
PHP引用(&amp;)各种使用方法实例详解
2014/03/20 PHP
PHP+Mysql实现多关键字与多字段生成SQL语句的函数
2014/11/05 PHP
[原创]php实现数组按拼音顺序排序的方法
2017/05/03 PHP
PHP基于redis计数器类定义与用法示例
2018/02/08 PHP
JS OOP包机制,类创建的方法定义
2009/11/02 Javascript
javascript new一个对象的实质
2010/01/07 Javascript
js使下拉列表框可编辑不止是选择
2013/12/12 Javascript
NodeJS Web应用监听sock文件实例
2015/02/18 NodeJs
使用Meteor配合Node.js编写实时聊天应用的范例
2015/06/23 Javascript
JavaScript实现动态添加,删除行的方法实例详解
2015/07/02 Javascript
关于获取DIV内部内容报错的原因分析及解决办法
2016/01/29 Javascript
快速掌握Node.js之Window下配置NodeJs环境
2016/03/21 NodeJs
js中获取 table节点各tr及td的内容简单实例
2016/10/14 Javascript
js和jquery中获取非行间样式
2017/05/05 jQuery
React 路由懒加载的几种实现方案
2018/10/23 Javascript
Vue将页面导出为图片或者PDF
2020/08/17 Javascript
详解vue为什么要求组件模板只能有一个根元素
2019/07/22 Javascript
[00:15]TI9观赛名额抽取
2019/07/10 DOTA
[05:06]DOTA2-DPC中国联赛 正赛 VG vs Magma选手采访
2021/03/11 DOTA
Flask SQLAlchemy一对一,一对多的使用方法实践
2013/02/10 Python
Python中的pprint折腾记
2015/01/21 Python
利用pyinstaller将py文件打包为exe的方法
2018/05/14 Python
python利用ffmpeg进行录制屏幕的方法
2019/01/10 Python
Python BeautifulSoup [解决方法] TypeError: list indices must be integers or slices, not str
2019/08/07 Python
python 队列基本定义与使用方法【初始化、赋值、判断等】
2019/10/24 Python
python 实现人和电脑猜拳的示例代码
2020/03/02 Python
python实现计算图形面积
2021/02/22 Python
英国婚礼商城:Wedding Mall
2019/11/02 全球购物
医生个人自我剖析材料
2014/10/08 职场文书
违反工作规定检讨书范文
2014/12/14 职场文书
担保贷款承诺书
2015/04/30 职场文书
毕业论文答辩开场白和结束语
2015/05/27 职场文书
导游词之平津战役纪念馆
2019/11/04 职场文书
Python中的tkinter库简单案例详解
2022/01/22 Python
nginx lua 操作 mysql
2022/05/15 Servers