AGVUI_python/ControllWidget.py

import math

from PyQt5.QtWidgets import QWidget,QFrame,QLabel,QLineEdit,QPushButton,QComboBox
from PyQt5.QtGui import QPixmap,QPainter,QResizeEvent,QCloseEvent,QPaintEvent,QFont
from PyQt5.QtCore import QSize,QTimer
from RobotData import Robot
import dijkstra.Map as mp
class ControlFrame(QFrame):

    def __init__(self,UI_data):
        QFrame.__init__(self)
        [self.name_,self.nodes_]=UI_data
        self.robot_=Robot(self.name_)
        self.setGeometry(0,0,500,500)
        self.InitUI()
        self.setFrameShape(self.StyledPanel)
        self.timer_=QTimer()
        self.timer_.timeout.connect(self.Update)
        self.timer_.start(100)

    def InitUI(self):

        self.basestatic=QLabel(self)
        self.basestatic.setText(self.name_)
        self.basestatic.setGeometry(20, 20, 80, 30)

        self.begstatic=QLabel(self)
        self.begstatic.setText("  起点")
        self.begstatic.setGeometry(100, 5, 50, 30)
        self.begQc=QComboBox(self)
        self.begQc.setGeometry(160, 5, 80, 30)

        self.endstatic=QLabel(self)
        self.endstatic.setText("  终点")
        self.endstatic.setGeometry(100, 50, 50, 30)
        self.endQc=QComboBox(self)
        self.endQc.setGeometry(160, 50, 80, 30)

        self.btnSend=QPushButton(self)
        self.btnSend.setGeometry(150, 100, 100, 40)
        self.btnSend.setText("  启动导航")
        self.btnSend.clicked.connect(self.btnSendClick)

        self.btnCancel=QPushButton(self)
        self.btnCancel.setGeometry(150, 150, 100, 40)
        self.btnCancel.setText("  取消导航")
        self.btnCancel.clicked.connect(self.btnCancelClick)

        self.begQc.addItems(self.nodes_)
        self.endQc.addItems(self.nodes_)
        self.endQc.activated.connect(self.endQCChanged)

        self.posestatic=QLabel(self)
        self.posestatic.setGeometry(260, 5, 100, 90)

        self.LedLabel=QLabel(self)
        self.LedLabel.setGeometry(280, 120, 50, 50)

    def Update(self):
        if self.robot_.timedPose_.timeout()==False:
            x=self.robot_.timedPose_.statu.x
            y=self.robot_.timedPose_.statu.y
            yaw=self.robot_.timedPose_.statu.theta
            self.posestatic.setText("x : %.3f\ny : %.3f\nΘ : %.2f"%(x,y,yaw*180/math.pi))
            if self.robot_.IsNavigating():
                djks_map=mp.DijikstraMap()
                [label,pt]=djks_map.findNeastNode([x,y])
                if self.nodes_.index(label)>=0:
                    self.begQc.setCurrentText(label)

        statu="min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px;  border:1px solid black;background:green"
        if self.robot_.Connected()==False:
            statu="min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px; border:1px solid black;background:gray"
            self.endQc.setEnabled(False)
        elif self.robot_.IsNavigating()==True:
            statu="min-width: 32px; min-height: 32px;max-width:32px;\
                max-height: 32px;border-radius: 16px; border:1px solid black;background:yellow"
            self.endQc.setEnabled(False)
        else:
            self.endQc.setEnabled(True)
        self.LedLabel.setStyleSheet(statu)

    def endQCChanged(self):
        id1=self.begQc.currentText()
        id2=self.endQc.currentText()
        self.robot_.GeneratePath(id1,id2)

    def btnSendClick(self):
        self.robot_.ExecNavtask()
    def btnCancelClick(self):
        self.robot_.CancelNavTask()