AGVUI_python/ControllWidget.py

import datetime
import math
import random
import time
from concurrent.futures import ThreadPoolExecutor, as_completed, wait
from PyQt5.QtWidgets import QWidget, QFrame, QLabel, QLineEdit, QPushButton, QComboBox, QCheckBox
from PyQt5.QtGui import QPixmap, QPainter, QResizeEvent, QCloseEvent, QPaintEvent, QFont
from PyQt5.QtCore import QSize, QTimer, Qt

import Count
import message_pb2
from RobotData import Robot
import dijkstra.Map as mp
from custom_define import ControllerStatus
import MapGLWidget as viewer
import dijkstra.Map as Map


class ControlFrame(QFrame):
    threadPool_ = ThreadPoolExecutor(5)
    last_beg = "**"
    begId_ = "------"
    targetId_ = "------"

    def __init__(self, config):
        QFrame.__init__(self)

        self.street_nodes_ = config['street_nodes']
        self.space_nodes_ = config['space_nodes']
        self.label_ = config['label']
        [id, ip, port, user, pawd] = config['mqtt']
        subtopics = config['subTopics']
        cmdTopic = config['cmdTopic']
        robotColor = config['robotColor']

        self.all_nodes_ = []
        for node in self.street_nodes_:
            self.all_nodes_.append(node)
        for node in self.space_nodes_:
            self.all_nodes_.append(node)

        self.select_map = None
        self.default_wheel_base = 2.6

        rpc = config['rpc']
        self.robot_ = Robot(rpc, self.label_)
        self.robot_.connect(id, ip, port, user, pawd)
        self.robot_.SetSubDataTopic(subtopics, self.messageArrived)
        self.robot_.SetCmdTopic(cmdTopic)
        self.robot_.loop_start()
        self.robot_.SetColor(robotColor, robotColor)

        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 messageArrived(self):
        pass
        # self.gl.update()

    def InitUI(self):

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

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

        self.endstatic = QLabel(self)
        self.endstatic.setText("去马路点")
        self.endstatic.setGeometry(90, 50, 80, 30)
        self.endStreetQc = QComboBox(self)
        self.endStreetQc.setGeometry(160, 50, 80, 30)

        self.endstatic = QLabel(self)
        self.endstatic.setText("去车位点")
        self.endstatic.setGeometry(90, 95, 80, 30)
        self.endSpaceQc = QComboBox(self)
        self.endSpaceQc.setGeometry(160, 95, 80, 30)

        self.btnSend = QPushButton(self)
        self.btnSend.setGeometry(150, 140, 100, 40)
        self.btnSend.setText("  开始")
        self.btnSend.clicked.connect(self.btnSendClick)

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

        self.begQc.addItems(self.all_nodes_)
        self.endStreetQc.addItems(self.street_nodes_)
        self.endStreetQc.addItem("------")
        self.endSpaceQc.addItem("------")
        self.endSpaceQc.addItems(self.space_nodes_)
        self.endStreetQc.activated.connect(self.endStreetQCChanged)
        self.endSpaceQc.activated.connect(self.endSpaceQCChanged)

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

        self.LedLabel = QLabel(self)
        self.LedLabel.setGeometry(80, 160, 50, 50)

        # 发送轴距
        self.wheelbasestatic = QLabel(self)
        self.wheelbasestatic.setText("轴距:")
        self.wheelbasestatic.setGeometry(260, 100, 40, 30)
        self.WheelBaseEdit = QLineEdit(self)
        self.WheelBaseEdit.setText(str(self.default_wheel_base))
        self.WheelBaseEdit.setGeometry(300, 100, 50, 30)

        self.btnModelCheck = QCheckBox("整体协调", self)
        self.btnModelCheck.setGeometry(360, 130, 100, 40)
        self.btnModelCheck.clicked.connect(self.MainAgvchangecb)

        self.btnClampCheck = QCheckBox("夹持", self)
        self.btnClampCheck.setGeometry(260, 160, 60, 40)
        self.btnClampCheck.clicked.connect(self.ClampClick)

        self.btnLifterCheck = QCheckBox("提升", self)
        self.btnLifterCheck.setGeometry(360, 160, 60, 40)
        self.btnLifterCheck.clicked.connect(self.LifterClick)

        self.btnAuto = QCheckBox(self)
        self.btnAuto.setText("自动测试")
        self.btnAuto.setGeometry(260, 195, 100, 40)
        self.btnAuto.stateChanged.connect(self.AutoCheck)

        self.btnAutoDirect = QCheckBox(self)
        self.btnAutoDirect.setText("自由方向")
        self.btnAutoDirect.setGeometry(260, 130, 100, 40)
        self.btnAutoDirect.setCheckState(Qt.Checked)

        self.btnAutoBegChanged = QCheckBox(self)
        self.btnAutoBegChanged.setText("起点自动")
        self.btnAutoBegChanged.setGeometry(260, 10, 100, 20)
        self.btnAutoBegChanged.setCheckState(Qt.Checked)

        # 选择地图
        self.mapstatic = QLabel(self)
        self.mapstatic.setText("地图")
        self.mapstatic.setGeometry(360, 5, 80, 30)

        self.select_map_Qc = QComboBox(self)
        self.select_map_Qc.setGeometry(400, 10, 100, 20)
        maps = ["Front", "Back", "Base"]
        self.select_map_Qc.addItem("------")
        self.select_map_Qc.addItems(maps)
        self.select_map_Qc.activated.connect(self.select_map_QcChanged)

        # 选择流程
        self.seq_static = QLabel(self)
        self.seq_static.setText("流程")
        self.seq_static.setGeometry(360, 35, 80, 30)

        self.select_seq_Qc = QComboBox(self)
        self.select_seq_Qc.setGeometry(400, 40, 100, 20)
        sequence = ["Park", "Pick"]
        self.select_seq_Qc.addItem("------")
        self.select_seq_Qc.addItems(sequence)
        self.select_seq_Qc.activated.connect(self.select_seq_QcChanged)

    def Update(self):
        if self.robot_.timedRobotStatu_.timeout() == False:
            x = self.robot_.timedRobotStatu_.statu.x
            y = self.robot_.timedRobotStatu_.statu.y
            yaw = self.robot_.timedRobotStatu_.statu.theta
            self.posestatic.setText("x : %.3f\ny : %.3f\nΘ : %.2f°" % (x, y, yaw * 180 / math.pi))
            if self.btnAutoBegChanged.checkState() == Qt.Checked:
                [label, pt] = mp.MapManager().findNeastNode("Base", [x, y])
                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.endStreetQc.setEnabled(False)
            self.endSpaceQc.setEnabled(False)
            self.btnModelCheck.setEnabled(False)
            self.btnClampCheck.setEnabled(False)
            self.btnLifterCheck.setEnabled(False)
        else:
            self.endStreetQc.setEnabled(True)
            self.endSpaceQc.setEnabled(True)
            self.btnModelCheck.setEnabled(True)
            self.btnClampCheck.setEnabled(True)
            self.btnLifterCheck.setEnabled(True)
        self.LedLabel.setStyleSheet(statu)

        # self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
        # self.WheelBaseEdit.setVisible(self.robot_.IsMainAgv())
        self.btnModelCheck.setVisible(self.robot_.IsMainAgv())
        if self.robot_.IsMainModeStatu():
            self.btnModelCheck.setCheckState(Qt.Checked)
        else:
            self.btnModelCheck.setCheckState(Qt.Unchecked)

        if self.robot_.IsClampClosed():
            self.btnClampCheck.setCheckState(Qt.Checked)
        else:
            self.btnClampCheck.setCheckState(Qt.Unchecked)

        if self.robot_.IsLiferRose():
            self.btnLifterCheck.setCheckState(Qt.Checked)
        else:
            self.btnLifterCheck.setCheckState(Qt.Unchecked)

        if self.robot_.IsNavigating():
            self.begId_ = self.robot_.begId_
            self.targetId_ = self.robot_.targetId_
            djks_map = mp.MapManager()
            beg_node = djks_map.GetVertex("Base", self.begId_)
            end_node = djks_map.GetVertex("Base", self.targetId_)
            if beg_node is not None:
                self.begQc.setCurrentText(self.begId_)
            if end_node is not None:
                if isinstance(end_node, (mp.SpaceNode)):
                    self.endStreetQc.setCurrentText("------")
                    self.endSpaceQc.setCurrentText(self.targetId_)
                if isinstance(end_node, (mp.StreetNode)):
                    self.endStreetQc.setCurrentText(self.targetId_)
                    self.endSpaceQc.setCurrentText("------")

        # if self.robot_.IsMainModeStatu():
        #     self.btnAutoDirect.setCheckState(Qt.Unchecked)

    def AutoCheck(self):
        if self.btnAuto.checkState() == Qt.Checked:
            self.robot_.autoTest_ = True
        else:
            self.robot_.autoTest_ = False

    def ClampClick(self):
        if self.robot_.IsClampClosed() == False:
            self.threadPool_.submit(self.robot_.ClampClose)
        else:
            self.threadPool_.submit(self.robot_.ClampOpen)

    def LifterClick(self):
        if self.robot_.IsLiferRose() == False:
            self.threadPool_.submit(self.robot_.LiferRise)
        else:
            self.threadPool_.submit(self.robot_.LiferDown)

    def MainAgvchangecb(self):
        if self.robot_.IsMainModeStatu() == False:
            leng = float(self.WheelBaseEdit.text())
            self.threadPool_.submit(self.robot_.SwitchMoveMode, 1, leng)
            self.robot_.L_ = leng
        else:
            self.threadPool_.submit(self.robot_.SwitchMoveMode, 0, 0)

    def endStreetQCChanged(self):
        id1 = self.begQc.currentText()
        id2 = self.endStreetQc.currentText()
        if not id1 == "------" and not id2 == "------":
            self.robot_.GeneratePath(id1, id2, self.select_map)
            self.begId_ = id1
            self.targetId_ = id2

    def endSpaceQCChanged(self):
        id1 = self.begQc.currentText()
        id2 = self.endSpaceQc.currentText()
        if not id1 == "------" and not id2 == "------":
            self.robot_.GeneratePath(id1, id2, self.select_map)
            self.begId_ = id1
            self.targetId_ = id2

    def btnSendClick(self):
        if self.btnAuto.checkState() == Qt.Checked:
            if self.robot_.IsMainModeStatu():
                self.threadPool_.submit(self.robot_.AutoTestNavClamp,
                                        self.begId_, self.targetId_)
            else:
                print("agv not in main statu")
        else:
            autoDirect = False
            if self.btnAutoDirect.checkState() == Qt.Checked:
                print("自由方向")
                autoDirect = True
            self.robot_.GeneratePath(self.begId_, self.targetId_, self.select_map)
            self.threadPool_.submit(self.robot_.Navigatting,
                                    self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()),
                                    self.robot_.current_sequence_)

    def btnCancelClick(self):
        self.controller_status = ControllerStatus.eCancel
        self.threadPool_.submit(self.robot_.CancelNavTask)
        self.btnAuto.setCheckState(Qt.Unchecked)

    def select_map_QcChanged(self):
        self.select_map = self.select_map_Qc.currentText()
        mp.MapManager().ResetMap("Front", 0, float(self.WheelBaseEdit.text()) / 2)
        mp.MapManager().ResetMap("Back", 0, -float(self.WheelBaseEdit.text()) / 2)
        print("changed selected map to" + self.select_map)

    def select_seq_QcChanged(self):
        print("当前流程:", self.select_seq_Qc.currentText())
        if self.select_seq_Qc.currentText() == "Park":
            self.robot_.SetSequence(1)
        elif self.select_seq_Qc.currentText() == "Pick":
            self.robot_.SetSequence(2)
        else:
            self.robot_.SetSequence(0)

