....

ControllWidget.py

@@ -7,7 +7,7 @@ from PyQt5.QtWidgets import QWidget, QFrame, QLabel, QLineEdit, QPushButton, QCo
 from PyQt5.QtGui import QPixmap, QPainter, QResizeEvent, QCloseEvent, QPaintEvent, QFont
 from PyQt5.QtCore import QSize, QTimer, Qt
 
-import auto_test
+import Count
 from RobotData import Robot
 import dijkstra.Map as mp
 
@@ -79,12 +79,12 @@ class ControlFrame(QFrame):
 
         self.btnSend = QPushButton(self)
         self.btnSend.setGeometry(150, 140, 100, 40)
-        self.btnSend.setText("  启动导航")
+        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.setText("  取消")
         self.btnCancel.clicked.connect(self.btnCancelClick)
 
         self.begQc.addItems(self.all_nodes_)
@@ -96,7 +96,7 @@ class ControlFrame(QFrame):
         self.endSpaceQc.activated.connect(self.endSpaceQCChanged)
 
         self.posestatic = QLabel(self)
-        self.posestatic.setGeometry(260, 5, 100, 90)
+        self.posestatic.setGeometry(260, 20, 100, 90)
 
         self.LedLabel = QLabel(self)
         self.LedLabel.setGeometry(80, 160, 50, 50)
@@ -104,12 +104,12 @@ class ControlFrame(QFrame):
         # 发送轴距
         self.wheelbasestatic = QLabel(self)
         self.wheelbasestatic.setText("轴距:")
-        self.wheelbasestatic.setGeometry(260, 90, 40, 30)
+        self.wheelbasestatic.setGeometry(260, 100, 40, 30)
         self.WheelBaseEdit = QLineEdit(self)
         self.WheelBaseEdit.setText("3.3")
-        self.WheelBaseEdit.setGeometry(300, 90, 50, 30)
+        self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
-        self.btnModelCheck = QCheckBox("双车模式", self)
+        self.btnModelCheck = QCheckBox("整体协调", self)
         self.btnModelCheck.setGeometry(360, 130, 100, 40)
         self.btnModelCheck.clicked.connect(self.MainAgvchangecb)
 
@@ -127,6 +127,11 @@ class ControlFrame(QFrame):
         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)
+
     def Update(self):
 
         if self.robot_.timedRobotStatu_.timeout() == False:
