clamp_realtime_viewer/CustomFrame.py

import os
import subprocess

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 QRect,Qt
import numpy as np
import def_pb2 as pb
import GrpcClient as rpc
import vtk
from vtkmodules.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor
#from vtk.qt.QVTKRenderWindowInteractor import QVTKRenderWindowInteractor

import threading
import json
class ControlFrame(QFrame):
    def __init__(self):
        QFrame.__init__(self)
        with open("./ui_config.json",'r',encoding='utf-8') as fp:
            self.config=json.load(fp)
        self.setGeometry(0, 0, 500, 500)
        self.InitUI()
        self.setFrameShape(self.StyledPanel)

    def OnAction(self,dtype,id):
        client=rpc.GrpcStream()
        ip=self.config[self.IPcombox.currentText()]
        port=int(self.PortEdit.text())
        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, 25)
        self.IPcombox = QComboBox(self)
        self.IPcombox.addItems(self.config.keys())
        self.IPcombox.setGeometry(90, 5, 150, 25)

        self.endstatic = QLabel(self)
        self.endstatic.setText("Port:")
        self.endstatic.setGeometry(20, 35, 80, 25)
        self.PortEdit = QLineEdit(self)
        self.PortEdit.setText("9876")
        self.PortEdit.setGeometry(90, 35, 150, 25)

        self.btnDataStreamCheck = QCheckBox("实时显示", self)
        self.btnDataStreamCheck.setGeometry(150, 65, 120, 30)
        self.btnDataStreamCheck.clicked.connect(self.RealTimeDatacb)

        self.staticError = QLabel(self)
        self.staticError.setText("信息:")
        self.staticError.setGeometry(20, 100, 230, 400)
        self.staticError.setWordWrap(True)
        self.staticError.setAlignment(Qt.AlignTop)
    def DisplayErrorInfo(self,info):
        self.staticError.setText("信息:"+info)
    def RealTimeDatacb(self):
        self.staticError.setText("信息:")
        try:
            if self.btnDataStreamCheck.checkState()==Qt.Checked:
                client=rpc.GrpcStream()
                ip=self.config[self.IPcombox.currentText()]
                port=int(self.PortEdit.text())
                client.OpenDataStream(ip,port)
            else:
                client=rpc.GrpcStream()
                ip=self.config[self.IPcombox.currentText()]
                port=int(self.PortEdit.text())
                client.CloseDataStream(ip,port)
        except Exception as e:
            QMessageBox.information(self, 'ERROR',str(e),QMessageBox.Ok,QMessageBox.Ok)
    def closeEvent(self, a0: QCloseEvent) -> None:
        if self.btnDataStreamCheck.checkState()==Qt.Checked:
            client=rpc.GrpcStream()
            ip=self.config[self.IPcombox.currentText()]
            port=int(self.PortEdit.text())
            client.CloseDataStream(ip,port)
        rpc.GrpcStream().close()

class VtkPointCloud:
    def __init__(self, zMin=-10.0, zMax=10.0, maxNumPoints=1e7):
        self.maxNumPoints = maxNumPoints
        self.vtkPolyData = vtk.vtkPolyData()
        self.clearPoints()
        mapper = vtk.vtkPolyDataMapper()
        mapper.SetInputData(self.vtkPolyData)
        mapper.SetColorModeToDefault()
        mapper.SetScalarRange(zMin, zMax)
        mapper.SetScalarVisibility(1)
        self.vtkActor = vtk.vtkActor()


        self.vtkActor.SetMapper(mapper)
        self.vtkActor.GetProperty().SetPointSize(1)
        self.vtkActor.GetProperty().SetColor(1, 1, 1)

    def addPoint(self, point):
        if self.vtkPoints.GetNumberOfPoints() < self.maxNumPoints:
            pointId = self.vtkPoints.InsertNextPoint(point)
            self.vtkDepth.InsertNextValue(point[2])
            self.vtkCells.InsertNextCell(1)
            self.vtkCells.InsertCellPoint(pointId)
        self.vtkCells.Modified()
        self.vtkPoints.Modified()
        self.vtkDepth.Modified()

    def clearPoints(self):
        self.vtkPoints = vtk.vtkPoints()
        self.vtkCells = vtk.vtkCellArray()
        self.vtkDepth = vtk.vtkDoubleArray()
        self.vtkDepth.SetName('DepthArray')
        self.vtkPolyData.SetPoints(self.vtkPoints)
        self.vtkPolyData.SetVerts(self.vtkCells)
        #self.vtkPolyData.GetPointData().SetScalars(self.vtkDepth)
        self.vtkPolyData.GetPointData().SetActiveScalars('DepthArray')

class pclViewer(QVTKRenderWindowInteractor):
    def __init__(self,parent=None):
        super(pclViewer,self).__init__(parent=parent)
    def mousePressEvent(self, ev):
        btn = ev.button()
        if btn == Qt.RightButton:
            print(" r btn")
        else:
            pass
            #QVTKRenderWindowInteractor.mousePressEvent(self,ev)


