|
@@ -1,12 +1,11 @@
|
|
|
|
|
|
-from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,QMessageBox,
|
|
|
+from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,QMessageBox,QComboBox,
|
|
|
QLineEdit,QTextEdit,QCheckBox,QLabel,QFrame,QPushButton,QMenu,QAction)
|
|
|
from PyQt5.QtGui import QPixmap,QImage,QPainter,QResizeEvent,QCloseEvent,QPaintEvent
|
|
|
-from PyQt5.QtCore import QSize,QTimer,QRect,Qt
|
|
|
+from PyQt5.QtCore import QRect,Qt
|
|
|
import numpy as np
|
|
|
import math
|
|
|
import def_pb2 as pb
|
|
|
-from concurrent.futures import ThreadPoolExecutor, as_completed, wait
|
|
|
import GrpcClient as rpc
|
|
|
import vtk
|
|
|
import cv2
|
|
@@ -16,6 +15,7 @@ from vtkmodules.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
|
|
|
|
|
|
import threading
|
|
|
class ControlFrame(QFrame):
|
|
|
+ cameraMap={"相机1":1,"相机2":2,"相机3":3,"相机4":4}
|
|
|
def __init__(self):
|
|
|
QFrame.__init__(self)
|
|
|
self.setGeometry(0, 0, 500, 500)
|
|
@@ -29,7 +29,14 @@ class ControlFrame(QFrame):
|
|
|
self.images_=images
|
|
|
|
|
|
def DisplayMeasureInfo(self,info : pb.MeasureInfo):
|
|
|
- self.ErrorInfo.setText(info.error)
|
|
|
+ xstr="x\t: %.3f\n"%(info.x) if info.HasField("x") else "x\t: None\n"
|
|
|
+ ystr="y\t: %.3f\n"%(info.y) if info.HasField("y") else "y\t: None\n"
|
|
|
+ thetastr="角度\t: %.3f\n"%(info.theta*180.0/np.pi) if info.HasField("theta") else "角度\t: None\n"
|
|
|
+ widthstr="宽\t: %.3f\n"%(info.width) if info.HasField("width") else "宽\t: None\n"
|
|
|
+ wheelbasestr="轴距\t: %.3f\n"%(info.wheelbase) if info.HasField("wheelbase") else "轴距\t: None\n"
|
|
|
+ fthetastr="前轮角\t: %.3f\n"%(info.ftheta*180.0/np.pi) if info.HasField("ftheta") else "前轮角\t: None\n"
|
|
|
+ error=info.error
|
|
|
+ self.ErrorInfo.setText(xstr+ystr+thetastr+widthstr+wheelbasestr+fthetastr+error)
|
|
|
def OnAction(self,dtype,id):
|
|
|
client=rpc.GrpcStream()
|
|
|
ip=self.IPEdit.text()
|
|
@@ -46,57 +53,80 @@ class ControlFrame(QFrame):
|
|
|
|
|
|
self.begstatic = QLabel(self)
|
|
|
self.begstatic.setText(" IP:")
|
|
|
- self.begstatic.setGeometry(20, 5, 80, 30)
|
|
|
+ self.begstatic.setGeometry(20, 5, 80, 25)
|
|
|
self.IPEdit = QLineEdit(self)
|
|
|
- self.IPEdit.setText("192.168.2.55")
|
|
|
- self.IPEdit.setGeometry(90, 5, 150, 30)
|
|
|
+ self.IPEdit.setText("10.211.31.16")
|
|
|
+ self.IPEdit.setGeometry(90, 5, 150, 25)
|
|
|
|
|
|
self.endstatic = QLabel(self)
|
|
|
self.endstatic.setText("Port:")
|
|
|
- self.endstatic.setGeometry(20, 50, 80, 30)
|
|
|
+ self.endstatic.setGeometry(20, 35, 80, 25)
|
|
|
self.PortEdit = QLineEdit(self)
|
|
|
self.PortEdit.setText("9876")
|
|
|
- self.PortEdit.setGeometry(90, 50, 150, 30)
|
|
|
+ self.PortEdit.setGeometry(90, 35, 150, 25)
|
|
|
+
|
|
|
+ self.btnImageStreamCheck = QCheckBox("实时图像", self)
|
|
|
+ self.btnImageStreamCheck.setGeometry(20, 65, 120, 30)
|
|
|
+ self.btnImageStreamCheck.clicked.connect(self.RealTimeImagecb)
|
|
|
+
|
|
|
+ self.btnDataStreamCheck = QCheckBox("实时结果打印", self)
|
|
|
+ self.btnDataStreamCheck.setGeometry(150, 65, 120, 30)
|
|
|
+ self.btnDataStreamCheck.clicked.connect(self.RealTimeDatacb)
|
|
|
|
|
|
self.btnGrab = QPushButton(self)
|
|
|
- self.btnGrab.setGeometry(20, 140, 100, 40)
|
|
|
+ self.btnGrab.setGeometry(20, 100, 100, 40)
|
|
|
self.btnGrab.setText("抓取图像")
|
|
|
self.btnGrab.clicked.connect(self.btnGrabImageClick)
|
|
|
|
|
|
self.btnCancel = QPushButton(self)
|
|
|
- self.btnCancel.setGeometry(150, 140, 100, 40)
|
|
|
+ self.btnCancel.setGeometry(150, 100, 100, 30)
|
|
|
self.btnCancel.setText("抓取点云")
|
|
|
self.btnCancel.clicked.connect(self.btnGrabCloudClick)
|
|
|
|
|
|
self.btnMergeSave = QPushButton(self)
|
|
|
- self.btnMergeSave.setGeometry(20, 200, 100, 40)
|
|
|
+ self.btnMergeSave.setGeometry(20, 160, 100, 40)
|
|
|
self.btnMergeSave.setText("拼接保存")
|
|
|
self.btnMergeSave.clicked.connect(self.btnMergeSaveClick)
|
|
|
|
|
|
self.btnSaveAllCloud = QPushButton(self)
|
|
|
- self.btnSaveAllCloud.setGeometry(150, 200, 100, 40)
|
|
|
- self.btnSaveAllCloud.setText("保存allPoints")
|
|
|
+ self.btnSaveAllCloud.setGeometry(150, 135, 100, 30)
|
|
|
+ self.btnSaveAllCloud.setText("合并保存点云")
|
|
|
|
|
|
- self.btnImageStreamCheck = QCheckBox("实时图像", self)
|
|
|
- self.btnImageStreamCheck.setGeometry(20, 90, 120, 40)
|
|
|
- self.btnImageStreamCheck.clicked.connect(self.RealTimeImagecb)
|
|
|
-
|
|
|
- self.btnDataStreamCheck = QCheckBox("实时结果打印", self)
|
|
|
- self.btnDataStreamCheck.setGeometry(150, 90, 120, 40)
|
|
|
- self.btnDataStreamCheck.clicked.connect(self.RealTimeDatacb)
|
|
|
+ self.btnSaveWheelSampleCloud = QPushButton(self)
|
|
|
+ self.btnSaveWheelSampleCloud.setGeometry(150, 170, 100, 30)
|
|
|
+ self.btnSaveWheelSampleCloud.setText("保存轮胎样本")
|
|
|
|
|
|
self.MatrixEdit = QTextEdit(self)
|
|
|
self.MatrixEdit.setText("输入4x4变换矩阵")
|
|
|
- self.MatrixEdit.setGeometry(20, 250, 500, 100)
|
|
|
+ self.MatrixEdit.setGeometry(20, 205, 500, 90)
|
|
|
#self.MatrixEdit.setLineWrapMode()
|
|
|
self.MatrixEdit.setAlignment(Qt.AlignTop)
|
|
|
self.btnMatrix = QPushButton(self)
|
|
|
- self.btnMatrix.setGeometry(20, 360, 100, 40)
|
|
|
- self.btnMatrix.setText("转换rpy")
|
|
|
+ self.btnMatrix.setGeometry(20, 300, 100, 30)
|
|
|
+ self.btnMatrix.setText("转换rpy,xyz")
|
|
|
self.btnMatrix.clicked.connect(self.matrix2rpy)
|
|
|
|
|
|
+ self.ParameterEdit = QTextEdit(self)
|
|
|
+ self.ParameterEdit.setText("")
|
|
|
+ self.ParameterEdit.setGeometry(20, 340, 130, 150)
|
|
|
+
|
|
|
+ self.CameraListEdit=QComboBox(self)
|
|
|
+ self.CameraListEdit.addItems(self.cameraMap.keys())
|
|
|
+ self.CameraListEdit.setGeometry(155, 340, 100, 30)
|
|
|
+
|
|
|
+
|
|
|
+ self.btnChange = QPushButton(self)
|
|
|
+ self.btnChange.setGeometry(155, 390, 100, 40)
|
|
|
+ self.btnChange.setText("修改参数")
|
|
|
+ self.btnChange.clicked.connect(self.ChangeCameraParameter)
|
|
|
+
|
|
|
+ self.btnSave = QPushButton(self)
|
|
|
+ self.btnSave.setGeometry(155, 450, 100, 40)
|
|
|
+ self.btnSave.setText("写入参数")
|
|
|
+ self.btnSave.clicked.connect(self.SaveCameraParameter)
|
|
|
+
|
|
|
self.ErrorInfo = QLabel(self)
|
|
|
- self.ErrorInfo.setGeometry(20, 450, 250, 400)
|
|
|
+ self.ErrorInfo.setGeometry(20, 495, 250, 400)
|
|
|
self.ErrorInfo.setWordWrap(True)
|
|
|
self.ErrorInfo.setAlignment(Qt.AlignTop)
|
|
|
|
|
@@ -150,7 +180,7 @@ class ControlFrame(QFrame):
|
|
|
ip=self.IPEdit.text()
|
|
|
port=int(self.PortEdit.text())
|
|
|
try:
|
|
|
- client.GrabImage(ip,port,0)
|
|
|
+ client.GrabImage(ip,port,-1)
|
|
|
except Exception as e:
|
|
|
QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
|
|
|
|
|
@@ -166,7 +196,7 @@ class ControlFrame(QFrame):
|
|
|
img_down=np.concatenate([img3,img4],axis=1)
|
|
|
image=np.concatenate([img_up,img_down],axis=0)
|
|
|
|
|
|
- file=time.strftime("%Y%m%d-%H%M%S")+'.png'
|
|
|
+ file=time.strftime("%Y%m%d-%H%M%S")+'.jpg'
|
|
|
cv2.imwrite("./images/"+file,image)
|
|
|
else:
|
|
|
print(" ---------------")
|
|
@@ -189,7 +219,7 @@ class ControlFrame(QFrame):
|
|
|
port=int(self.PortEdit.text())
|
|
|
try:
|
|
|
for id in range(4):
|
|
|
- client.GrabCloud(ip,port,id+1)
|
|
|
+ client.GrabCloud(ip,port,id)
|
|
|
except Exception as e:
|
|
|
QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
|
|
|
def rotationMatrixToEulerAngles(self,R) :
|
|
@@ -205,28 +235,41 @@ class ControlFrame(QFrame):
|
|
|
y = math.atan2(-R[2,0], sy)
|
|
|
z = 0
|
|
|
return np.array([x, y, z])
|
|
|
+ def SaveCameraParameter(self):
|
|
|
+ camera=self.CameraListEdit.currentText()
|
|
|
+ id=self.cameraMap[camera]
|
|
|
+ text=self.ParameterEdit.toPlainText()
|
|
|
+
|
|
|
+ print(" 保存相机%d参数:%s"%(id,text))
|
|
|
+ def ChangeCameraParameter(self):
|
|
|
+ camera=self.CameraListEdit.currentText()
|
|
|
+ id=self.cameraMap[camera]
|
|
|
+ text=self.ParameterEdit.toPlainText()
|
|
|
+
|
|
|
+ print(" 修改相机%d参数:%s"%(id,text))
|
|
|
def matrix2rpy(self):
|
|
|
text=""
|
|
|
str=self.MatrixEdit.toPlainText()
|
|
|
nums=str.split('\n')
|
|
|
if not len(nums)==4:
|
|
|
- text="Data format Error"
|
|
|
+ text="变换矩阵格式错误 需要4x4矩阵"
|
|
|
else:
|
|
|
matrix=np.zeros(shape=[4,4],dtype=np.float32)
|
|
|
count=0
|
|
|
for line in nums:
|
|
|
datas=line.split(' ')
|
|
|
if not len(datas)==4:
|
|
|
- text="Data format Error"
|
|
|
+ text="变换矩阵格式错误 需要4x4矩阵"
|
|
|
break
|
|
|
else:
|
|
|
matrix[count,:]=[float(datas[0]),float(datas[1]),float(datas[2]),float(datas[3])]
|
|
|
count+=1
|
|
|
|
|
|
rpy=self.rotationMatrixToEulerAngles(matrix)
|
|
|
- text="rpy : %.3f %.3f %.3f\nxyz : %.3f %.3f %.3f"%(math.degrees(rpy[0]),math.degrees(rpy[1]),math.degrees(rpy[2]),
|
|
|
+ text="\"roll\" : %.3f,\n\"pitch\": %.3f,\n\"yaw\": %.3f,\n\"x\":%.3f,\n\"y\":%.3f,\n\"z\":%.3f,"%(math.degrees(rpy[0]),math.degrees(rpy[1]),math.degrees(rpy[2]),
|
|
|
matrix[0,3],matrix[1,3],matrix[2,3])
|
|
|
- QMessageBox.information(self, '旋转变换转rpy-t',text,QMessageBox.Ok,QMessageBox.Ok)
|
|
|
+ self.ParameterEdit.setText(text)
|
|
|
+ #QMessageBox.information(self, '旋转变换转rpy-t',text,QMessageBox.Ok,QMessageBox.Ok)
|
|
|
def btnCancelClick(self):
|
|
|
pass
|
|
|
class ImageViewer(QLabel):
|
|
@@ -245,8 +288,8 @@ class ImageViewer(QLabel):
|
|
|
self.update()
|
|
|
|
|
|
def updataData(self):
|
|
|
- self.OnAction(self.dtype,self.id)
|
|
|
- print("%d,%d"%(self.dtype,self.id))
|
|
|
+ self.OnAction(self.dtype,self.id-1)
|
|
|
+ print("%d,%d"%(self.dtype,self.id-1))
|
|
|
|
|
|
def contextMenuEvent(self, a0):
|
|
|
menu=QMenu(self)
|
|
@@ -358,10 +401,13 @@ class VtkPointCloudCanvas(QWidget):
|
|
|
if type==1:
|
|
|
if prob>0.9:
|
|
|
self._point_cloud.addPoint(point)
|
|
|
- colors.InsertNextTypedTuple((int(255*prob),int(255-prob*255),255-int(prob*255)))
|
|
|
+ colors.InsertNextTypedTuple((0,255,0))
|
|
|
else:
|
|
|
self._point_cloud.addPoint(point)
|
|
|
- colors.InsertNextTypedTuple((int(255*prob),int(255-prob*255),255-int(prob*255)))
|
|
|
+ if prob>0.9:
|
|
|
+ colors.InsertNextTypedTuple((0,255,0))
|
|
|
+ else:
|
|
|
+ colors.InsertNextTypedTuple((255,0,0))
|
|
|
self._point_cloud.vtkPolyData.GetPointData().SetScalars(colors)
|
|
|
self._vtk_widget.update()
|
|
|
def resetViewer(self):
|
|
@@ -406,7 +452,7 @@ class PointCLViwer(QSplitter):
|
|
|
def resetViewer(self):
|
|
|
self.pclViewer.resetViewer()
|
|
|
def updataData(self):
|
|
|
- self.OnAction(self.dtype,self.id)
|
|
|
+ self.OnAction(self.dtype,self.id-1)
|
|
|
def displayWheelPoints(self):
|
|
|
self.pclViewer.displayPCL(self.pointCloud,1)
|
|
|
def displayAllPoints(self):
|
|
@@ -498,6 +544,8 @@ class ViewerFrame(QFrame):
|
|
|
if c==1:
|
|
|
npdata = npdata.reshape([h,w])
|
|
|
qimg=QImage(npdata.data,w,h,QImage.Format_Grayscale8)
|
|
|
+
|
|
|
+
|
|
|
pix=QPixmap(qimg)
|
|
|
return pix
|
|
|
def PbCloud2Pts(self,cloud:pb.PointCloud):
|