python实现Nao机器人的单目测距
作者:小禅Chaika 发布时间:2021-04-09 16:37:10
标签:python,机器人,测距
本文实例为大家分享了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)
来源:https://blog.csdn.net/qazwsx0316/article/details/105164732
0
投稿
猜你喜欢
- 栅格系统的形成1692年,新登基的法国国王路易十四感到法国的印刷水平强差人意,因此命令成立一个管理印刷的皇家特别委员会。他们的首要任务是设计
- 简单使用最开始,我们用最短的代码体验一下logging的基本功能。import logginglogger = logging.getLog
- 如何用javascript来判定选择单选的radio哪个被选择,多选的checkbox有那几个被选中? <!DOCTYPE
- login.html <script language = "javascript" type = "t
- 问题:关于如何生成随机记录(二)如何从指定表中随机抽取一定量的记录?sql server 中 select top 10 * fr
- PHP页面中如果不希望出现以下情况: 单引号被转义为 \' 双引号被转义为 \" 那么可以进行如下设置以防止: 方法一:在
- 基本的网站页面设计元素布局比例统计,给大家做个参考,看看您的网站是否和下面的统计一致:标志图案:位置统计结果左上角84%右上角6%上方居中6
- python对Ref文档进行去重首先将txt文档提取到Excel表格中筛选出重复项,并且整理到txt中:需要去重的目标txt也准备好:接下来
- 所有文件都包含在各个不同的目录下,不过Python也能轻松处理。os模块有许多方法能帮你创建,删除和更改目录。mkdir()方法可以使用os
- PHP count_chars() 函数实例返回一个字符串,包含所有在 "Hello World!" 中使用过的不同字符
- 最近在学python的过程中无意间发现一个python库:wxpy,其可以实现让微信自动接收、处理消息并进行回复的一系列功能。感觉挺有意思的
- 用python做一个简单的随机点名程序(不重复点名)这是我来到csdn的第一篇文章,内容如果有瑕疵的地方或者代码可以进一步改善,请大家对我指
- 问题描述现有一个有向无权图。如下图所示: 问题:使用某个顶点s作为输入参数,找出从s到所有其他顶点的最短路径。 说明:因为是无权
- 如果进入了shell交互窗口,python的退出方式只能够用函数不能够用命令,这有时候让人感觉到很不习惯。因为函数会比命令多一个括号的输入,
- 昨天面试上来就是一个算法,平时基本的算法还行,结果变个法就不会了。。。感觉应该刷一波Leecode冷静下。。。今天抽空看下。题目就是要求O(
- 网上看到的python去掉字符串中的标点符号的方法,大多是基于python2的,不适用python3,调整后代码如下:代码lower_cas
- BCP语句的作用:BCP是SQL提供的进行数据传输的实用程序,这种语句提供了非常快的数据导入的方法。(当然7。0也有BCP的替代方法就是DT
- 本次系统环境os: Deepin(Linux)Python: 3.7lib: PyYAML=5.3.1 | selenium=3.141.0
- 本文实例分析了CI框架出现mysql数据库连接资源无法释放的解决方法。分享给大家供大家参考,具体如下:使用ci框架提供的类查询数据:$thi
- 阅读上一章:Css布局Chapter 13 为文字指定样式我想以一章的篇幅来讨论用CSS设定文字样式的做法是个好点子.一般处理文字内容大概是