class CountFrame(QFrame):
    def __init__(self):
        QFrame.__init__(self)

        self.default_wheel_base = 2.6
        self.InitUI()
        self.timer_ = QTimer()
        self.timer_.timeout.connect(self.UpdateUI)
        self.timer_.start(200)

    def InitUI(self):
        self.static1 = QLabel(self)
        self.static1.setText("重载导航次数 : /")
        self.static1.setGeometry(5, 5, 200, 30)

        self.static2 = QLabel(self)
        self.static2.setText("单车导航次数 : /")
        self.static2.setGeometry(5, 40, 200, 30)

        self.btnFrontCheck = QCheckBox("前车地图", self)
        self.btnFrontCheck.setGeometry(210, 5, 100, 30)
        self.btnFrontCheck.clicked.connect(self.FrontChecked)

        self.btnBackCheck = QCheckBox("后车地图", self)
        self.btnBackCheck.setGeometry(310, 5, 100, 30)
        self.btnBackCheck.clicked.connect(self.FrontChecked)

        self.btnMainCheck = QCheckBox("整车地图", self)
        self.btnMainCheck.setGeometry(210, 40, 100, 30)
        self.btnMainCheck.clicked.connect(self.FrontChecked)

        self.btnBaseCheck = QCheckBox("原始地图", self)
        self.btnBaseCheck.setGeometry(310, 40, 100, 30)
        self.btnBaseCheck.clicked.connect(self.FrontChecked)

    def FrontChecked(self):
        # wheelBase = self.default_wheel_base
        maps = []
        if self.btnFrontCheck.checkState() == Qt.Checked:
            maps.append("Front")
        if self.btnBackCheck.checkState() == Qt.Checked:
            maps.append("Back")
        if self.btnMainCheck.checkState() == Qt.Checked:
            maps.append("Main")
        if self.btnBaseCheck.checkState() == Qt.Checked:
            maps.append("Base")
        view = viewer.MapGLWidget()
        view.SetEnableMaps(maps)

    def UpdateUI(self):
        load_str = "重载导航次数 : %d" % (Count.TestCount().loadCount())
        single_str = "单车导航次数 : %d" % (Count.TestCount().singleCount())
        self.static1.setText(load_str)
        self.static2.setText(single_str)