采用线程池自动测试，导航与夹持

ControllWidget.py

@@ -1,82 +1,90 @@
+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
 
-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 auto_test
 from RobotData import Robot
 import dijkstra.Map as mp
-import threadPool
-from RobotTask import SingleNavTask,SingleClampCloseTask,\
-    SingleClampOpenTask,LoadedNavTask,CancelTask
+from RobotTask import SingleNavTask, SingleClampCloseTask, \
+    SingleClampOpenTask, LoadedNavTask, CancelTask
+
+
 class ControlFrame(QFrame):
-    last_beg="**"
-    begId_="------"
-    targetId_="------"
-    def __init__(self,config):
+    threadPool_ = ThreadPoolExecutor(2)
+    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.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_=[]
+        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.robot_=Robot(self.label_)
-        self.robot_.connect(id,ip,port,user,pawd)
-        self.robot_.SetSubDataTopic(subtopics,self.messageArrived)
+        self.robot_ = Robot(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.robot_.SetColor(robotColor, robotColor)
 
-        self.setGeometry(0,0,500,500)
+        self.setGeometry(0, 0, 500, 500)
         self.InitUI()
         self.setFrameShape(self.StyledPanel)
-        self.timer_=QTimer()
+        self.timer_ = QTimer()
         self.timer_.timeout.connect(self.Update)
         self.timer_.start(100)
 
     def messageArrived(self):
         pass
-        #self.gl.update()
+        # self.gl.update()
+
     def InitUI(self):
 
-        self.basestatic=QLabel(self)
+        self.basestatic = QLabel(self)
         self.basestatic.setText(self.label_)
         self.basestatic.setGeometry(20, 20, 80, 30)
 
-        self.begstatic=QLabel(self)
+        self.begstatic = QLabel(self)
         self.begstatic.setText("    起点")
         self.begstatic.setGeometry(90, 5, 80, 30)
-        self.begQc=QComboBox(self)
+        self.begQc = QComboBox(self)
         self.begQc.setGeometry(160, 5, 80, 30)
 
-        self.endstatic=QLabel(self)
+        self.endstatic = QLabel(self)
         self.endstatic.setText("去马路点")
         self.endstatic.setGeometry(90, 50, 80, 30)
-        self.endStreetQc=QComboBox(self)
+        self.endStreetQc = QComboBox(self)
         self.endStreetQc.setGeometry(160, 50, 80, 30)
 
-        self.endstatic=QLabel(self)
+        self.endstatic = QLabel(self)
         self.endstatic.setText("去车位点")
         self.endstatic.setGeometry(90, 95, 80, 30)
-        self.endSpaceQc=QComboBox(self)
+        self.endSpaceQc = QComboBox(self)
         self.endSpaceQc.setGeometry(160, 95, 80, 30)
 
-        self.btnSend=QPushButton(self)
+        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 = QPushButton(self)
         self.btnCancel.setGeometry(150, 190, 100, 40)
         self.btnCancel.setText("  取消导航")
         self.btnCancel.clicked.connect(self.btnCancelClick)
@@ -89,73 +97,101 @@ class ControlFrame(QFrame):
         self.endStreetQc.activated.connect(self.endStreetQCChanged)
         self.endSpaceQc.activated.connect(self.endSpaceQCChanged)
 
-        self.posestatic=QLabel(self)
+        self.posestatic = QLabel(self)
         self.posestatic.setGeometry(260, 5, 100, 90)
 
-        self.LedLabel=QLabel(self)
+        self.LedLabel = QLabel(self)
         self.LedLabel.setGeometry(80, 160, 50, 50)
 
-        #发送轴距
-        self.wheelbasestatic=QLabel(self)
+        # 发送轴距
+        self.wheelbasestatic = QLabel(self)
         self.wheelbasestatic.setText("轴距:")
         self.wheelbasestatic.setGeometry(260, 90, 40, 30)
-        self.WheelBaseEdit=QLineEdit(self)
+        self.WheelBaseEdit = QLineEdit(self)
         self.WheelBaseEdit.setText("3.3")
         self.WheelBaseEdit.setGeometry(300, 90, 50, 30)
 
-        self.btnModelCheck=QCheckBox("双车模式",self)
-        self.btnModelCheck.setGeometry(260, 140, 100, 40)
-        self.btnModelCheck.stateChanged.connect(self.MainAgvchangecb)
+        self.btnModelCheck = QCheckBox("双车模式", self)
+        self.btnModelCheck.setGeometry(260, 130, 100, 40)
+        self.btnModelCheck.clicked.connect(self.MainAgvchangecb)
 
-        self.btnClampCheck=QCheckBox("夹持",self)
-        self.btnClampCheck.setGeometry(360, 140, 60, 40)
-        self.btnClampCheck.stateChanged.connect(self.ClampClick)
+        self.btnClampCheck = QCheckBox("夹持", self)
+        self.btnClampCheck.setGeometry(260, 160, 60, 40)
+        self.btnClampCheck.clicked.connect(self.ClampClick)
 
-        self.btnAuto=QCheckBox(self)
+        self.btnAuto = QCheckBox(self)
         self.btnAuto.setText("自动测试")
-        self.btnAuto.setGeometry(260, 190, 100, 40)
+        self.btnAuto.setGeometry(260, 195, 100, 40)
+        self.btnAuto.stateChanged.connect(self.AutoCheck)
 
     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_.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))
 
