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)