class VtkPointCloudCanvas(QWidget):
    def __init__(self, *args, **kwargs):
        super(VtkPointCloudCanvas, self).__init__(*args, **kwargs)
        self.lock_=threading.RLock()

        self._layout = QVBoxLayout()
        self.setLayout(self._layout)
        self._vtk_widget = pclViewer(self)
        self._vtk_widget.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera())  # 设置交互方式==常用的方式 移动摄像机

        self._layout.addWidget(self._vtk_widget)

        self._render = vtk.vtkRenderer()
        self._vtk_widget.GetRenderWindow().AddRenderer(self._render)
        self._iren = self._vtk_widget.GetRenderWindow().GetInteractor()
        self._point_cloud = VtkPointCloud()
        self._render.AddActor(self._point_cloud.vtkActor)

        transform = vtk.vtkTransform()
        transform.Translate(0, -0.1, 0.0)
        axes = vtk.vtkAxesActor()
        axes.SetUserTransform(transform)
        axes.SetTotalLength([0.1,0.1,0])

        axes.SetXAxisLabelText('x')
        axes.SetYAxisLabelText('y')
        axes.SetZAxisLabelText('')
        colors = vtk.vtkNamedColors()
        axes.GetXAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Red"))
        axes.GetYAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Green"))
        axes.GetZAxisCaptionActor2D().GetCaptionTextProperty().SetColor(colors.GetColor3d("Blue"))
        self._render.AddActor(axes)

        self.camera=self._render.GetActiveCamera()
        self.show()
        self._iren.Initialize()

    def close(self) -> bool:
        print("  tk close")
        self._point_cloud.clearPoints()

    def displayPCL(self,cloud:np.array):
        colors = vtk.vtkUnsignedCharArray()
        colors.SetNumberOfComponents(3)
        colors.SetName("Colors")

        self._point_cloud.clearPoints()
        for point in cloud:
            self._point_cloud.addPoint(point)
            colors.InsertNextTypedTuple((255,255,255))
        self._point_cloud.vtkPolyData.GetPointData().SetScalars(colors)
        self._vtk_widget.update()
    def resetViewer(self):
        self.update()


class PointCLViwer(QSplitter):
    def __init__(self,OnAction,id):
        super(PointCLViwer, self).__init__()
        self.OnAction=OnAction
        self.dtype=2
        self.id=id
        self.pointCloud=[]
        self.InitUI()
    def InitUI(self):
        self.pclViewer=VtkPointCloudCanvas(self)
        self.addWidget(self.pclViewer)
        self.setStretchFactor(0,4)

    def displayCloud(self,points:np.array):
        self.pointCloud=points
        self.pclViewer.displayPCL(points)
    def resetViewer(self):
        self.pclViewer.resetViewer()
    def updataData(self):
        self.OnAction(self.dtype,self.id-1)
    def displayAllPoints(self):
        self.pclViewer.displayPCL(self.pointCloud)
    def contextMenuEvent(self, a0):
        pass


class ViewerFrame(QFrame):
    def __init__(self,OnAction):
        super(ViewerFrame, self).__init__()
        self.OnAction=OnAction
        self.InitUI()
    def InitUI(self):

        self.table1=QTabWidget(self)
        self.table2=QTabWidget(self)
        self.table3=QTabWidget(self)
        self.table4=QTabWidget(self)
        self.table1.setTabPosition(QTabWidget.TabPosition.North)
        self.table2.setTabPosition(QTabWidget.TabPosition.North)
        self.table3.setTabPosition(QTabWidget.TabPosition.North)
        self.table4.setTabPosition(QTabWidget.TabPosition.North)

        self.pcViewer1=PointCLViwer(self.OnAction,1)
        self.pcViewer2=PointCLViwer(self.OnAction,2)
        self.pcViewer3=PointCLViwer(self.OnAction,3)
        self.pcViewer4=PointCLViwer(self.OnAction,4)

        self.table1.addTab(self.pcViewer1,"C3")
        self.table2.addTab(self.pcViewer2,"C4")
        self.table3.addTab(self.pcViewer3,"C5")
        self.table4.addTab(self.pcViewer4,"C6")

    def closeEvent(self, a0: QCloseEvent) -> None:

        pass
    def resizeEvent(self, a0: QResizeEvent) -> None:
        w, h = self.size().width(), self.size().height()
        w=w-15
        h=h-15
        self.table1.setGeometry(5, 5, w/2, h/2)
        self.table2.setGeometry(10+w/2, 5, w/2, h/2)
        self.table3.setGeometry(5, 10+h/2, w/2, h/2)
        self.table4.setGeometry(10+w/2, 10+h/2, w/2, h/2)

    def PbCloud2Pts(self,cloud):
        size=len(cloud)
        npdata=np.zeros(shape=[size,3],dtype=np.float32)
        i=0
        for point in cloud:
            x=point.x
            y=point.y
            npdata[i,:]=[x,y-0.1,0]
            i=i+1
        return npdata
    def DisplayCloud(self,clouds:pb.clouds):
        self.pcViewer1.displayCloud(self.PbCloud2Pts(clouds.clamp_1))
        self.pcViewer2.displayCloud(self.PbCloud2Pts(clouds.clamp_2))
        self.pcViewer3.displayCloud(self.PbCloud2Pts(clouds.clamp_3))
        self.pcViewer4.displayCloud(self.PbCloud2Pts(clouds.clamp_4))