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数据结构树和二叉树简介
Apr 29 Python
python利用拉链法实现字典方法示例
Mar 25 Python
Python生成8位随机字符串的方法分析
Dec 05 Python
一道python走迷宫算法题
Jan 22 Python
python 获取键盘输入,同时有超时的功能示例
Nov 13 Python
浅谈Python中函数的定义及其调用方法
Jul 19 Python
win10安装tesserocr配置 Python使用tesserocr识别字母数字验证码
Jan 16 Python
python爬虫开发之使用Python爬虫库requests多线程抓取猫眼电影TOP100实例
Mar 10 Python
python多进程使用函数封装实例
May 02 Python
详解Python IO编程
Jul 24 Python
Python读写压缩文件的方法
Jul 30 Python
Django数据统计功能count()的使用
Nov 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
多文件上载系统完整版
2006/10/09 PHP
PHP 已经成熟
2006/12/04 PHP
php 友好URL的实现(吐血推荐)
2008/10/04 PHP
无JS,完全php面向过程数据分页实现代码
2012/08/27 PHP
PHP开启opcache提升代码性能
2015/04/26 PHP
基于PHP后台的Android新闻浏览客户端
2016/05/23 PHP
PHP使用Redis替代文件存储Session的方法
2017/02/15 PHP
php array 转json及java 转换 json数据格式操作示例
2019/11/13 PHP
Array.prototype.concat不是通用方法反驳[译]
2012/09/20 Javascript
js导出格式化的excel 实例方法
2013/07/17 Javascript
Node.js中的模块机制学习笔记
2014/11/04 Javascript
jQuery+ajax实现动态执行脚本的方法
2015/01/27 Javascript
浅谈EasyUI中编辑treegrid的方法
2015/03/01 Javascript
javascript清空table表格的方法
2015/05/14 Javascript
原生js获取元素样式的简单方法
2016/08/06 Javascript
vue-router:嵌套路由的使用方法
2017/02/21 Javascript
微信小程序项目实践之九宫格实现及item跳转功能
2018/07/19 Javascript
ajaxfileupload.js实现上传文件功能
2019/04/19 Javascript
vue中使用[provide/inject]实现页面reload的方法
2019/09/30 Javascript
javascript实现简易数码时钟
2020/03/30 Javascript
Python 流程控制实例代码
2009/09/25 Python
使用Python发送邮件附件以定时备份MySQL的教程
2015/04/25 Python
python Socket之客户端和服务端握手详解
2017/09/18 Python
python用户管理系统的实例讲解
2017/12/23 Python
python-opencv颜色提取分割方法
2018/12/08 Python
Django Rest framework之认证的实现代码
2018/12/17 Python
Python matplotlib绘制饼状图功能示例
2019/09/10 Python
CSS3 网页下拉菜单代码解释 中文翻译
2010/02/27 HTML / CSS
利用HTML5中的Canvas绘制一张笑脸的教程
2015/05/07 HTML / CSS
我有一个char * 型指针正巧指向一些int 型变量, 我想跳过它们。 为什么如下的代码((int *)p)++; 不行?
2013/05/09 面试题
商务日语毕业生自荐信范文
2013/11/14 职场文书
2014端午节活动策划方案
2014/01/27 职场文书
房屋租赁委托书范本
2014/10/04 职场文书
2014年扫黄打非工作总结
2014/12/03 职场文书
2016新年感言
2015/08/03 职场文书
springcloud之Feign超时问题的解决
2021/06/24 Java/Android