add messagebox

Viewer/CustomFrame.py

@@ -1,13 +1,16 @@
 
-from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,
-                             QLineEdit,QCheckBox,QLabel,QFrame,QPushButton,QMenu,QAction)
+from PyQt5.QtWidgets import (QWidget,QVBoxLayout,QTabWidget,QSplitter, QFileDialog,QMessageBox,
+                             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
 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
+import time
 from vtkmodules.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
 #from vtk.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
 
@@ -15,30 +18,37 @@ import threading
 class ControlFrame(QFrame):
     def __init__(self):
         QFrame.__init__(self)
-        self.threadPool_ = ThreadPoolExecutor(5)
         self.setGeometry(0, 0, 500, 500)
+        self.images_=pb.ResImage()
         self.InitUI()
         self.setFrameShape(self.StyledPanel)
         #self.timer_ = QTimer()
         #self.timer_.timeout.connect(self.Update)
         #self.timer_.start(100)
+    def OnImages(self,images:pb.ResImage):
+        self.images_=images
+
     def DisplayMeasureInfo(self,info : pb.MeasureInfo):
         self.ErrorInfo.setText(info.error)
     def OnAction(self,dtype,id):
         client=rpc.GrpcStream()
         ip=self.IPEdit.text()
         port=int(self.PortEdit.text())
-        if dtype==1:
-            self.threadPool_.submit(client.GrabImage,ip,port,id)
-        if dtype==2:
-            self.threadPool_.submit(client.GrabCloud,ip,port,id)
+        try:
+            if dtype==1:
+                client.GrabImage(ip,port,id)
+            if dtype==2:
+                client.GrabCloud(ip,port,id)
+        except Exception as e:
+            QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
+
     def InitUI(self):
 
         self.begstatic = QLabel(self)
         self.begstatic.setText("   IP:")
         self.begstatic.setGeometry(20, 5, 80, 30)
         self.IPEdit = QLineEdit(self)
-        self.IPEdit.setText("192.168.2.45")
+        self.IPEdit.setText("192.168.2.55")
         self.IPEdit.setGeometry(90, 5, 150, 30)
 
         self.endstatic = QLabel(self)
@@ -58,6 +68,14 @@ class ControlFrame(QFrame):
         self.btnCancel.setText("抓取点云")
         self.btnCancel.clicked.connect(self.btnGrabCloudClick)
 
+        self.btnMergeSave = QPushButton(self)
+        self.btnMergeSave.setGeometry(20, 200, 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.btnImageStreamCheck = QCheckBox("实时图像", self)
         self.btnImageStreamCheck.setGeometry(20, 90, 120, 40)
@@ -67,46 +85,62 @@ class ControlFrame(QFrame):
         self.btnDataStreamCheck.setGeometry(150, 90, 120, 40)
         self.btnDataStreamCheck.clicked.connect(self.RealTimeDatacb)
 
+        self.MatrixEdit = QTextEdit(self)
+        self.MatrixEdit.setText("输入4x4变换矩阵")
+        self.MatrixEdit.setGeometry(20, 250, 500, 100)
+        #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.clicked.connect(self.matrix2rpy)
+
         self.ErrorInfo = QLabel(self)
-        self.ErrorInfo.setGeometry(20, 200, 250, 400)
+        self.ErrorInfo.setGeometry(20, 450, 250, 400)
         self.ErrorInfo.setWordWrap(True)
         self.ErrorInfo.setAlignment(Qt.AlignTop)
+
     def RealTimeDatacb(self):
-        if self.btnDataStreamCheck.checkState()==Qt.Checked:
-            client=rpc.GrpcStream()
-            ip=self.IPEdit.text()
-            port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.OpenDataStream,ip,port)
-        else:
-            client=rpc.GrpcStream()
-            ip=self.IPEdit.text()
-            port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.CloseDataStream,ip,port)
+        try:
+            if self.btnDataStreamCheck.checkState()==Qt.Checked:
+                client=rpc.GrpcStream()
+                ip=self.IPEdit.text()
+                port=int(self.PortEdit.text())
+                client.OpenDataStream(ip,port)
+            else:
+                client=rpc.GrpcStream()
+                ip=self.IPEdit.text()
+                port=int(self.PortEdit.text())
+                client.CloseDataStream(ip,port)
+        except Exception as e:
+            QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
     def RealTimeImagecb(self):
