python Realization of Monocular Distance Measurement for Nao Robot
- 2021-11-29 07:43:45
- OfStack
In this paper, we share the specific code of python to realize Nao robot monocular distance measurement for your reference. The specific contents are as follows
This code is suitable for Nao robot to do visual recognition and ranging experiments, only provide key code parts, try to use cv2 to optimize the code will be more concise yo!
The main functions of this code are:
1. In the initial posture, find the target by changing the camera and turning the head
2. The distance and angle between the target and Nao are calculated by color threshold
Features can be extended:
1. Measure and correct the direction and distance many times during the movement to improve the accuracy
2. After finding the target, choose which foot to kick the target by measuring the target
#!/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]
#从我们的像素阵列创建1个PIL图像。
im=Image.fromstring("RGB",(imageWidth,imageHeight),array)
#保存图像。
im.save("temp.jpg","JPEG")
#***********************************************
#@函数名： findColorPattern(img,pattern)
#@参数： 略
#@返回值： 无
#@功能说明： 将RGB图像转化为2值图：此方法用的是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
#@返回值： 无
#@功能说明： 利用2值图，计算球的像素坐标。其原理是：遍历各行各列
# 像素的数值的和，最大的组合即为球心坐标
def xyProject(matrix,imagesize):
#声明1个数据类型为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
#@返回值： 无
#@功能说明： 将上面的1系列函数整合起来
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)
#@参数：
#@返回值： 无
#@功能说明： 当找到球后，可能会存在1定的误差。
# 因此需要判断球位于机器人前方的哪1侧，再来确定用哪只脚踢球
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)