-            djks_map=mp.DijikstraMap()
-            [label,pt]=djks_map.findNeastNode([x,y])
-            if self.all_nodes_.index(label)>=0:
-                if not label==self.last_beg:
+            djks_map = mp.DijikstraMap()
+            [label, pt] = djks_map.findNeastNode([x, y])
+            if self.all_nodes_.index(label) >= 0:
+                if not label == self.last_beg:
                     self.begQc.setCurrentText(label)
-                    self.last_beg=label
-                    print(" change beg ",self.last_beg)
+                    self.last_beg = label
+                    print(" change beg ", self.last_beg)
 
-        statu="min-width: 32px; min-height: 32px;max-width:32px;\
+        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;\
+        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;\
+            # 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)
         else:
             self.endStreetQc.setEnabled(True)
             self.endSpaceQc.setEnabled(True)
             self.btnModelCheck.setEnabled(True)
+            self.btnClampCheck.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.btnAuto.checkState()==Qt.Checked:
-            if self.robot_.IsNavigating()==False and self.robot_.Connected():
+        if self.robot_.IsClampClosed():
+            self.btnClampCheck.setCheckState(Qt.Checked)
+        else:
+            self.btnClampCheck.setCheckState(Qt.Unchecked)
+
+        if self.robot_.IsNavigating():
+            self.begId_ = self.robot_.begId_
+            self.targetId_ = self.robot_.targetId_
+            djks_map = mp.DijikstraMap()
+            beg_node = djks_map.GetVertex(self.begId_)
+            end_node = djks_map.GetVertex(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.btnAuto.checkState() == Qt.Checked:
+            pass
+            '''if self.robot_.IsNavigating()==False and self.robot_.Connected():
                 beg=self.begQc.currentText()
                 djks_map=mp.DijikstraMap()
                 node=djks_map.GetVertex(beg)
@@ -163,60 +199,57 @@ class ControlFrame(QFrame):
                 if node is not None:
                     if isinstance(node,(mp.SpaceNode)):
                         end="O"
-                        self.endStreetQc.setCurrentText(end)
-                        self.endSpaceQc.setCurrentText("------")
                     if isinstance(node,(mp.StreetNode)):
                         end_nodes=["E","B","C","D"]
                         id=random.randint(0,1000)%4
-                        end=end_nodes[id]
-                        self.endSpaceQc.setCurrentText(end)
-                        self.endStreetQc.setCurrentText("------")
+                        end=end_nodes[id]'''
 
-                    self.robot_.GeneratePath(beg,end)
-                    self.btnSendClick()'''
 
+    def AutoCheck(self):
+        if self.btnAuto.checkState()==Qt.Checked:
+            self.robot_.autoTest_=True
+        else:
+            self.robot_.autoTest_=False
     def ClampClick(self):
-        if self.btnClampCheck.checkState() == Qt.Checked:
-            task=SingleClampCloseTask(self.robot_)
-            threadPool.ThreadPool().InputTask(task)
-        if self.btnClampCheck.checkState() == Qt.Unchecked:
-            task=SingleClampOpenTask(self.robot_)
-            threadPool.ThreadPool().InputTask(task)
+        if self.robot_.IsClampClosed() == False:
+            self.threadPool_.submit(self.robot_.ClampClose)
+        else:
+            self.threadPool_.submit(self.robot_.ClampOpen)
+
     def MainAgvchangecb(self):
-        if self.btnModelCheck.checkState() == Qt.Checked:
-            leng=float(self.WheelBaseEdit.text())
-            self.robot_.SwitchMoveMode(2,leng)
-        if self.btnModelCheck.checkState() == Qt.Unchecked:
-            self.robot_.SwitchMoveMode(1,0)
+        if self.robot_.IsMainModeStatu() == False:
+            leng = float(self.WheelBaseEdit.text())
+            self.threadPool_.submit(self.robot_.SwitchMoveMode, 2, leng)
+        else:
+            self.threadPool_.submit(self.robot_.SwitchMoveMode, 1, 0)
+
     def endStreetQCChanged(self):
-        id1=self.begQc.currentText()
-        id2=self.endStreetQc.currentText()
-        if not id1=="------" and not id2=="------":
-            self.robot_.GeneratePath(id1,id2)
+        id1 = self.begQc.currentText()
+        id2 = self.endStreetQc.currentText()
+        if not id1 == "------" and not id2 == "------":
+            self.robot_.GeneratePath(id1, id2)
             self.begId_=id1
             self.targetId_=id2
-            self.endSpaceQc.setCurrentText("------")
+
     def endSpaceQCChanged(self):
-        id1=self.begQc.currentText()
-        id2=self.endSpaceQc.currentText()
-        if not id1=="------" and not id2=="------":
-            self.robot_.GeneratePath(id1,id2)
+        id1 = self.begQc.currentText()
+        id2 = self.endSpaceQc.currentText()
+        if not id1 == "------" and not id2 == "------":
+            self.robot_.GeneratePath(id1, id2)
             self.begId_=id1
             self.targetId_=id2
-            self.endStreetQc.setCurrentText("------")
+
     def btnSendClick(self):
-        wheelbase=float(self.WheelBaseEdit.text())
-        task=None
-        if self.btnModelCheck.checkState()==Qt.Unchecked:
-            task=SingleNavTask(self.robot_,self.begId_,self.targetId_)
-        else:
-            task=LoadedNavTask(self.robot_,self.begId_,self.targetId_,wheelbase)
-        threadPool.ThreadPool().InputTask(task)
+        if self.btnAuto.checkState()==Qt.Checked:
+            self.threadPool_.submit(self.robot_.AutoTestNavClamp,
+                                    self.begId_, self.targetId_)
+        else :
+            self.threadPool_.submit(self.robot_.TestNavClampOnce,
+                                    self.begId_, self.targetId_)
 
     def btnCancelClick(self):
+        self.threadPool_.submit(self.robot_.CancelNavTask)
         self.btnAuto.setCheckState(Qt.Unchecked)
-        task=CancelTask(self.robot_)
-        threadPool.ThreadPool().InputTask(task)
 
 
 class CountFrame(QFrame):
@@ -225,30 +258,21 @@ class CountFrame(QFrame):
 
         self.InitUI()
 
-        self.timer_=QTimer()
+        self.timer_ = QTimer()
         self.timer_.timeout.connect(self.UpdateUI)
         self.timer_.start(200)
 
     def InitUI(self):
-        self.static1=QLabel(self)
+        self.static1 = QLabel(self)
         self.static1.setText("重载导航次数 : /")
         self.static1.setGeometry(5, 5, 200, 30)
 
-        self.static2=QLabel(self)
+        self.static2 = QLabel(self)
         self.static2.setText("单车导航次数 : /")
         self.static2.setGeometry(5, 40, 200, 30)
 
-
-
-
     def UpdateUI(self):
-        load_str="重载导航次数 : %d"%(threadPool.ThreadPool().loadCount())
-        single_str="单车导航次数 : %d"%(threadPool.ThreadPool().singleCount())
+        load_str = "重载导航次数 : %d" % (auto_test.TestCount().loadCount())
+        single_str = "单车导航次数 : %d" % (auto_test.TestCount().singleCount())
         self.static1.setText(load_str)
         self.static2.setText(single_str)
-
-
-
-
-
-

MapGLWidget.py

@@ -261,8 +261,8 @@ class MapGLWidget(PyGLWidget):
 
         # 绘制agv
         if robot is not None:
-            if robot.timedPose_.timeout() == False:
-                agv_statu = robot.timedPose_.statu
+            if robot.timedRobotStatu_.timeout() == False:
+                agv_statu = robot.timedRobotStatu_.statu
                 x = agv_statu.x
                 y = agv_statu.y
                 theta = agv_statu.theta

RobotData.py

@@ -1,133 +1,216 @@
-
-
 import time
 import message_pb2 as message
 from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
-from dijkstra.Map import DijikstraMap
+from dijkstra.Map import DijikstraMap,SpaceNode,StreetNode
 import uuid
+from auto_test import TestCount
+import random
+
+
 class TimeStatu:
-    def __init__(self,statu=None,timeout=3):
-        self.statu=statu
-        self.time=time.time()
-        self.timeout_ms=timeout
+    def __init__(self, statu=None, timeout=3):
+        self.statu = statu
+        self.time = time.time()
+        self.timeout_ms = timeout
 
     def timeout(self):
-        tm=time.time()
-        return tm-self.time>self.timeout_ms or self.statu==None
+        tm = time.time()
+        return tm - self.time > self.timeout_ms or self.statu == None
+
 
 class Robot(MqttAsync):
-    def __init__(self,name=""):
-        self.timedPose_=TimeStatu(message.AGVStatu,0)
-        self.timedNavStatu_=TimeStatu(message.NavStatu,0)
-        self.dataTopic_={}
-        self.messageArrivedSignal=None
-        self.currentNavData_=None
-        self.navCmdTopic_=None
-        self.currentNavPathNodes_=None
-        self.currentNavPath_=None
-
-        self.pathColor_=[1,1,0]
-        self.robotColor_=[0.7,0.2,0.3]      #agv当前颜色
-        self.name_=name
-
-        self.l_=0.8       #轮长
-        self.L_=1.3       #轴距
-        self.W_=2.5       #宽
+    def __init__(self, name=""):
+        MqttAsync.__init__(self)
+        self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
+        self.timedNavStatu_ = TimeStatu(message.NavStatu, 0)
+        self.dataTopic_ = {}
+        self.messageArrivedSignal = None
+        self.currentNavData_ = None
+        self.navCmdTopic_ = None
+        self.currentNavPathNodes_ = None
+        self.currentNavPath_ = None
+
+        self.pathColor_ = [1, 1, 0]
+        self.robotColor_ = [0.7, 0.2, 0.3]  # agv当前颜色
+        self.name_ = name
+
+        self.begId_="------"
+        self.targetId_="------"
+
+        self.autoTest_=False
+
+        self.l_ = 0.8  # 轮长
+        self.L_ = 1.3  # 轴距
+        self.W_ = 2.5  # 宽
 
     def Color(self):
         if self.IsMainModeStatu():
-            return [0,0,0]
+            return [0, 0, 0]
         return self.robotColor_
+
     def IsMainAgv(self):
-        if self.timedNavStatu_.timeout()==False:
-            if self.timedNavStatu_.statu.main_agv==True:
+        if self.timedNavStatu_.timeout() == False:
+            if self.timedNavStatu_.statu.main_agv == True:
                 return True
         return False
+
     def IsMainModeStatu(self):
         if self.IsMainAgv():
             if self.timedNavStatu_.timeout() == False:
-                if self.timedNavStatu_.statu.move_mode==2:
+                if self.timedNavStatu_.statu.move_mode == 2:
                     return True
         return False
-    def SetSubDataTopic(self,match:dict,messageSignal=None):
-        self.dataTopic_=match
-        self.messageArrivedSignal=messageSignal
+
+    def SetSubDataTopic(self, match: dict, messageSignal=None):
+        self.dataTopic_ = match
+        self.messageArrivedSignal = messageSignal
         for item in match.items():
-            [topic,_]=item
-            self.subscribe(topic,self.on_message)
-    def SetCmdTopic(self,topic):
-        self.navCmdTopic_=topic
-    def SetColor(self,pathColor,robotColor):
-        self.pathColor_=pathColor
-        self.robotColor_=robotColor
-    def ResetPose(self,agv : message.AGVStatu):
-        self.timedPose_=TimeStatu(agv,0.5)
-    def ResetNavStatu(self,statu:message.NavStatu):
-        self.timedNavStatu_=TimeStatu(statu,0.5)
-
-    def on_message(self,client, userdata, msg):
-        topic=msg.topic
+            [topic, _] = item
+            self.subscribe(topic, self.on_message)
+
+    def SetCmdTopic(self, topic):
+        self.navCmdTopic_ = topic
+
+    def SetColor(self, pathColor, robotColor):
+        self.pathColor_ = pathColor
+        self.robotColor_ = robotColor
+
+    def ResetPose(self, agv: message.RobotStatu):
+        self.timedRobotStatu_ = TimeStatu(agv, 0.5)
+
+    def ResetNavStatu(self, statu: message.NavStatu):
+        self.timedNavStatu_ = TimeStatu(statu, 0.5)
+
+    def on_message(self, client, userdata, msg):
+        topic = msg.topic
         if self.dataTopic_.get(topic) is not None:
-            dtype=self.dataTopic_[topic]
-            if dtype==message.AGVStatu.__name__:
-                proto=message.AGVStatu()
-                jtf.Parse(msg.payload.decode(),proto)
+            dtype = self.dataTopic_[topic]
+            if dtype == message.RobotStatu.__name__:
+                proto = message.RobotStatu()
+                jtf.Parse(msg.payload.decode(), proto)
                 self.ResetPose(proto)
-            if dtype==message.NavStatu.__name__:
-                self.last_running=self.IsNavigating()
-                proto=message.NavStatu()
-                jtf.Parse(msg.payload.decode(),proto)
+            if dtype == message.NavStatu.__name__:
+                self.last_running = self.IsNavigating()
+                proto = message.NavStatu()
+                jtf.Parse(msg.payload.decode(), proto)
                 self.ResetNavStatu(proto)
-                if self.last_running==True and self.IsNavigating()==False:
-                    self.currentNavPathNodes_=None
-                    self.currentNavPath_=None
+                if self.last_running == True and self.IsNavigating() == False:
+                    self.currentNavPathNodes_ = None
+                    self.currentNavPath_ = None
             if self.messageArrivedSignal is not None:
                 self.messageArrivedSignal()
 
     def MpcSelectTraj(self):
-        traj=[]
-        if self.timedNavStatu_.timeout()==False:
-            nav=self.timedNavStatu_.statu
+        traj = []
+        if self.timedNavStatu_.timeout() == False:
+            nav = self.timedNavStatu_.statu
             for pose2d in nav.selected_traj.poses:
-                traj.append([pose2d.x,pose2d.y,pose2d.theta])
+                traj.append([pose2d.x, pose2d.y, pose2d.theta])
         return traj
 
     def MpcPredictTraj(self):
-        traj=[]
-        if self.timedNavStatu_.timeout()==False:
-            nav=self.timedNavStatu_.statu
+        traj = []
+        if self.timedNavStatu_.timeout() == False:
+            nav = self.timedNavStatu_.statu
             for pose2d in nav.predict_traj.poses:
-                traj.append([pose2d.x,pose2d.y,pose2d.theta])
+                traj.append([pose2d.x, pose2d.y, pose2d.theta])
         return traj
 
     def Connected(self):
-        return self.timedNavStatu_.timeout()==False and self.timedPose_.timeout()==False
+        return self.timedNavStatu_.timeout() == False and self.timedRobotStatu_.timeout() == False
+
     def IsNavigating(self):
-        if self.timedNavStatu_.timeout()==False:
-            key=self.timedNavStatu_.statu.key
-            if key=="" or key==None:
+        if self.timedNavStatu_.timeout() == False:
+            key = self.timedNavStatu_.statu.key
+            if key == "" or key == None:
                 return False
         return True
-    def GeneratePath(self,begID,endID):
-        self.currentNavPathNodes_=DijikstraMap().GetShortestPath(begID,endID)
-        self.currentNavPath_=DijikstraMap().CreatePath(self.currentNavPathNodes_,0.5)
+
+    def GeneratePath(self, begID, endID):
+        self.currentNavPathNodes_ = DijikstraMap().GetShortestPath(begID, endID)
+        self.currentNavPath_ = DijikstraMap().CreatePath(self.currentNavPathNodes_, 0.5)
         if self.currentNavPathNodes_ is not None and self.currentNavPath_ is not None:
+            self.begId_=begID
+            self.targetId_=endID
             return True
         return False
+
+
+    def PositionId(self):
+        if self.timedRobotStatu_.timeout():
+            return None
+        x=self.timedRobotStatu_.statu.x
+        y=self.timedRobotStatu_.statu.y
+        djks_map = DijikstraMap()
+        [label, pt] = djks_map.findNeastNode([x, y])
+        return [label,pt]
+    '''
+    阻塞直到导航完成
+    '''
+    def Navigatting(self, begId, targetId):
+        print("nav")
+        self.GeneratePath(begId, targetId)
+        self.ExecNavtask()
+        while self.IsNavigating() == True:
+            if self.Connected() == False:
+                print("robot disconnected cancel task")
+                self.CancelNavTask()
+                return False
+            time.sleep(0.5)
+        print(" Nav completed!!!")
+        return True
+    def TestNavClampOnce(self,begId,targetId):
+        if self.IsClampClosed()==False:
+            print("closed")
+            if self.ClampClose()==False:
+                return False
+
+        if self.Navigatting(begId,targetId):
+            if self.ClampOpen():
+                TestCount().loadCountAdd()
+                return True
+        return False
+    def AutoTestNavClamp(self,begId,targetId):
+        beg=begId
+        target=targetId
+        while self.autoTest_:
+            if self.TestNavClampOnce(beg,target)==False:
+                print("  quit auto Test")
+                break
+            print("1111")
+            posInfo=self.PositionId()
+            if posInfo is not None:
+                [label,pt]=posInfo
+                beg=label
+                print("beg",beg)
+                node=DijikstraMap().GetVertex(beg)
+
+                if node is not None:
+                    if isinstance(node,(SpaceNode)):
+                        target="O"
+                    if isinstance(node,(StreetNode)):
+                        end_nodes=["E","B","C","D"]
+                        id=random.randint(0,1000)%4
+                        target=end_nodes[id]
+                    print("3333",target)
+            print("  Next nav clamp cmd,%s to %s"%(beg,target))
+
+
+
     def ExecNavtask(self):
         if self.navCmdTopic_ == None:
             print("Robot has not set nav cmd topic")
             return False
         if self.IsNavigating():
-            print(" robot is navigating ,key:%s"%(self.timedNavStatu_.statu.key))
+            print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
             return False
 
-        cmd=None
-        if self.currentNavPathNodes_ is not None and self.timedPose_.timeout()==False:
-            statu=self.timedPose_.statu
+        cmd = None
+        if self.currentNavPathNodes_ is not None and self.timedRobotStatu_.timeout() == False:
+            statu = self.timedRobotStatu_.statu
             if statu is not None:
-                cmd=DijikstraMap().CreateNavCmd([statu.x,statu.y,statu.theta],self.currentNavPathNodes_)
+                cmd = DijikstraMap().CreateNavCmd([statu.x, statu.y, statu.theta], self.currentNavPathNodes_)
         else:
             print("current path is none")
             return False
@@ -136,62 +219,116 @@ class Robot(MqttAsync):
             print("Nav cmd is None")
             return False
 
-        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
         time.sleep(1)
-        while self.IsNavigating()==False:
-            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        while self.IsNavigating() == False:
+            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
-    def SwitchMoveMode(self,mode,wheelbase):
 
-        cmd =message.NavCmd()
-        if mode==2:
-            cmd.wheelbase=wheelbase
-            cmd.action=4
-        if mode==1:
-            cmd.action=5
+    def SwitchMoveMode(self, mode, wheelbase):
+        if self.IsMainAgv() == False:
+            print(" this agv is single can not switch mode")
+            return False
+        cmd = message.NavCmd()
+        if mode == 2:
+            cmd.wheelbase = wheelbase
+            cmd.action = 4
+        if mode == 1:
+            cmd.action = 5
+
+        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+        while True:
+            if mode == 2:
+                if self.IsMainModeStatu():
+                    return True
+            if mode == 1:
+                if self.IsMainModeStatu() == False:
+                    return True
+            time.sleep(0.1)
+            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+        return False
+
+    def IsClampClosed(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
+                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
+                return (clamp == 1 and clamp_other == 1)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.clamp == 1
 
-        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        return False
+
+    def IsClampRunning(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
+                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
+                return (clamp == 0 or clamp_other == 0)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.clamp == 0
+
+        return False
+
+    def IsClampOpened(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
+                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
+                return (clamp == 2 and clamp_other == 2)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.clamp == 2
+
+        return False
 
     def ClampClose(self):
-        cmd =message.NavCmd()
+        if self.IsClampClosed() == True:
+            print("clamp closed")
+            return True
+        cmd = message.NavCmd()
         key = str(uuid.uuid4())
-        cmd.key=(key)
-        act=message.Action()
-        act.type=3
+        cmd.key = (key)
+        act = message.Action()
+        act.type = 3
         cmd.actions.add().CopyFrom(act)
-        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
         time.sleep(1)
-        while self.IsNavigating()==False:
-            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        while True:
+            if self.IsClampClosed() == True:
+                return True
+            if self.IsClampRunning()==False:
+                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
-        return True
+        return False
 
     def ClampOpen(self):
-        cmd =message.NavCmd()
+        if self.IsClampOpened() == True:
+            print("clamp opended")
+            return True
+        cmd = message.NavCmd()
         key = str(uuid.uuid4())
-        cmd.key=(key)
-        act=message.Action()
-        act.type=4
+        cmd.key = (key)
+        act = message.Action()
+        act.type = 4
         cmd.actions.add().CopyFrom(act)
-        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
         time.sleep(1)
-        while self.IsNavigating()==False:
-            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        while True:
+            if self.IsClampOpened() == True:
+                return True
+            if self.IsClampRunning() == False:
+                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
-        return True
+        return False
+
     def CancelNavTask(self):
-        cmd=message.NavCmd()
-        cmd.action=3
-        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
-        while self.IsNavigating()==True:
-            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        cmd = message.NavCmd()
+        cmd.action = 3
+        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+        while self.IsNavigating() == True:
+            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
         print(" Cancel task completed!!!")
         return True
-
-
-
-
-

RobotTask.py

@@ -1,106 +0,0 @@
-from RobotData import Robot
-from dijkstra.Map import DijikstraMap
-import time
-
-
-class Task:
-    def __init__(self):
-        self.exit_ = False
-        pass
-
-    def run(self):
-        pass
-
-    def cancel(self):
-        self.exit_ = True
-
-
-class SingleNavTask(Task):
-    def __init__(self, robot: Robot, begId, targetId):
-        Task.__init__(self)
-        self.robot_ = robot
-        self.begId_ = begId
-        self.targetId_ = targetId
-
-    def findNearestNode(self):
-        if self.robot_.timedPose_.timeout() == False:
-            x = self.robot_.timedPose_.statu.x
-            y = self.robot_.timedPose_.statu.y
-            [id, pt] = DijikstraMap().findNeastNode([x, y])
-            return [id, pt]
-        return None
-
-    def Navigatting(self):
-        if self.robot_.IsNavigating() == False:
-            print("Nav beginning...")
-            self.robot_.GeneratePath(self.begId_, self.targetId_)
-            self.robot_.ExecNavtask()
-            while self.exit_ == False:
-                if self.robot_.Connected() == False:
-                    self.robot_.CancelNavTask()
-                    return False
-                if self.robot_.IsNavigating() == True:
-                    time.sleep(0.5)
-            print(" Nav completed!!!")
-            return True
-        else:
-            print(" robot is in navigatting...")
-            return False
-
-
-    def run(self):
-        return self.Navigatting()
-
-
-class SingleClampCloseTask(Task):
-    def __init__(self, robot: Robot):
-        Task.__init__(self)
-        self.robot_ = robot
-
-    def run(self):
-        if self.robot_.IsNavigating() == False:
-            self.robot_.ClampClose()
-            print(" robot clamp close...")
-            return True
-        return False
-
-
-class SingleClampOpenTask(Task):
-    def __init__(self, robot: Robot):
-        Task.__init__(self)
-        self.robot_ = robot
-
-    def run(self):
-        if self.robot_.IsNavigating() == False:
-            self.robot_.ClampOpen()
-            while self.exit_ == False:
-                if self.robot_.Connected() == False:
-                    self.robot_.CancelNavTask()
-                    return False
-                if self.robot_.IsNavigating() == True:
-                    time.sleep(0.5)
-            return True
-        return False
-
-
-class LoadedNavTask(SingleNavTask):
-    def __init__(self, robot: Robot, begId,targetId, wheelBase):
-        SingleNavTask.__init__(self,robot, begId,targetId)
-        self.wheelBase_ = wheelBase
-
-    def run(self):
-        if self.robot_.IsMainAgv() == True:
-            if self.robot_.IsMainModeStatu() == True:
-                self.robot_.wheelbase_ = self.wheelBase_
-                return self.Navigatting()
-            else:
-                print("Agv is not in main statu")
-        print(" Robot is not in LoadStatu ")
-        return False
-
-class CancelTask(Task):
-    def __init__(self,robot:Robot):
-        Task.__init__(self)
-        self.robot_=robot
-    def run(self):
-        self.robot_.CancelNavTask()

auto_test.py

@@ -3,63 +3,10 @@ import queue
 import threading
 import time
 from dijkstra.Map import singleton
-from RobotData import Robot
 import json
 
-
-class NavTask:
-    def __init__(self, robot: Robot):
-        self.robot_ = robot
-        self.eixt_ = False
-
-    def __del__(self):
-        self.eixt_ = True
-
-    def run(self):
-        pass
-
-
-class SingleNavTask(NavTask):
-    def __init__(self, robot: Robot):
-        NavTask.__init__(self,robot)
-        pass
-
-    def run(self):
-        if self.robot_.IsMainAgv() == False:
-            if self.robot_.ExecNavtask():
-                while self.eixt_ == False:
-                    if self.robot_.IsNavigating() == True:
-                        time.sleep(0.5)
-                return True
-        print("Nav Send Failed: Robot is not in LoadStatu ")
-        return False
-
-
-class LoadNavTask(NavTask):
-    def __init__(self, robot: Robot, wheelBase):
-        NavTask.__init__(self,robot)
-        self.wheelBase_ = wheelBase
-
-    def run(self):
-        if self.robot_.IsMainAgv() == True:
-            if self.robot_.IsMainModeStatu() == True:
-                self.robot_.wheelbase_ = self.wheelBase_
-                if self.robot_.ExecNavtask():
-                    while self.eixt_ == False:
-                        if self.robot_.Connected()==False:
-                            self.robot_.CancelNavTask()
-                            return False
-                        if self.robot_.IsNavigating() == True:
-                            time.sleep(0.5)
-                        else:
-                            print("Nav completed !!!")
-                            return True
-
-        print("Nav Send Failed: Robot is not in LoadStatu ")
-        return False
-
 @singleton
-class TaskExecutor(threading.Thread):
+class TestCount():
     conf_file_="./count.json"
     navCount_={}
     navTasks_ = queue.Queue()
@@ -67,8 +14,6 @@ class TaskExecutor(threading.Thread):
     running_ = False
 
     def __init__(self):
-        threading.Thread.__init__(self)
-        self.exit_ = False
         self.Loadcfg()
 
     def Loadcfg(self):
@@ -86,20 +31,23 @@ class TaskExecutor(threading.Thread):
 
     def Close(self):
         self.exit_ = True
-
-    def InputNavTask(self, task: NavTask):
-        self.navTasks_.put(task)
-
+    def loadCountAdd(self,ad=1):
+        self.navCount_["load_count"]+=ad
+        self.SaveCount()
+    def singleCountAdd(self,ad=1):
+        self.navCount_["single_count"]+=ad
+        self.SaveCount()
+
+    '''
     def run(self) -> None:
         while self.exit_ == False:
             if self.navTasks_.empty() == False:
                 task = self.navTasks_.get()
                 if task.run():
                     if isinstance(task,(LoadNavTask)):
-                        self.navCount_["load_count"]+=1
-                        print(" Load Nav +1 : %d"%(self.loadCount()))
+                        
                     if isinstance(task,(SingleNavTask)):
                         self.navCount_["single_count"]+=1
                         print(" Single Nav +1 : %d"%(self.singleCount()))
                     self.SaveCount()
-            time.sleep(0.1)
+            time.sleep(0.1)'''

count.json

@@ -1 +1 @@
-{"load_count": 267, "single_count": 411}
+{"load_count": 278, "single_count": 411}

message.proto

@@ -11,13 +11,15 @@ message LidarOdomStatu {
 message AgvStatu{
   float v=1;
   float w=2;
+  int32 clamp=3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
+  int32 clamp_other=4; //另外一节
 
 }
 
-message Speed {
+message ToAgvCmd {
   int32 H=1;  //心跳
   int32 M=2;	//模式：2整车模式，1：单车
-  int32 T=3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 4,夹持，5，松夹持，其他/未接收到：停止
+  int32 T=3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 5,夹持，6，松夹持，其他/未接收到：停止
   float V=4;  //角速度
   float W=5;  //线速度
   float L=6;  //轴距
@@ -73,5 +75,12 @@ message NavStatu
   Trajectory predict_traj = 7;
 
 }
+message RobotStatu
+{
+  float x=1;
+  float y=2;
+  float theta=3;
+  AgvStatu agvStatu=4;
+}
 
 

mqtt_async.py

@@ -1,7 +1,8 @@
 from paho.mqtt import client as mqtt_client
-
+from threading import Lock
 class MqttAsync(object):
     def __init__(self):
+        self.lock_=Lock()
         self.connected=False
     def connect(self,client_id,ip,port,user,password):
         self.client_id=client_id
@@ -20,7 +21,8 @@ class MqttAsync(object):
     def publish(self,topic,msg):
         if self.connected==False:
             print("mqtt is not connected Failed to send message to topic {topic}")
-        result = self.client.publish(topic, msg,qos=1)
+        with self.lock_:
+            result = self.client.publish(topic, msg,qos=1)
         status = result[0]
         if status == 0:
             pass
@@ -29,9 +31,11 @@ class MqttAsync(object):
 
 
     def subscribe(self,topic,callback):
-        self.client.subscribe(topic)
-        self.client.on_message = callback
+        with self.lock_:
+            self.client.subscribe(topic)
+            self.client.on_message = callback
 
     def loop_start(self):
-        self.client.loop_start()
+        with self.lock_:
+            self.client.loop_start()
 

test.py

@@ -46,7 +46,7 @@ import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
 import uuid
-from auto_test import TaskExecutor,NavTask,SingleNavTask,LoadNavTask
+
 #===============================================================================
 
 
@@ -61,13 +61,12 @@ class MainWindow(QMainWindow):
         super(MainWindow,self).__init__(parent)
         self.basic()
         self.count_frame_=ControllWidget.CountFrame()
-        TaskExecutor().start()
         self.LoadMap()
         config1={"label":"AgvMain",
                  "street_nodes":self.ui_nodes["street_nodes"],
                  "space_nodes":self.ui_nodes["space_nodes"],
                  "mqtt":["pyui1","192.168.0.70",1883,"admin","zx123456"],
-                 "subTopics":{"agv_main/agv_statu":message.AGVStatu.__name__,
+                 "subTopics":{"agv_main/agv_statu":message.RobotStatu.__name__,
                               "agv_main/nav_statu":message.NavStatu.__name__},
                  "cmdTopic":"agv_main/nav_cmd",
                  "robotColor":[0.7,0.2,0.3]}
@@ -76,7 +75,7 @@ class MainWindow(QMainWindow):
                  "street_nodes":self.ui_nodes["street_nodes"],
                  "space_nodes":self.ui_nodes["space_nodes"],
                  "mqtt":["pyui2","192.168.0.70",1883,"admin","zx123456"],
-                 "subTopics":{"agv_child/agv_statu":message.AGVStatu.__name__,
+                 "subTopics":{"agv_child/agv_statu":message.RobotStatu.__name__,
                               "agv_child/nav_statu":message.NavStatu.__name__},
                  "cmdTopic":"agv_child/nav_cmd",
                  "robotColor":[0.4,0.2,0.8]}
@@ -141,7 +140,6 @@ class MainWindow(QMainWindow):
         self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))
 
     def closeEvent(self, QCloseEvent):
-        TaskExecutor().Close()
         self.gl.close()
 
     #分割窗口

threadPool.py

@@ -1,71 +0,0 @@
-import queue
-import threading
-import time
-from dijkstra.Map import singleton
-import json
-from RobotTask import SingleClampOpenTask,SingleNavTask,SingleClampCloseTask,LoadedNavTask
-
-@singleton
-class ThreadPool:
-    conf_file_="./count.json"
-    navCount_={}
-    def __init__(self):
-        self.task_queue_=queue.Queue()
-        self.running_queue_=queue.Queue()
-        self.exit_=False
-        self.running_=0
-        self.threads_=[]
-        for i in range(2):
-            th=threading.Thread(target=self.working)
-            th.start()
-            self.threads_.append(th)
-        self.Loadcfg()
-
-    def Loadcfg(self):
-        with open(self.conf_file_,'r',encoding='utf-8') as fp:
-            self.navCount_=json.load(fp)
-    def singleCount(self):
-        return self.navCount_["single_count"]
-    def loadCount(self):
-        return self.navCount_["load_count"]
-
-    def SaveCount(self):
-        with open(self.conf_file_,'w',encoding='utf-8') as fp:
-            json.dump(self.navCount_,fp)
-            return map
-
-    def Close(self):
-        self.exit_ = True
-
-    '''def __del__(self):
-        self.exit_=True
-        for thread in self.threads_:
-            thread.join()'''
-    def InputTask(self,task):
-        self.task_queue_.put(task)
-    def WaitTaskCompleted(self):
-        while self.exit_==False and self.running_queue_.empty()==False:
-            time.sleep(0.1)
-    def working(self):
-        while self.exit_==False:
-            try:
-                if self.task_queue_.qsize()==0:
-                    time.sleep(0.1)
-                    continue
-                task=self.task_queue_.get_nowait()
-                self.running_queue_.put(1)  #增加正在执行任务数
-                if task.run():
-                    pass
-                    '''if isinstance(task,(LoadedNavTask)):
-                        self.navCount_["load_count"]+=1
-                        print(" Load Nav +1 : %d"%(self.loadCount()))
-                    if isinstance(task,(SingleNavTask)):
-                        self.navCount_["single_count"]+=1
-                        print(" Single Nav +1 : %d"%(self.singleCount()))
-                    self.SaveCount()'''
-                else:
-                    pass
-                self.running_queue_.get()
-            except queue.Empty:
-                pass
-            time.sleep(0.1)