-        if self.btnImageStreamCheck.checkState()==Qt.Checked:
-            self.btnGrab.setEnabled(False)
-            client=rpc.GrpcStream()
-            ip=self.IPEdit.text()
-            port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.OpenImageStream,ip,port)
-        else:
-            self.btnGrab.setEnabled(True)
-            client=rpc.GrpcStream()
-            ip=self.IPEdit.text()
-            port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.CloseImageStream,ip,port)
+        try:
+            if self.btnImageStreamCheck.checkState()==Qt.Checked:
+                self.btnGrab.setEnabled(False)
+                client=rpc.GrpcStream()
+                ip=self.IPEdit.text()
+                port=int(self.PortEdit.text())
+                client.OpenImageStream(ip,port)
+            else:
+                self.btnGrab.setEnabled(True)
+                client=rpc.GrpcStream()
+                ip=self.IPEdit.text()
+                port=int(self.PortEdit.text())
+                client.CloseImageStream(ip,port)
+        except Exception as e:
+            QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
     def closeEvent(self, a0: QCloseEvent) -> None:
         if self.btnImageStreamCheck.checkState()==Qt.Checked:
             client=rpc.GrpcStream()
             ip=self.IPEdit.text()
             port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.CloseImageStream,ip,port)
+            client.CloseImageStream(ip,port)
         if self.btnDataStreamCheck.checkState()==Qt.Checked:
             client=rpc.GrpcStream()
             ip=self.IPEdit.text()
             port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.CloseDataStream,ip,port)
-        self.threadPool_.shutdown(wait=False)
+            client.CloseDataStream(ip,port)
         rpc.GrpcStream().close()
     def btnGrabImageClick(self):
         if self.btnImageStreamCheck.checkState()==Qt.Checked:
@@ -115,13 +149,84 @@ class ControlFrame(QFrame):
             client=rpc.GrpcStream()
             ip=self.IPEdit.text()
             port=int(self.PortEdit.text())
-            self.threadPool_.submit(client.GrabImage,ip,port,0)
+            try:
+                client.GrabImage(ip,port,0)
+            except Exception as e:
+                QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
+
+    def btnMergeSaveClick(self):
+        #
+        if  self.images_.HasField("img1") and self.images_.HasField("img2")\
+                and self.images_.HasField("img3") and self.images_.HasField("img4"):
+            img1=self.PbImg2numpy(self.images_.img1)
+            img2=self.PbImg2numpy(self.images_.img2)
+            img3=self.PbImg2numpy(self.images_.img3)
+            img4=self.PbImg2numpy(self.images_.img4)
+            img_up=np.concatenate([img1,img2],axis=1)
+            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'
+            cv2.imwrite("./images/"+file,image)
+        else:
+            print("  ---------------")
+    def PbImg2numpy(self,image:pb.Image):
+        img1=image
+        w=img1.width
+        h=img1.height
+        c=img1.channel
+        btarry=bytearray(img1.data)
+        npdata = np.frombuffer(btarry, dtype=np.uint8)
+        if c==3:
+            npdata = npdata.reshape([h,w,c])
+        if c==1:
+            npdata = npdata.reshape([h,w])
+        return npdata
+
     def btnGrabCloudClick(self):
         client=rpc.GrpcStream()
         ip=self.IPEdit.text()
         port=int(self.PortEdit.text())
