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 获取文件列表(或是目录例表)
Mar 25 Python
压缩包密码破解示例分享(类似典破解)
Jan 17 Python
python简单文本处理的方法
Jul 10 Python
Python实现KNN邻近算法
Jan 28 Python
基于pycharm导入模块显示不存在的解决方法
Oct 13 Python
Python3爬虫爬取百姓网列表并保存为json功能示例【基于request、lxml和json模块】
Dec 05 Python
python opencv 批量改变图片的尺寸大小的方法
Jun 28 Python
Python分割训练集和测试集的方法示例
Sep 19 Python
python函数装饰器之带参数的函数和带参数的装饰器用法示例
Nov 06 Python
python如何实现不可变字典inmutabledict
Jan 08 Python
Python制作数据预测集成工具(值得收藏)
Aug 21 Python
python遍历路径破解表单的示例
Nov 21 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
Smarty结合Ajax实现无刷新留言本实例
2007/01/02 PHP
解析php中session的实现原理以及大网站应用应注意的问题
2013/06/17 PHP
关于js和php对url编码的处理方法
2014/03/04 PHP
PHP间隔一段时间执行代码的方法
2014/12/02 PHP
thinkphp3.2实现上传图片的控制器方法
2016/04/28 PHP
PHP基于Closure类创建匿名函数的方法详解
2017/08/17 PHP
Linux基于php-fpm模式的lamp搭建phpmyadmin的方法
2018/10/25 PHP
jQuery 位置函数offset,innerWidth,innerHeight,outerWidth,outerHeight,scrollTop,scrollLeft
2010/03/23 Javascript
JavaScript中圆括号()和方括号[]的特殊用法疑问解答
2013/08/06 Javascript
原生JS实现加入收藏夹的代码
2013/10/24 Javascript
深入剖析JavaScript中的枚举功能
2014/03/06 Javascript
jQuery实现首页顶部可伸缩广告特效代码
2015/04/15 Javascript
javascript实现省市区三级联动下拉框菜单
2015/11/17 Javascript
深入理解JavaScript中为什么string可以拥有方法
2016/05/24 Javascript
AngularJS 如何在控制台进行错误调试
2016/06/07 Javascript
微信小程序多张图片上传功能
2017/06/07 Javascript
es6学习之解构时应该注意的点
2017/08/29 Javascript
MUI 实现侧滑菜单及其主体部分上下滑动的方法
2018/01/25 Javascript
python字符串对其居中显示的方法
2015/07/11 Python
python 打印直角三角形,等边三角形,菱形,正方形的代码
2017/11/21 Python
python opencv 直方图反向投影的方法
2018/02/24 Python
python3.6的venv模块使用详解
2018/08/01 Python
python实现屏保计时器的示例代码
2018/08/08 Python
Tornado实现多进程/多线程的HTTP服务详解
2019/07/25 Python
django中使用事务及接入支付宝支付功能
2019/09/15 Python
Python-jenkins模块之folder相关操作介绍
2020/05/12 Python
Python约瑟夫生者死者小游戏实例讲解
2021/01/04 Python
自我鉴定写作要点
2014/01/17 职场文书
县委班子四风对照检查材料思想汇报
2014/09/29 职场文书
乡镇党建工作汇报材料
2014/10/27 职场文书
2014年前台文员工作总结
2014/12/08 职场文书
党员活动总结
2015/02/04 职场文书
2015年女生节活动总结
2015/02/27 职场文书
护理专业自荐信范文
2015/03/06 职场文书
SpringBoot整合Mybatis Generator自动生成代码
2021/08/23 Java/Android
使用 Docker Compose 构建复杂的多容器App
2022/04/30 Servers