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将html转成PDF的实现代码(包含中文)
Mar 04 Python
Python Queue模块详细介绍及实例
Dec 27 Python
Pycharm学习教程(3) 代码运行调试
May 03 Python
python opencv设置摄像头分辨率以及各个参数的方法
Apr 02 Python
Python入门学习指南分享
Apr 11 Python
基于Python实现定时自动给微信好友发送天气预报
Oct 25 Python
python pcm音频添加头转成Wav格式文件的方法
Jan 09 Python
对Python生成汉字字库文字,以及转换为文字图片的实例详解
Jan 29 Python
python实现BP神经网络回归预测模型
Aug 09 Python
Python sep参数使用方法详解
Feb 12 Python
python编程进阶之类和对象用法实例分析
Feb 21 Python
python实现简易版学生成绩管理系统
Jun 22 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
Thinkphp实现站点静态化的方法详解
2017/03/21 PHP
PHP常用字符串函数小结(推荐)
2018/08/05 PHP
JQuery 图片延迟加载并等比缩放插件
2009/11/09 Javascript
dwz 如何去掉ajaxloading具体代码
2013/05/22 Javascript
理解Javascript闭包
2013/11/01 Javascript
JS完整获取IE浏览器信息包括类型、版本、语言等等
2014/05/22 Javascript
JS基于cookie实现来宾统计记录访客信息的方法
2015/08/04 Javascript
七夕情人节丘比特射箭小游戏
2015/08/20 Javascript
JS实现关闭当前页而不弹出提示框的方法
2016/06/22 Javascript
详解Vue的computed(计算属性)使用实例之TodoList
2017/08/07 Javascript
vue 组件中使用 transition 和 transition-group实现过渡动画
2019/07/09 Javascript
[53:10]Secret vs Pain 2018国际邀请赛小组赛BO2 第一场 8.17
2018/08/20 DOTA
Python计算程序运行时间的方法
2014/12/13 Python
Python基于scrapy采集数据时使用代理服务器的方法
2015/04/16 Python
python实现发送邮件功能
2017/07/22 Python
Python中pandas dataframe删除一行或一列:drop函数详解
2018/07/03 Python
在Python中Dataframe通过print输出多行时显示省略号的实例
2018/12/22 Python
利用Pyhton中的requests包进行网页访问测试的方法
2018/12/26 Python
Django 用户认证组件使用详解
2019/07/23 Python
python numpy存取文件的方式
2020/04/01 Python
Windows下PyCharm2018.3.2 安装教程(图文详解)
2019/10/24 Python
Python定义一个Actor任务
2020/07/29 Python
CSS3教程:边框属性border的极致应用
2009/04/02 HTML / CSS
CSS3区域模块region相关编写示例
2015/08/28 HTML / CSS
override和overload的区别
2016/03/09 面试题
小加工厂管理制度
2014/01/21 职场文书
业绩考核岗位职责
2014/02/01 职场文书
国旗下的演讲稿
2014/05/08 职场文书
工作经常出错的检讨书
2014/09/13 职场文书
党员教师学习党的群众路线教育实践活动心得体会
2014/10/31 职场文书
2014收银员工作总结范文
2014/12/16 职场文书
民间借贷借条如何写
2015/05/26 职场文书
黑白记忆观后感
2015/06/18 职场文书
2016新年感言
2015/08/03 职场文书
Springboot如何使用logback实现多环境配置?
2021/06/16 Java/Android
Python基于百度API识别并提取图片中文字
2021/06/27 Python