-        for id in range(4):
-            self.threadPool_.submit(client.GrabCloud,ip,port,id+1)
+        try:
+            for id in range(4):
+                client.GrabCloud(ip,port,id+1)
+        except Exception as e:
+            QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
+    def rotationMatrixToEulerAngles(self,R) :
+        sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
+        singular = sy < 1e-6
+
+        if  not singular :
+            x = math.atan2(R[2,1] , R[2,2])
+            y = math.atan2(-R[2,0], sy)
+            z = math.atan2(R[1,0], R[0,0])
+        else :
+            x = math.atan2(-R[1,2], R[1,1])
+            y = math.atan2(-R[2,0], sy)
+            z = 0
+        return np.array([x, y, z])
+    def matrix2rpy(self):
+        text=""
+        str=self.MatrixEdit.toPlainText()
+        nums=str.split('\n')
+        if not len(nums)==4:
+            text="Data format Error"
+        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"
+                    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]),
+                                            matrix[0,3],matrix[1,3],matrix[2,3])
+        QMessageBox.information(self, '旋转变换转rpy-t',text,QMessageBox.Ok,QMessageBox.Ok)
     def btnCancelClick(self):
         pass
 class ImageViewer(QLabel):
@@ -239,20 +344,24 @@ class VtkPointCloudCanvas(QWidget):
         self._iren.Initialize()
 
     def close(self) -> bool:
-
         print("  tk close")
         self._point_cloud.clearPoints()
 
-    def displayPCL(self,cloud:np.array):
+    def displayPCL(self,cloud:np.array,type):  #type 1:只显示轮胎，其他：显示所有
         colors = vtk.vtkUnsignedCharArray()
         colors.SetNumberOfComponents(3)
         colors.SetName("Colors")
 
         self._point_cloud.clearPoints()
         for point in cloud:
-            self._point_cloud.addPoint(point)
             prob=point[-1]
-            colors.InsertNextTypedTuple((int(255*prob),int(255-prob*255),255-int(prob*255)))
+            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)))
+            else:
+                self._point_cloud.addPoint(point)
+                colors.InsertNextTypedTuple((int(255*prob),int(255-prob*255),255-int(prob*255)))
         self._point_cloud.vtkPolyData.GetPointData().SetScalars(colors)
         self._vtk_widget.update()
     def resetViewer(self):
@@ -280,27 +389,53 @@ class PointCLViwer(QSplitter):
                 f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
 
         print("Save Cloud to %s"%(fileName2))
+    def saveWheel(self):
+        fileName2, ok2 = QFileDialog.getSaveFileName(self, "文件保存", "C:/","All Files (*);;Text Files (*.txt)")
+        print(fileName2 ,ok2)
+        with open(fileName2,"w+") as f:
+            for point in self.pointCloud:
+                prob=point[3]
+                if prob>0.9:
+                    f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
+
+        print("Save Cloud to %s"%(fileName2))
 
     def displayCloud(self,points:np.array):
         self.pointCloud=points
-        self.pclViewer.displayPCL(points)
+        self.pclViewer.displayPCL(points,0)
     def resetViewer(self):
         self.pclViewer.resetViewer()
     def updataData(self):
         self.OnAction(self.dtype,self.id)
+    def displayWheelPoints(self):
+        self.pclViewer.displayPCL(self.pointCloud,1)
+    def displayAllPoints(self):
+        self.pclViewer.displayPCL(self.pointCloud,0)
     def contextMenuEvent(self, a0):
         menu=QMenu(self)
-        updata_act=QAction("更新点云")
+        updata_act=QAction("抓取点云")
         updata_act.triggered.connect(self.updataData)
         menu.addAction(updata_act)
 
+        show_wheel=QAction("仅显示轮胎")
+        show_wheel.triggered.connect(self.displayWheelPoints)
+        menu.addAction(show_wheel)
+
+        show_all=QAction("显示所有")
+        show_all.triggered.connect(self.displayAllPoints)
+        menu.addAction(show_all)
+
         #act=QAction("还原视野")
         #act.triggered.connect(self.resetViewer)
         #menu.addAction(act)
 