@@ -134,6 +139,9 @@ class ControlFrame(QFrame):
             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.DijikstraMap().findNeastNode([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"
@@ -187,7 +195,6 @@ class ControlFrame(QFrame):
         if self.robot_.IsMainModeStatu():
             self.btnAutoDirect.setCheckState(Qt.Unchecked)
 
-
     def AutoCheck(self):
         if self.btnAuto.checkState()==Qt.Checked:
             self.robot_.autoTest_=True
@@ -247,7 +254,6 @@ class CountFrame(QFrame):
         QFrame.__init__(self)
 
         self.InitUI()
-
         self.timer_ = QTimer()
         self.timer_.timeout.connect(self.UpdateUI)
         self.timer_.start(200)
@@ -262,7 +268,7 @@ class CountFrame(QFrame):
         self.static2.setGeometry(5, 40, 200, 30)
 
     def UpdateUI(self):
-        load_str = "重载导航次数 : %d" % (auto_test.TestCount().loadCount())
-        single_str = "单车导航次数 : %d" % (auto_test.TestCount().singleCount())
+        load_str = "重载导航次数 : %d" % (Count.TestCount().loadCount())
+        single_str = "单车导航次数 : %d" % (Count.TestCount().singleCount())
         self.static1.setText(load_str)
         self.static2.setText(single_str)

PyGLWidget.py

@@ -88,8 +88,6 @@ class PyGLWidget(QtOpenGL.QGLWidget):
         self.reset_view()
         self.translate([-10,-10,-25.])
 
-
-
     def resizeGL(self, width, height):
         glViewport(0, 0, width, height );
         self.set_projection( self.near_, self.far_, self.fovy_ );

RobotData.py

@@ -5,9 +5,9 @@ from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
 from dijkstra.Map import DijikstraMap,SpaceNode,StreetNode
 import uuid
-from auto_test import TestCount
 import random
 import math
+import Count
 
 class ActType(enum.Enum):
     eReady=0
@@ -88,10 +88,10 @@ class Robot(MqttAsync):
         self.robotColor_ = robotColor
 
     def ResetPose(self, agv: message.RobotStatu):
-        self.timedRobotStatu_ = TimeStatu(agv, 0.5)
+        self.timedRobotStatu_ = TimeStatu(agv, 2)
 
     def ResetNavStatu(self, statu: message.NavStatu):
-        self.timedNavStatu_ = TimeStatu(statu, 0.5)
+        self.timedNavStatu_ = TimeStatu(statu, 2)
 
     def on_message(self, client, userdata, msg):
         topic = msg.topic
@@ -172,18 +172,14 @@ class Robot(MqttAsync):
                 return False'''
             time.sleep(0.5)
         print(" Nav completed!!!")
+
+        if self.IsMainModeStatu():
+            Count.TestCount().loadCountAdd()
+        else:
+            Count.TestCount().singleCountAdd()
+
         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
@@ -261,14 +257,6 @@ class Robot(MqttAsync):
         limitInOutV.min=0.03
         limitInOutV.max=0.5
 
-
-
-        '''
-        newAction.NodeVelocityLimit.CopyFrom(limitMPC_v)
-                newAction.NodeAngularLimit.CopyFrom(limitRotate)
-                newAction.adjustVelocitylimit.CopyFrom(limAdjustV)
-                newAction.adjustHorizonLimit.CopyFrom(limitAdjustH)
-        '''
         actions=self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
             [type,nodes]=action
@@ -331,7 +319,7 @@ class Robot(MqttAsync):
                 limAdjustV=message.SpeedLimit()
                 limitAdjustH=message.SpeedLimit()
                 limitMPC_v.min=0.03
-                limitMPC_v.max=1.2
+                limitMPC_v.max=1.5
                 limitRotate.min=3*math.pi/180.0
                 limitRotate.max=25*math.pi/180.0
                 limAdjustV.min=0.03
@@ -371,11 +359,11 @@ class Robot(MqttAsync):
                     if count==size-1:
                         accuracy.x=0.02
                         accuracy.y=0.02
-                        accuracy.theta=0.5*math.pi/(180.0)
+                        accuracy.theta=3*math.pi/(180.0)
                     else:
                         accuracy.x=0.05
                         accuracy.y=0.1
-                        accuracy.theta=3*math.pi/180.0
+                        accuracy.theta=5*math.pi/180.0
                     count+=1
 
                     pathNode=message.PathNode()
@@ -472,13 +460,6 @@ class Robot(MqttAsync):
 
         return actions
 
-
-
-
-
-
-
-
     def SwitchMoveMode(self, mode, wheelbase):
         if self.IsMainAgv() == False:
             print(" this agv is single can not switch mode")
@@ -517,7 +498,6 @@ class Robot(MqttAsync):
                 return self.timedRobotStatu_.statu.agvStatu.clamp == 1
 
         return False
-
     def IsClampRunning(self):
         if self.timedRobotStatu_.timeout() == False:
             if self.IsMainModeStatu():

auto_test.py

@@ -1,39 +0,0 @@
-import math
-import queue
-import threading
-import time
-from dijkstra.Map import singleton
-import json
-
-@singleton
-class TestCount():
-    conf_file_="./count.json"
-    navCount_={}
-    navTasks_ = queue.Queue()
-
-    running_ = False
-
-    def __init__(self):
-        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 loadCountAdd(self,ad=1):
-        self.navCount_["load_count"]+=ad
-        self.SaveCount()
-    def singleCountAdd(self,ad=1):
-        self.navCount_["single_count"]+=ad
-        self.SaveCount()

count.json

@@ -1 +1 @@
-{"load_count": 364, "single_count": 411}
+{"load_count": 381, "single_count": 493}