-        actSave=QAction("保存")
+        actSave=QAction("保存所有点")
         actSave.triggered.connect(self.save)
         menu.addAction(actSave)
+
+        actSaveWheel=QAction("保存轮胎点")
+        actSaveWheel.triggered.connect(self.saveWheel)
+        menu.addAction(actSaveWheel)
         ret=menu.exec_(a0.globalPos())
 
 

Viewer/GrpcClient.py

@@ -1,5 +1,5 @@
 import time
-
+from PyQt5.QtWidgets import QMessageBox
 import grpc
 import def_pb2 as pb
 import def_pb2_grpc as mrpc
@@ -34,24 +34,21 @@ class GrpcStream(threading.Thread):
         self.stub = mrpc.StreamServerStub(self.channel)
         cmd=pb.RequestCmd()
         cmd.Id=id
-        try:
-            resImage=self.stub.GetImage(cmd)
-            if self.imagesCallBack is not None:
-                self.imagesCallBack(resImage)
-        except Exception as e:
-            print(e)
+
+        resImage=self.stub.GetImage(cmd)
+        if self.imagesCallBack is not None:
+            self.imagesCallBack(resImage)
+
     def GrabCloud(self,ip,port,id): # id:1-4,其他表示所有
         connectstr='%s:%d'%(ip,port)
         self.channel=grpc.insecure_channel(connectstr)
         self.stub = mrpc.StreamServerStub(self.channel)
         cmd=pb.RequestCmd()
         cmd.Id=id
-        try:
-            resCloud=self.stub.GetCloud(cmd)
-            if self.cloudCallBack is not None:
-                self.cloudCallBack(resCloud)
-        except Exception as e:
-            print(e)
+
+        resCloud=self.stub.GetCloud(cmd)
+        if self.cloudCallBack is not None:
+            self.cloudCallBack(resCloud)
     def OpenDataStream(self,ip,port):
         connectstr='%s:%d'%(ip,port)
         channel=grpc.insecure_channel(connectstr)
@@ -88,6 +85,7 @@ class GrpcStream(threading.Thread):
                             self.imagesCallBack(feature)
                 except Exception as e:
                     self.features_=None
+
     def loopDataStream(self):
         while self.exit_ == False:
             time.sleep(0.001)

Viewer/MainWnd.py

@@ -16,11 +16,26 @@ class MainWindow(QMainWindow):
         self.basic()
         self.Controller = ControlFrame()
         self.viewerFrame = ViewerFrame(self.Controller.OnAction)
-
+        self.Controller.btnSaveAllCloud.clicked.connect(self.saveAllCloud)
 
         splitter= self.split_()
         self.setCentralWidget(splitter)
-
+    def saveAllCloud(self):
+        fileName2, ok2 = QFileDialog.getSaveFileName(self, "文件保存", "C:/","All Files (*);;Text Files (*.txt)")
+        print(fileName2 ,ok2)
+        with open(fileName2,"w+") as f:
+            for point in self.viewerFrame.pcViewer1.pointCloud:
+                prob=point[3]
+                f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
+            for point in self.viewerFrame.pcViewer2.pointCloud:
+                prob=point[3]
+                f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
+            for point in self.viewerFrame.pcViewer3.pointCloud:
+                prob=point[3]
+                f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
+            for point in self.viewerFrame.pcViewer4.pointCloud:
+                prob=point[3]
+                f.write("%f %f %f %d %d %d\n"%(point[0],point[1],point[2],int(255*prob),int(255-prob*255),255-int(prob*255)))
     #窗口基础属性
     def basic(self):
         #设置标题，大小，图标
@@ -49,6 +64,7 @@ class MainWindow(QMainWindow):
         return splitter
     def OnImagesHandle(self,images:pb.ResImage):
         self.viewerFrame.DisplayImage(images)
+        self.Controller.OnImages(images)
 
     def OnCloudHandle(self,clouds:pb.ResCloud):
         self.viewerFrame.DisplayCloud(clouds)