ControllWidget.py

@@ -13,6 +13,8 @@ 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)
@@ -31,15 +33,17 @@ class ControlFrame(QFrame):
         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)
 
-        rpc=config['rpc']
-        self.robot_ = Robot(rpc,self.label_)
+        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)
@@ -110,7 +114,7 @@ class ControlFrame(QFrame):
         self.wheelbasestatic.setText("轴距:")
         self.wheelbasestatic.setGeometry(260, 100, 40, 30)
         self.WheelBaseEdit = QLineEdit(self)
-        self.WheelBaseEdit.setText("0")
+        self.WheelBaseEdit.setText(str(self.default_wheel_base))
         self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
         self.btnModelCheck = QCheckBox("整体协调", self)
@@ -140,15 +144,38 @@ class ControlFrame(QFrame):
         self.btnAutoBegChanged.setGeometry(260, 10, 100, 20)
         self.btnAutoBegChanged.setCheckState(Qt.Checked)
 
-    def Update(self):
+        # 选择地图
+        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])
+            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;\
@@ -195,8 +222,8 @@ class ControlFrame(QFrame):
             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_)
+            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:
@@ -211,10 +238,11 @@ class ControlFrame(QFrame):
         #     self.btnAutoDirect.setCheckState(Qt.Unchecked)
 
     def AutoCheck(self):
-        if self.btnAuto.checkState()==Qt.Checked:
-            self.robot_.autoTest_=True
+        if self.btnAuto.checkState() == Qt.Checked:
+            self.robot_.autoTest_ = True
         else:
-            self.robot_.autoTest_=False
+            self.robot_.autoTest_ = False
+
     def ClampClick(self):
         if self.robot_.IsClampClosed() == False:
             self.threadPool_.submit(self.robot_.ClampClose)
@@ -239,44 +267,60 @@ class ControlFrame(QFrame):
         id1 = self.begQc.currentText()
         id2 = self.endStreetQc.currentText()
         if not id1 == "------" and not id2 == "------":
-            self.robot_.GeneratePath(id1, id2,"Base")
-            self.begId_=id1
-            self.targetId_=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,"Base")
-            self.begId_=id1
-            self.targetId_=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.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:
+            autoDirect = False
+            if self.btnAutoDirect.checkState() == Qt.Checked:
                 print("自由方向")
-                autoDirect=True
-            self.robot_.GeneratePath(self.begId_, self.targetId_,"Base")
+                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.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)
@@ -307,8 +351,8 @@ class CountFrame(QFrame):
         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")

ManualOperationWidget.py

@@ -17,6 +17,8 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
         super().__init__()
         self.setupUi(self)
 
+        self.speed_lineEdit.setText("0.5")
+
         self.robot_dict = robot_dict
         self.selected_robot_name = None
         self.current_operation = ManualOperationType.eReady
@@ -179,7 +181,7 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
             speed = 0.0
             if self.speed_lineEdit_check():
                 speed = float(self.speed_lineEdit.text())
-            speed = min(speed, 0.5)
+            speed = min(speed, 2.0)
             self.speed_lineEdit.setText(str(speed))
             self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc, speed=speed)
             self.step_acc += 1

PyGLWidget.py

@@ -86,7 +86,7 @@ class PyGLWidget(QtOpenGL.QGLWidget):
         glClearColor(0.6, 0.6, 0.6, 0.5)
         glEnable(GL_DEPTH_TEST)
         self.reset_view()
-        self.translate([-10,-10,-25.])
+        self.translate([0,11.5,-35.])
 
     def resizeGL(self, width, height):
         glViewport(0, 0, width, height );

RobotData.py

@@ -50,6 +50,9 @@ class Robot(MqttAsync):
         self.currentNavPathNodes_ = None
         self.currentNavPath_ = None
 
+        # 当前正在执行的流程默认0，存车1， 取车2
+        self.current_sequence_: int = 0
+
         self.pathColor_ = [1, 1, 0]
         self.robotColor_ = [0.7, 0.2, 0.3]  # agv当前颜色
         self.name_ = name
@@ -60,7 +63,7 @@ class Robot(MqttAsync):
         self.autoTest_ = False
 
         self.l_ = 0.8  # 轮长
-        self.L_ = 1.3  # 轴距
+        self.L_ = 2.9  # 轴距
         self.W_ = 2.5  # 宽
         self.heat_ = 0  # 心跳
 
@@ -152,7 +155,7 @@ class Robot(MqttAsync):
         return True'''
 
     def GeneratePath(self, begID, endID, mapName):
-        self.currentNavPathNodes_ = MapManager().GetShortestPath(mapName,begID, endID)
+        self.currentNavPathNodes_ = MapManager().GetShortestPath(mapName, begID, endID)
 
         # if task_type == "DoubleAGV":
         #     print("AdjustPath------------------")
@@ -185,13 +188,18 @@ class Robot(MqttAsync):
     阻塞直到导航完成
     '''
 
-    def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type="None"):
+    def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type: int = 0):
         if wheelbase < 2.4 or wheelbase > 3.2:
-            return print("wheelbase is out of range!\n")
+            print("wheelbase is out of range!\n")
+            return False
         print("nav")
 
+        self.SetSequence(task_type)
         # self.ExecPathNodes(autoDirect)
-        self.ExecNavtask(autoDirect, wheelbase)
+        if self.ExecNavtask(autoDirect, wheelbase) == False:
+            self.ResetSequence()
+            return False
+        self.ResetSequence()
 
         if self.IsMainModeStatu():
             Count.TestCount().loadCountAdd()
@@ -262,6 +270,7 @@ class Robot(MqttAsync):
         key = str(uuid.uuid4())
         cmd.key = key
         cmd.action = 0
+        cmd.sequence = self.current_sequence_
 
         actions = self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
@@ -306,9 +315,10 @@ class Robot(MqttAsync):
         print("client received: ", response)
         if not response.ret == 0:
             print("nav failed :%s" % (response.info))
+            return False
         else:
             print("nav completed !!!")
-        return True
+            return True
 
     def CreateNavPathNodesAction(self, path, autoDirect):
         # if path is not None and self.timedRobotStatu_.timeout() == False:
@@ -327,6 +337,7 @@ class Robot(MqttAsync):
                     pathNode = message.PathNode()
                     pathNode.x = node.x_
                     pathNode.y = node.y_
+                    pathNode.id = node.id_
 
                     newAction.type = 4
                     if autoDirect:
@@ -458,7 +469,7 @@ class Robot(MqttAsync):
         channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
-        return False
+        return True
 
     def IsClampClosed(self):
         if self.timedRobotStatu_.timeout() == False:
@@ -639,7 +650,7 @@ class Robot(MqttAsync):
                 cmd.W1 = -speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
             if current_operation == ManualOperationType.eForwardMove:
                 cmd.T1 = 3
-                cmd.V1= speed * sigmoid(step_acc, 0, 1)
+                cmd.V1 = speed * sigmoid(step_acc, 0, 1)
                 cmd.W1 = 0
             if current_operation == ManualOperationType.eBackwardMove:
                 cmd.T1 = 3
@@ -770,26 +781,26 @@ class Robot(MqttAsync):
                 if self.manual_status == ManualOperationType.eCounterclockwiseRotate:
                     cmd.T1 = 1
                     cmd.V1 = 0
-                    cmd.W1 = speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                    cmd.W1 = speed_w / 180 * math.pi * (step_sum - step_acc) / step_sum
                 if self.manual_status == ManualOperationType.eClockwiseRotate:
                     cmd.T1 = 1
                     cmd.V1 = 0
-                    cmd.W1 = -speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                    cmd.W1 = -speed_w / 180 * math.pi * (step_sum - step_acc) / step_sum
                 if self.manual_status == ManualOperationType.eForwardMove:
                     cmd.T1 = 3
-                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eBackwardMove:
                     cmd.T1 = 3
-                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = -speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eLeftMove:
                     cmd.T1 = 2
-                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eRightMove:
                     cmd.T1 = 2
-                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = -speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 cmd.V2 = cmd.V1
                 cmd.V3 = cmd.V1
@@ -815,44 +826,47 @@ class Robot(MqttAsync):
         print("Manual_Unit_SlowlyStop completed.")
         return True
 
-    def GenerateActionMPC(self, beg_name, end_name, autoDirect, wheelbase=0, task_type="None"):
-        self.GeneratePath(beg_name, end_name, task_type=task_type)
-        actions = list()
-
-        split_path = self.SplitPath(self.currentNavPathNodes_)
-        for action in split_path:
-            [type, nodes] = action
-            if type == "input":
-                [street, space] = nodes
-                protoSpace = self.generateProtoNode(space)
-                protoStreet = self.generateProtoNode(street)
-                act = message.NewAction()
-                act.type = 1
-                act.wheelbase = wheelbase
-                act.spaceNode.CopyFrom(protoSpace)
-                act.streetNode.CopyFrom(protoStreet)
-
-                actions.append(act)
-            if type == "output":
-                [space, street] = nodes
-                protoSpace = self.generateProtoNode(space)
-                protoStreet = self.generateProtoNode(street)
-                act = message.NewAction()
-                act.type = 2
-                act.wheelbase = wheelbase
-                act.spaceNode.CopyFrom(protoSpace)
-                act.streetNode.CopyFrom(protoStreet)
-
-                actions.append(act)
-            if type == "nav":
-                act = self.CreateNavPathNodesAction(nodes, autoDirect)
-                if act is None:
-                    print("Nav cmd is None")
-                    return False
-                else:
-                    actions.append(act)
-
-        return actions
-
-
-
+    # def GenerateActionMPC(self, beg_name, end_name, autoDirect, wheelbase=0, task_type="None"):
+    #     self.GeneratePath(beg_name, end_name)
+    #     actions = list()
+    #
+    #     split_path = self.SplitPath(self.currentNavPathNodes_)
+    #     for action in split_path:
+    #         [type, nodes] = action
+    #         if type == "input":
+    #             [street, space] = nodes
+    #             protoSpace = self.generateProtoNode(space)
+    #             protoStreet = self.generateProtoNode(street)
+    #             act = message.NewAction()
+    #             act.type = 1
+    #             act.wheelbase = wheelbase
+    #             act.spaceNode.CopyFrom(protoSpace)
+    #             act.streetNode.CopyFrom(protoStreet)
+    #
+    #             actions.append(act)
+    #         if type == "output":
+    #             [space, street] = nodes
+    #             protoSpace = self.generateProtoNode(space)
+    #             protoStreet = self.generateProtoNode(street)
+    #             act = message.NewAction()
+    #             act.type = 2
+    #             act.wheelbase = wheelbase
+    #             act.spaceNode.CopyFrom(protoSpace)
+    #             act.streetNode.CopyFrom(protoStreet)
+    #
+    #             actions.append(act)
+    #         if type == "nav":
+    #             act = self.CreateNavPathNodesAction(nodes, autoDirect)
+    #             if act is None:
+    #                 print("Nav cmd is None")
+    #                 return False
+    #             else:
+    #                 actions.append(act)
+    #
+    #     return actions
+
+    def SetSequence(self, s_type: int):
+        self.current_sequence_ = s_type
+
+    def ResetSequence(self):
+        self.current_sequence_ = 0

TaskTable.py

@@ -0,0 +1,56 @@
+import threading
+from google.protobuf import message as _message
+
+
+class TaskTable(object):
+    def __init__(self):
+        self.taskMap = {}
+        self.lock = threading.Lock()
+
+    def PushTask(self, primary_key, msg: _message.Message):
+        if len(self.taskMap.items()) > 0:
+            return False
+        else:
+            self.lock.acquire()
+            self.taskMap[primary_key] = msg
+            self.lock.release()
+            return True
+
+    def UpdateResult(self, primary_key, ret):
+        if primary_key in self.taskMap:
+            taskTable = self.taskMap[primary_key]
+            self.lock.acquire()
+            self.taskMap[primary_key] = [taskTable, ret]
+            self.lock.release()
+            return True
+
+        else:
+            return False
+
+    def GetTask(self):
+        if len(self.taskMap.items()) == 0:
+            return None
+        else:
+            for item in self.taskMap.items():
+                key, task = item
+                if isinstance(task, (_message.Message)):
+                    return task
+                else:
+                    return None
+
+    def GetResult(self, primary_key):
+        if len(self.taskMap.items()) == 0:
+            return None
+        else:
+            if primary_key in self.taskMap:
+                result = self.taskMap[primary_key]
+                if isinstance(result, (list)):
+                    ret = result[1]
+                else:
+                    return None
+                self.lock.acquire()
+                self.taskMap.clear()
+                self.lock.release()
+                return ret
+            else:
+                return None

count.json

@@ -1 +1 @@
-{"load_count": 863, "single_count": 2575}
+{"load_count": 932, "single_count": 2900}

dijkstra/Map.py

@@ -1,6 +1,6 @@
 import math
 from copy import deepcopy
-
+import json
 from dijkstra.dijkstra_algorithm import Graph
 import numpy as np
 import scipy.spatial as spt
@@ -60,7 +60,7 @@ map 采用单例
 
 class DijikstraMap(object):
     def __init__(self):
-        self.nodes_ = {} #dict ,{id: StreetNode/SpaceNode}
+        self.nodes_ = {}  # dict ,{id: StreetNode/SpaceNode}
         self.graph_ = Graph()
 
     def GetVertex(self, id):
@@ -288,39 +288,75 @@ class DijikstraMap(object):
 @singleton
 class MapManager(object):
     def __init__(self):
-
-        self.maps={}
-        self.maps["Base"]=DijikstraMap()
-        self.maps["Main"]=DijikstraMap()
-        self.maps["Front"]=DijikstraMap()
-        self.maps["Back"]=DijikstraMap()
-
-    def GetVertex(self, mapName,id):
+        self.default_wheel_base = 2.6
+        self.maps = {}
+        self.maps["Base"] = DijikstraMap()
+        self.maps["Front"] = DijikstraMap()
+        self.maps["Back"] = DijikstraMap()
+
+    def readJson(self, file):
+        with open(file, 'r', encoding='utf-8') as fp:
+            map = json.load(fp)
+            return map
+
+    def LoadJson(self, file):
+
+        self.maps["Base"] = DijikstraMap()
+        self.maps["Front"] = DijikstraMap()
+        self.maps["Back"] = DijikstraMap()
+
+        map = self.readJson(file)
+
+        for node in map['street_nodes'].items():
+            [id, point] = node
+            street_node = StreetNode(id, point[0], point[1])
+            self.maps["Base"].AddVertex(street_node)
+
+        for node in map['space_nodes'].items():
+            [id, point] = node
+            [x, y, yaw] = point
+            space_node = SpaceNode(id, point[0], point[1], yaw)
+            self.maps["Base"].AddVertex(space_node)
+
+        for road in map['roads'].items():
+            [roadName, points] = road
+            for pt1 in points:
+                for pt2 in points:
+                    if not pt1 == pt2:
+                        self.maps["Base"].AddEdge(pt1, pt2)
+        self.ResetMap("Front", 0, self.default_wheel_base/2)
+        self.ResetMap("Back", 0, -self.default_wheel_base/2)
+
+    def ResetMap(self, mapName, dx, dy):
+        self.maps[mapName] = deepcopy(self.maps["Base"])
+        for id in self.maps[mapName].nodes_:
+            if id.find("R_") >= 0:
+                node = self.maps["Base"].GetVertex(id)
+                self.maps[mapName].ResetVertexValue(StreetNode(node.id_, node.x_ + dx, node.y_ + dy))
+        return self.maps[mapName]
+
+    def GetVertex(self, mapName, id):
         return self.maps[mapName].GetVertex(id)
-    def AddVertex(self,mapName, node):
+
+    def AddVertex(self, mapName, node):
         self.maps[mapName].AddVertex(node)
 
-    def AddEdge(self,mapName, id1, id2, direct=False):
+    def AddEdge(self, mapName, id1, id2, direct=False):
         self.maps[mapName].AddEdge(id1, id2, direct)
 
-    def AddVertex_t(self, node):
-        self.map_t.AddVertex(node)
-
-    def Reset(self,mapName):
+    def Reset(self, mapName):
         self.maps[mapName] = DijikstraMap()
 
-    def VertexDict(self,mapName):
+    def VertexDict(self, mapName):
         return self.maps[mapName].VertexDict()
 
-    def Edges(self,mapName):
+    def Edges(self, mapName):
         return self.maps[mapName].Edges()
 
-
-    def findNeastNode(self, mapName,pt):
+    def findNeastNode(self, mapName, pt):
         return self.maps[mapName].findNeastNode(pt)
 
-    def GetShortestPath(self,mapName, beg, end):
+    def GetShortestPath(self, mapName, beg, end):
         print("beg: ", self.maps[mapName].graph_.points[beg])
         print("end: ", self.maps[mapName].graph_.points[end])
         return self.maps[mapName].GetShortestPath(beg, end)
-

dijkstra/dijkstra_algorithm.py

@@ -37,8 +37,26 @@ class Graph:
         """
         point : [x, y]
         """
+
         self.points[name] = point
 
+        for i in range(len(self.graph_edges)):
+            name1,name2,_=self.graph_edges[i]
+            if name1==name or name2==name:
+                pt1=self.points[name1]
+                pt2=self.points[name2]
+                distance = math.sqrt(math.pow(pt1[0] - pt2[0], 2) + math.pow(pt1[1] - pt2[1], 2)) + 0.000001
+                self.graph_edges[i]=[name1,name2,distance]
+                self.nodes.update([name1, name2])
+
+                for item in self.adjacency_list[name1]:
+                    neighbor,_=item
+                    if neighbor==name2:
+                        self.adjacency_list[name1].discard(item)
+                        self.adjacency_list[name1].add((neighbor,distance))
+                        break
+
+
     def AddEdge(self, name1, name2):
         if self.points.get(name1) == None or self.points.get(name2) == None:
             print("node :%s or %s not exis" % (name1, name2))
@@ -54,6 +72,7 @@ class Graph:
             self.adjacency_list[edge[0]].add((edge[1], edge[2]))
         return True
 
+
     def __getitem__(self, item):
         return self.points[item]
 

map.json

@@ -1,112 +1,44 @@
 {
   "street_nodes": {
-    "F_Input_R145": [
-      -8.57, -5.61
-    ],
-    "C_Input_R145": [
-      -8.57, -7.16
-    ],
-    "B_Input_R145": [
-      -8.57, -8.46
-    ],
-    "F_Input_R147": [
-      -10.98, -5.61
-    ],
-    "C_Input_R147": [
-      -10.98, -7.16
-    ],
-    "B_Input_R147": [
-      -10.98, -8.46
-    ],
-     "F_OutLink": [
-      -0.32, -5.61
-    ],
-    "C_OutLink": [
-      -0.32, -7.16
-    ],
-     "B_OutLink": [
-      -0.32, -8.46
-    ],
-    "F_Input_R65": [
-      -16.665, -5.61
-    ],
-    "C_Input_R65": [
-      -16.665, -7.16
-    ],
-    "B_Input_R65": [
-      -16.665, -8.46
-    ]
+    "R_65": [-16.665, -7.16],
+    "R_124": [17.87,-23.92],
+    "R_131": [-14.29,-23.92],
+    "R_132": [-14.20,-23.92],
+    "R_136": [-19.00,-23.92],
+    "R_137": [-22.39,-23.92],
+    "R_145": [-8.57, -7.16],
+    "R_147": [-10.97, -7.16],
+    "R_1101": [-0.32, -7.16],
+    "R_1102": [-0.32,-23.92]
   },
 
   "space_nodes": {
-    "S147": [
-      -10.97, -13.48, -90
-    ],
-    "S1100": [
-      -0.32, 1.0, 90
-    ],
-    "S65": [
-      -16.665, -0.980, 89.15
-    ],
-    "S145": [
-      -8.57, -13.48, -89.15
-    ]
+    "S_65": [-16.665, -0.980, 89.15],
+    "S_124": [17.88,-30.40,-90],
+    "S_131": [-14.27,-17.66,-90],
+    "S_132": [-14.23,-30.18,-90],
+    "S_136": [-19.0,-30.22,-90],
+    "S_137": [-22.368,-17.66,90.55],
+    "S_145": [-8.57, -13.48, -89.15],
+    "S_147": [-10.96, -13.48, -90],
+    "S_1101": [-0.32, 1.0, 90],
+    "S_1102": [-0.32,-31.92,-90]
   },
-  "roads": {
-    "C_road_Input": [
-      "C_Input_R145",
-      "C_Input_R147",
-      "C_Input_R65",
-      "C_OutLink"
-    ],
-    "F_road_Input": [
-      "F_Input_R145",
-      "F_Input_R147",
-      "F_Input_R65",
-      "F_OutLink"
-    ],
-    "B_road_Input": [
-      "B_Input_R145",
-      "B_Input_R147",
-      "B_Input_R65",
-      "B_OutLink"
-    ],
-    "C_road_145": [
-      "S145",
-      "C_Input_R145"
-    ],
-    "F_road_145": [
-      "S145",
-      "F_Input_R145"
-    ],
-    "B_road_145": [
-      "S145",
-      "B_Input_R145"
-    ],
-    "C_road_147": [
-      "S147",
-      "C_Input_R147"
-    ],
-    "F_road_147": [
-      "S147",
-      "F_Input_R147"
-    ],
-    "B_road_147": [
-      "S147",
-      "B_Input_R147"
-    ],
 
-    "C_road_65": [
-      "S65",
-      "C_Input_R65"
-    ],
-    "F_road_65": [
-      "S65",
-      "F_Input_R65"
-    ],
-    "B_road_65": [
-      "S65",
-      "B_Input_R65"
-    ]
+  "roads": {
+    "Row1": ["R_65", "R_147", "R_145", "R_1101"],
+    "Row2": ["R_124","R_131", "R_137", "R_1102", "R_132","R_136"],
+    "Col1": ["R_1101", "R_1102"
+    ],
+    "R65_S65": ["R_65", "S_65"],
+    "R124_S124": ["R_124", "S_124"],
+    "R131_S131": ["R_131","S_131"],
+    "R132_S132": ["R_132", "S_132"],
+    "R136_S136": ["R_136", "S_136"],
+    "R137_S137": ["R_137", "S_137"],
+    "R145_S145": ["S_145", "R_145"],
+    "R147_S147": ["R_147", "S_147"],
+    "R1101_S1101": ["R_1101", "S_1101"],
+    "S1102_R1102": ["S_1102", "R_1102"]
   }
 }

message.proto

@@ -15,6 +15,9 @@ message AgvStatu{
   int32 clamp_other=4; //另外一节
   int32 lifter=5;   //提升机构  0中间状态，1上升到位， 2下降到位
   int32 lifter_other = 6;     //另外一节
+  repeated int32 zcb = 7; //载车板
+  int32 door = 8; // 门控
+
 }
 
 message ToAgvCmd {
@@ -32,7 +35,8 @@ message ToAgvCmd {
     float D1 = 12; //距目标点距离
     float Y1=13;    //前车小w
     float Y2=14;    //后车小w
-    int32 end = 15;
+    int32 CL =15;
+    int32 end = 16;
 }
 
 message Pose2d
@@ -49,7 +53,7 @@ message PathNode  //导航路径点及精度
   float l=3;    //目标点旋转矩形方程，代表精度范围
   float w=4;
   float theta=5;
-  string id=6;
+  optional string id=6;
 }
 
 message Trajectory
@@ -75,6 +79,8 @@ message NavCmd
   int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
   string key=2;
   repeated NewAction newActions=5;
+  optional int32 sequence = 6; // 0：无， 1：存车， 2：取车
+
 }
 message NavResponse{
   int32 ret=1;

mytool/RabbitMq_helper/async_communication.py

@@ -90,8 +90,10 @@ class RabbitAsyncCommunicator(threading.Thread):
         async with queue.iterator() as queue_iter:
             async for message in queue_iter:
                 if not callback==None:
-                    self._pool.submit(callback,message.body.decode())
-                    await message.ack()
+                    if await callback(message.body.decode())==True:
+                        print("ask")
+                        await message.ack()
+                        print("ask over")
                 if self._closing==True:
                     return
     async def recv_statu(self,ex_name,key,ttl=200):

test.py

@@ -32,6 +32,7 @@
 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #
 # ===============================================================================
+import asyncio
 import math
 import threading
 import time
@@ -55,6 +56,10 @@ from mytool.txt_helper.txt_operation import TXTOperation
 from custom_define import RobotName
 from mytool.RabbitMq_helper import async_communication as rabitmq
 
+import concurrent.futures
+import TaskTable
+from queue import Queue
+
 
 # ===============================================================================
 
@@ -65,14 +70,28 @@ class MainWindow(QMainWindow):
     ui_nodes = {}
     ui_nodes["street_nodes"] = []
     ui_nodes["space_nodes"] = []
+    taskTable = TaskTable.TaskTable()
 
     def __init__(self, parent=None):
         super(MainWindow, self).__init__(parent)
         self.basic()
+        self.showMaximized()
         self.count_frame_ = ControllWidget.CountFrame()
-        self.loadJsonMap("./map.json")
-        self.isLocalTest = False
-        self.isAutoDispatchOnline = True
+        self.isLocalTest = True
+        self.isAutoDispatchOnline = False
+
+        self.Cancel_ = False
+
+        task_thread = threading.Thread(target=self.execTask)
+        task_thread.start()
+
+        self.mapManager.LoadJson("./map.json")
+
+        for node_id in self.mapManager.VertexDict("Base"):
+            if node_id.find("S_") >= 0:
+                self.ui_nodes["space_nodes"].append(node_id)
+            if node_id.find("R_") >= 0:
+                self.ui_nodes["street_nodes"].append(node_id)
 
         rpc_1 = "192.168.0.70:9090"
         mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
@@ -104,6 +123,8 @@ class MainWindow(QMainWindow):
                                  "agv_child/nav_statu": message.NavStatu.__name__},
                    "cmdTopic": "agv_child/nav_cmd",
                    "robotColor": [0.4, 0.2, 0.8]}
+
+        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
         self.Controller1 = ControllWidget.ControlFrame(config1)
         self.Controller2 = ControllWidget.ControlFrame(config2)
         robot_dict = {self.Controller1.robot_.name_: self.Controller1.robot_,
@@ -115,6 +136,7 @@ class MainWindow(QMainWindow):
 
         self.gl.SetRobot1Instance(self.Controller1.robot_)
         self.gl.SetRobot2Instance(self.Controller2.robot_)
+        self.gl.SetMaps(self.mapManager.maps)
 
         self.timer_ = QTimer()
         self.timer_.timeout.connect(self.Update)
@@ -130,7 +152,21 @@ class MainWindow(QMainWindow):
             self.g_rabbitmq.setDaemon(True)  # 守护线程随主线程结束
             self.g_rabbitmq.start()
 
-    def recv_park_request(self, msg):
+    def execTask(self):
+        while self.Cancel_ == False:
+            task = self.taskTable.GetTask()
+            if task != None:
+                if isinstance(task, (message.park_table)):
+                    print(" exec park task :%s" % (task.primary_key))
+                    ret = self.AutoParkTask(task)
+                    self.taskTable.UpdateResult(task.primary_key, ret)
+                if isinstance(task, (message.pick_table)):
+                    print(" exec pick task :%s" % (task.primary_key))
+                    ret = self.AutoPickTask(task)
+                    self.taskTable.UpdateResult(task.primary_key, ret)
+            time.sleep(0.5)
+
+    async def recv_park_request(self, msg):
         print("Recv_park_request------------------\n", msg)
 
         park_table = message.park_table()
@@ -138,10 +174,13 @@ class MainWindow(QMainWindow):
             tf.Parse(msg, park_table)
         except Exception as e:
             print("Parse  error\n")
+            return False
+
         if self.isAutoDispatchOnline:
-            self.AutoParkTask(park_table)
+            return await self.PushParkTask(park_table)
+        return False
 
-    def recv_pick_request(self, msg):
+    async def recv_pick_request(self, msg):
         print("Recv_pick_request------------------\n", msg)
         pick_table = message.pick_table()
 
@@ -149,162 +188,36 @@ class MainWindow(QMainWindow):
             tf.Parse(msg, pick_table)
         except Exception as e:
             print("Parse  error\n")
-        if self.isAutoDispatchOnline:
-            self.AutoPickTask(pick_table)
-
-    def ReLoadMap(self, mapName, file):
-        self.mapManager.Reset(mapName)
-        map = self.readJson(file)
-        flag = "C_"
-        if mapName == "Front":
-            flag = "F_"
-        elif mapName == "Back":
-            flag = "B_"
-        elif mapName == "Main":
-            flag = "C_"
-        else:
-            print(" map name is error:%s" % (mapName))
-            return
-
-        for node in map['street_nodes'].items():
-            [id, point] = node
-            street_node = mp.StreetNode(id, point[0], point[1])
-            if street_node.id_.find(flag) >= 0:
-                if street_node.id_.find("OutLink")>=0:
-                    continue
-                self.mapManager.AddVertex(mapName, street_node)
-
-        for node in map['space_nodes'].items():
-            [id, point] = node
-            [x, y, yaw] = point
-            space_node = mp.SpaceNode(id, point[0], point[1], yaw)
-            self.mapManager.AddVertex(mapName, space_node)
-
-        for road in map['roads'].items():
-            [roadName, points] = road
-            for pt1 in points:
-                for pt2 in points:
-                    if not pt1 == pt2:
-                        if roadName.find(flag) >= 0:
-                            self.mapManager.AddEdge(mapName, pt1, pt2)
-
-    def loadJsonMap(self, file):
-        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
-        map = self.readJson(file)
-
-        for node in map['street_nodes'].items():
-            [id, point] = node
-            street_node = mp.StreetNode(id, point[0], point[1])
-            self.mapManager.AddVertex("Base", street_node)
-            if street_node.id_.find("F_") >= 0:
-                self.mapManager.AddVertex("Front", street_node)
-            if street_node.id_.find("B_") >= 0:
-                self.mapManager.AddVertex("Back", street_node)
-            if street_node.id_.find("C_") >= 0:
-                self.mapManager.AddVertex("Main", street_node)
-
-            self.ui_nodes["street_nodes"].append(id)
-        for node in map['space_nodes'].items():
-            [id, point] = node
-            [x, y, yaw] = point
-            space_node = mp.SpaceNode(id, point[0], point[1], yaw)
-            self.mapManager.AddVertex("Base", space_node)
-            self.mapManager.AddVertex("Front", space_node)
-            self.mapManager.AddVertex("Back", space_node)
-            self.mapManager.AddVertex("Main", space_node)
-
-            self.ui_nodes["space_nodes"].append(id)
-
-        for road in map['roads'].items():
-            [roadName, points] = road
-            for pt1 in points:
-                for pt2 in points:
-                    if not pt1 == pt2:
-                        self.mapManager.AddEdge("Base", pt1, pt2)
-                        if pt1.find("OutLink")>=0 or pt2.find("OutLink")>=0:
-                            continue
-                        if roadName.find("F_") >= 0:
-                            self.mapManager.AddEdge("Front", pt1, pt2)
-                        if roadName.find("B_") >= 0:
-                            self.mapManager.AddEdge("Back", pt1, pt2)
-                        if roadName.find("C_") >= 0:
-                            self.mapManager.AddEdge("Main", pt1, pt2)
-        self.gl.SetMaps(self.mapManager.maps)
+            return False
 
-    def updateMap_park(self, entrance_id, pose, mapName):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
-        self.ReLoadMap(mapName, "./map.json")
-        trans_x, trans_y, trans_a = pose
-
-        # 加车位点
-        self.mapManager.maps[mapName].AddVertex(mp.SpaceNode("S1101", trans_x, trans_y,trans_a))
-
-        entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
-        print("entrance_space pose: ", self.mapManager.maps[mapName].graph_.points[entrance_id])
-
-        street_name = "F_InputLink"
-        if mapName == "Front":
-            street_name = "F_InputLink"
-            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[0][0],
-                                                                  entrance_street_nodes[0][1]))  # 更新库位点对应马路点
-        if mapName == "Main":
-            street_name = "C_InputLink"
-            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[1][0],
-                                                                entrance_street_nodes[1][1]))  # 更新库位点对应马路点
-        if mapName == "Back":
-            street_name = "B_InputLink"
-            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[2][0],
-                                                                  entrance_street_nodes[2][1]))  # 更新库位点对应马路点
-
-        # 加临时边
-        for node_id, value in self.mapManager.VertexDict(mapName).items():
-            if node_id.find("Input") >= 0:
-                self.mapManager.maps[mapName].AddEdge(street_name, node_id)
-
-        self.mapManager.AddVertex(mapName, mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))  # 更新库位点
-        self.mapManager.maps[mapName].AddEdge(entrance_id, street_name)
-
-        print(mapName + " Map entrance_street pose ", self.mapManager.maps[mapName].graph_.points[street_name])
-        self.gl.SetMap(mapName, self.mapManager.maps[mapName])
-
-    def GenerateSpecifiedMap(self, map_type):  # 0:前车地图，1：后车地图，2：整车地图
-        pass
-        '''map = self.load_map("./map.json")
-        {
-            for node in map['street_nodes'].items():
-                [id, point] = node
-                street_node = mp.StreetNode(id, point[0], point[1])
-                self.mapManager.AddVertex(street_node)
-                self.gl.AddNode([id, "street_node", point])
-                self.ui_nodes["street_nodes"].append(id)
-            for node in map['space_nodes'].items():
-                [id, point] = node
-                [x, y, yaw] = point
-                space_node = mp.SpaceNode(id, point[0], point[1], yaw)
-                self.mapManager.AddVertex(space_node)
-                glNode = [id, "space_node", [x, y]]
-                self.gl.AddNode(glNode)
-                self.ui_nodes["space_nodes"].append(id)
-
-            for road in map['roads'].items():
-                self.gl.AddRoad(road)
-
-                [_, points] = road
-                for pt1 in points:
-                    for pt2 in points:
-                        if not pt1 == pt2:
-                            self.mapManager.AddEdge(pt1, pt2)
-        }
-
-
-
-        if type == 0:
-            pass
-        elif type == 1:
-            pass
-        elif type == 2:
-            pass
-        else:
-            print("type of specified map is wrong!\n")'''
+        if self.isAutoDispatchOnline:
+            return await self.PushPickTask(pick_table)
+        return False
+
+    def updateMap_park(self, entrance_id, pose, wheelbase):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
+        self.mapManager.ResetMap("Front", 0, wheelbase / 2)
+        [trans_x, trans_y, trans_a] = pose
+        self.mapManager.maps["Front"].ResetVertexValue(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))
+
+        [id, x, y] = self.ComputeStreetNode("Front", entrance_id, trans_x, trans_y, trans_a)
+        self.mapManager.maps["Front"].ResetVertexValue(mp.StreetNode(id, x, y))
+
+        self.mapManager.ResetMap("Back", 0, -wheelbase / 2)
+        [trans_x, trans_y, trans_a] = pose
+        self.mapManager.maps["Back"].ResetVertexValue(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))
+
+        [id, x, y] = self.ComputeStreetNode("Back", entrance_id, trans_x, trans_y, trans_a)
+        self.mapManager.maps["Back"].ResetVertexValue(mp.StreetNode(id, x, y))
+
+    def asyncExecute(self, tasks):
+        results = [feature.result() for feature in concurrent.futures.as_completed(tasks)]
+        print("==========================\n")
+        print(results)
+        print("==========================\n")
+        ret = True
+        for result in results:
+            ret = ret and result
+        return ret
 
     def GetMainFrontBackAGV(self):
         controlMain = self.Controller1
@@ -321,21 +234,53 @@ class MainWindow(QMainWindow):
             control1 = self.Controller2
         return [controlMain, control1, control2]
 
+    async def PushParkTask(self, park_table: message.park_table = None):
+        pushed = False
+        while pushed == False and self.Cancel_ == False:
+            pushed = self.taskTable.PushTask(park_table.primary_key, park_table)
+            await asyncio.sleep(0.3)
+        if pushed != True or self.Cancel_ != False:
+            return False
+        ret = None
+        while pushed and self.Cancel_ == False:
+            ret = self.taskTable.GetResult(park_table.primary_key)
+            if ret == None:
+                await asyncio.sleep(1)
+            else:
+                break
+
+        return (ret != None)
+
+    async def PushPickTask(self, pick_table: message.pick_table = None):
+        pushed = False
+        while pushed == False and self.Cancel_ == False:
+            pushed = self.taskTable.PushTask(pick_table.primary_key, pick_table)
+            await asyncio.sleep(0.3)
+        if pushed != True or self.Cancel_ != False:
+            return False
+        ret = None
+        while pushed and self.Cancel_ == False:
+            ret = self.taskTable.GetResult(pick_table.primary_key)
+            if ret == None:
+                await asyncio.sleep(1)
+            else:
+                break
+
+        return (ret != None)
+
     def AutoParkTask(self, park_table: message.park_table = None):
         print("AutoParkTask:---------------------\n")
+        task_flag = 1  # 存车
 
         [controlMain, control1, control2] = self.GetMainFrontBackAGV()
-        mainMap, frontMap, backMap = ["Main", "Front", "Back"]
-        entrance_id = "S" + str(park_table.terminal_id)  # "S1101"
+        mainMap, frontMap, backMap = ["Base", "Front", "Back"]
+        entrance_id = "S_" + str(park_table.terminal_id)  # "S1101"
         entrance_x, entrance_y, entrance_theta = [park_table.entrance_measure_info.measure_info_to_plc_forward.cx,
                                                   park_table.entrance_measure_info.measure_info_to_plc_forward.cy,
                                                   (
                                                           90 + park_table.entrance_measure_info.measure_info_to_plc_forward.theta) / 180 * math.pi]
         # 变换到地图坐标系
-        # [dx, dy, da] = [-0.223411843181, -0.643030941486, 178.9478 / 180 * math.pi]
-        # trans_x = -0.99983137 * entrance_x - 0.01836309 * entrance_y + dx
-        # trans_y = 0.01836309 * entrance_x - 0.99983137 * entrance_y + dy
-        [dx, dy, da] = [-0.234040126204, -0.647539079189, 178.9302736990612 / 180 * math.pi]
+        [dx, dy, da] = [-0.184040126204, -0.647539079189, 178.9302736990612 / 180 * math.pi]
         trans_x = -0.999825716019 * entrance_x - 0.018668506294 * entrance_y + dx
         trans_y = 0.018668506294 * entrance_x - 0.999825716019 * entrance_y + dy
         trans_a = entrance_theta + da - math.pi
@@ -343,7 +288,7 @@ class MainWindow(QMainWindow):
             trans_a += math.pi
         print("entrance:", entrance_id, trans_x, trans_y, trans_a / math.pi * 180)
 
-        target_id = "S" + str(park_table.allocated_space_info.table_id)  # "S147"
+        target_id = "S_" + str(park_table.allocated_space_info.table_id)  # "S147"
         print("target:", target_id)
 
         # 轴距
@@ -354,85 +299,136 @@ class MainWindow(QMainWindow):
         trans_x += wheel_base / 2 * math.cos(trans_a)
         trans_y += wheel_base / 2 * math.sin(trans_a)
 
-        threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.shutdown(wait=True)
-
-        # # 双单车入库-----------------------------------------------------
-        self.ReLoadMap(frontMap, "./map.json")
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], frontMap)
+        # # # 双单车入库-----------------------------------------------------
+        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+        #     return False
+        # autoDirect = True
+        # self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], wheel_base)
+        #
+        # cur_pose1 = control1.robot_.timedRobotStatu_.statu
+        # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
+        # control1.robot_.GeneratePath(label, entrance_id, frontMap)
+        # print("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        #
+        # cur_pose2 = control2.robot_.timedRobotStatu_.statu
+        # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
+        # control2.robot_.GeneratePath(label, entrance_id, backMap)
+        # print("Back: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        #
+        # threadPool = ThreadPoolExecutor(2)
+        # features = [
+        #     threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag),
+        #     threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag)
+        # ]
+        #
+        # if self.asyncExecute(features) == False:
+        #     print(" Park Failed ")
+        #     return False
+
+        # # ------------------------------------------
+        # # 双单车驶至入口前巷道点
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], wheel_base)
+        entrance_streetNode = entrance_id.replace("S_", "R_")
+        if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+            return False
         cur_pose1 = control1.robot_.timedRobotStatu_.statu
         [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
-        control1.robot_.GeneratePath(label, entrance_id, frontMap)
-        print("Main: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        control1.robot_.GeneratePath(label, entrance_streetNode, frontMap)
+        print("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_streetNode, wheel_base))
 
-        # 后车 ------------------------------------------------------
-        self.ReLoadMap(backMap, "./map.json")
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], backMap)
         cur_pose2 = control2.robot_.timedRobotStatu_.statu
         [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
-        control2.robot_.GeneratePath(label, entrance_id, backMap)
-
-        # print("Child: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        control2.robot_.GeneratePath(label, entrance_streetNode, backMap)
+        print("Back: begin:%s   target:%s    wheelBase:%f" % (label, entrance_streetNode, wheel_base))
 
         autoDirect = True
-
         threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.Navigatting,
-                          label, entrance_id, autoDirect, wheel_base)
-        threadPool.submit(control2.robot_.Navigatting,
-                          label, entrance_id, autoDirect, wheel_base)
+        features = [
+        threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag),
+        threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag)
+        ]
+        if self.asyncExecute(features) == False:
+            print(" Park Failed ")
+            return False
+
+        # 整车驶进入口---------------------------------
+        if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
+            return False
 
-        threadPool.shutdown(wait=True)
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
+        controlMain.robot_.GeneratePath(entrance_streetNode, entrance_id, map_name)
+        if controlMain.robot_.Navigatting(entrance_streetNode, entrance_id, True, wheel_base, task_flag) == False:
+            return False
+        # # ---------------------------------
 
         print(" input entrance completed!!!!")
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
-        threadPool.shutdown(wait=True)
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.ClampClose)
-        threadPool.shutdown(wait=True)
-
-        # 整车入库---------------------------------
-        map_name="Front"
-        if controlMain.robot_.timedRobotStatu_.statu.theta<0:
-            map_name="Back"
-
-        self.ReLoadMap(map_name, "./map.json")
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], map_name)
-        controlMain.robot_.GeneratePath(entrance_id, target_id, map_name)
-        print("begin:%s   target:%s    wheelBase:%f" % (entrance_id, target_id, wheel_base))
+        if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
+            return False
+        controlMain.robot_.ClampClose()
+
+        # 整车去车位---------------------------------
+        # map_name = "Front"
+        # if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+        #     map_name = "Back"
+        # controlMain.robot_.GeneratePath(entrance_id, target_id, map_name)
+        # print("begin:%s   target:%s    wheelBase:%f" % (entrance_id, target_id, wheel_base))
+        # controlMain.robot_.Navigatting(entrance_id, target_id, autoDirect, wheel_base, task_flag)
+
+        # 整车驶离入口到入口航道点
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
+        controlMain.robot_.GeneratePath(entrance_id, entrance_streetNode, map_name)
+        print("begin:%s   target:%s    wheelBase:%f" % (entrance_id, entrance_streetNode, wheel_base))
+        controlMain.robot_.Navigatting(entrance_id, entrance_streetNode, autoDirect, wheel_base, task_flag)
 
+        # 整车从入口航道点到车位
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
+        controlMain.robot_.GeneratePath(entrance_streetNode, target_id, map_name)
+        print("begin:%s   target:%s    wheelBase:%f" % (entrance_streetNode, target_id, wheel_base))
+        controlMain.robot_.Navigatting(entrance_streetNode, target_id, autoDirect, wheel_base, task_flag)
 
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.Navigatting,
-                          entrance_id, target_id, autoDirect, wheel_base)
-        threadPool.shutdown(wait=True)
 
+        print("-------------------------")
         # 整车松夹池------------------------------------
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.ClampOpen)
-        threadPool.shutdown(wait=True)
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.shutdown(wait=True)
-
-        # 双单车出库
-        self.ReLoadMap(frontMap, "./map.json")
-        controlMain, control1, control2 = self.GetMainFrontBackAGV()
-        control1.robot_.GeneratePath(target_id, "F_Input_R147", frontMap)
+        if controlMain.robot_.ClampOpen() == False:
+            return False
 
-        self.ReLoadMap(backMap, "./map.json")
-        control2.robot_.GeneratePath(target_id, "B_Input_R147", backMap)
-
-        threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.Navigatting,
-                          target_id, "F_Input_R147", autoDirect, wheel_base)
-        threadPool.submit(control2.robot_.Navigatting,
-                          target_id, "B_Input_R147", autoDirect, wheel_base)
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
 
-        threadPool.shutdown(wait=True)
+        # 整车出库---------------------------------
+        # 限定轴距2.6m
+        temp_wheel_base = self.mapManager.default_wheel_base
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], temp_wheel_base)
+        if (controlMain.robot_.SwitchMoveMode(1, temp_wheel_base) == False):
+            return False
+        entrance_streetNode = target_id.replace("S_", "R_")
+        controlMain.robot_.GeneratePath(target_id, entrance_streetNode, map_name)
+        if controlMain.robot_.Navigatting(target_id, entrance_streetNode, True, temp_wheel_base, task_flag) == False:
+            return False
+
+        #
+        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+        #     return False
+        #
+        # # 双单车出库
+        # controlMain, control1, control2 = self.GetMainFrontBackAGV()
+        # end_street = target_id.replace("S_", "R_")
+        # control1.robot_.GeneratePath(target_id, end_street, frontMap)
+        # control2.robot_.GeneratePath(target_id, end_street, backMap)
+        #
+        # threadPool = ThreadPoolExecutor(2)
+        # threadPool.submit(control1.robot_.Navigatting,
+        #                   target_id, end_street, autoDirect, wheel_base, task_flag)
+        # threadPool.submit(control2.robot_.Navigatting,
+        #                   target_id, end_street, autoDirect, wheel_base, task_flag)
+        # threadPool.shutdown(wait=True)
 
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
@@ -440,161 +436,159 @@ class MainWindow(QMainWindow):
         print("Publish_park_response------------------\n", park_table)
         return True
 
-    def updateMap_pick(self, export_id, mapName):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
-        self.ReLoadMap(mapName, "./map.json")
-
-        # export_id = "S1101"
-        # 添加出口车位点
-        export_x = self.mapManager.maps["Base"].graph_.points["S1100"][0]
-        export_y = self.mapManager.maps["Base"].graph_.points["S1100"][1]
-        self.mapManager.AddVertex(mapName, mp.SpaceNode(export_id, export_x, export_y, 90))
-        # 更新库位点对应马路点
-        flag = "F_"
-        if mapName == "Front":
-            export_y = self.mapManager.maps["Base"].graph_.points["F_OutLink"][1]
-            node = mp.StreetNode("F_exportLink", export_x, export_y)
-        elif mapName == "Back":
-            flag = "B_"
-            export_y = self.mapManager.maps["Base"].graph_.points["B_OutLink"][1]
-            node = mp.StreetNode("B_exportLink", export_x, export_y)
-        elif mapName == "Main":
-            flag = "C_"
-            export_y = self.mapManager.maps["Base"].graph_.points["C_OutLink"][1]
-            node = mp.StreetNode("C_exportLink", export_x, export_y)
-
-        self.mapManager.AddVertex(mapName, node)  # 更新库位点对应马路点
-        self.mapManager.AddEdge(mapName, export_id, node.id_)
-
-        for streetID, value in self.mapManager.VertexDict(mapName).items():
-            if streetID.find(flag) >= 0:
-                self.mapManager.AddEdge(mapName, node.id_, streetID)
-            print("Add edge :%s-%s  ", node.id_, streetID)
+    def updateMap_pick(self, wheelBase):  # 轴距
+        self.mapManager.ResetMap("Front", 0, wheelBase / 2)
+        self.mapManager.ResetMap("Back", 0, -wheelBase / 2)
 
     def AutoPickTask(self, pick_table: message.pick_table = None):
         print("AutoPickTask:---------------------\n")
+        task_flag = 2  # 取车
 
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
         mainMap, frontMap, backMap = ["Main", "Front", "Back"]
 
-        space_id = "S" + str(pick_table.actually_space_info.table_id)  # "S147"
+        space_id = "S_" + str(pick_table.actually_space_info.table_id)  # "S147"
 
-        export_id = "S" + str(pick_table.terminal_id)  # "S1101"
+        export_id = "S_" + str(pick_table.terminal_id)  # "S1101"
         print(space_id, export_id)
         # 轴距
         wheel_base = pick_table.actually_measure_info.measure_info_to_plc_forward.wheelbase
         control1.WheelBaseEdit.setText(str(wheel_base))
         control2.WheelBaseEdit.setText(str(wheel_base))
 
-
-        # 双单车入库
-        threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.shutdown(wait=True)
-
-
-        self.ReLoadMap(frontMap, "./map.json")
-        self.updateMap_pick(export_id, frontMap)
+        # # 双单车入库 -------------------------
+        # self.updateMap_pick(wheel_base)
+        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+        #     return False
+        # cur_pose1 = control1.robot_.timedRobotStatu_.statu
+        # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
+        # control1.robot_.GeneratePath(label, space_id, frontMap)
+        # print("Front: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        #
+        # cur_pose2 = control2.robot_.timedRobotStatu_.statu
+        # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
+        # control2.robot_.GeneratePath(label, space_id, backMap)
+        # print("Back: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        #
+        # autoDirect = True
+        # threadPool = ThreadPoolExecutor(2)
+        # features = [threadPool.submit(control1.robot_.Navigatting, label, space_id, autoDirect, wheel_base, task_flag),
+        #             threadPool.submit(control2.robot_.Navigatting, label, space_id, autoDirect, wheel_base, task_flag)]
+        # if self.asyncExecute(features) == False:
+        #     print(" Pick Failed ")
+        #     return False
+
+        # # ------------------------------------------
+        # # 双单车驶至车位前巷道点
+        self.updateMap_pick(wheel_base)
+        space_streetNode = space_id.replace("S_", "R_")
+        if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+            return False
         cur_pose1 = control1.robot_.timedRobotStatu_.statu
         [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
-        control1.robot_.GeneratePath(label, space_id, frontMap)
-        print("Main: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        control1.robot_.GeneratePath(label, space_streetNode, frontMap)
+        print("Front: begin:%s   target:%s    wheelBase:%f" % (label, space_streetNode, wheel_base))
 
-        self.ReLoadMap(backMap, "./map.json")
-        self.updateMap_pick(export_id, backMap)
         cur_pose2 = control2.robot_.timedRobotStatu_.statu
         [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
-        control2.robot_.GeneratePath(label, space_id, backMap)
-        print("Child: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
-
+        control2.robot_.GeneratePath(label, space_streetNode, backMap)
+        print("Back: begin:%s   target:%s    wheelBase:%f" % (label, space_streetNode, wheel_base))
 
         autoDirect = True
         threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.Navigatting,
-                          label, space_id, autoDirect, wheel_base)
-        threadPool.submit(control2.robot_.Navigatting,
-                          label, space_id, autoDirect, wheel_base)
-        threadPool.shutdown(wait=True)
+        features = [
+        threadPool.submit(control1.robot_.Navigatting, label, space_streetNode, autoDirect, wheel_base, task_flag),
+        threadPool.submit(control2.robot_.Navigatting, label, space_streetNode, autoDirect, wheel_base, task_flag)
+        ]
+        if self.asyncExecute(features) == False:
+            print(" Pick Failed ")
+            return False
+
+        # 整车驶进车位---------------------------------
+        if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
+            return False
+
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
+        controlMain.robot_.GeneratePath(space_streetNode, space_id, map_name)
+        if controlMain.robot_.Navigatting(space_streetNode, space_id, True, wheel_base, task_flag) == False:
+            return False
+        # # ---------------------------------
 
         # 整车夹持------------------------------------
         print(" input space completed!!!!")
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
-        threadPool.shutdown(wait=True)
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.ClampClose)
-        threadPool.shutdown(wait=True)
+        if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
+            return False
+        if controlMain.robot_.ClampClose() == False:
+            return False
 
         # 整车到出口---------------------------------
         map_name = "Front"
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
-        self.ReLoadMap(map_name, "./map.json")
-        self.updateMap_pick(export_id, map_name)
-
         controlMain.robot_.GeneratePath(space_id, export_id, map_name)
         print("Total: begin:%s   target:%s    wheelBase:%f" % (space_id, export_id, wheel_base))
-
-
-
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.Navigatting,
-                          space_id, export_id, True, wheel_base)
-        threadPool.shutdown(wait=True)
-
+        if controlMain.robot_.Navigatting(space_id, export_id, True, wheel_base, task_flag) == False:
+            return False
 
         # 整车松夹池------------------------------------
 
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.ClampOpen)
-        threadPool.shutdown(wait=True)
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
-        threadPool.shutdown(wait=True)
-
-        # 双单车出库
-        controlMain, control1, control2 = self.GetMainFrontBackAGV()
-        self.ReLoadMap(frontMap, "./map.json")
-        self.updateMap_pick(export_id, frontMap)
-        control1.robot_.GeneratePath(export_id, "F_exportLink", frontMap)
-
-        self.ReLoadMap(backMap, "./map.json")
-        self.updateMap_pick(export_id, backMap)
-        control2.robot_.GeneratePath(export_id, "B_exportLink", backMap)
+        if controlMain.robot_.ClampOpen() == False:
+            return False
 
+        # 整车回巷道------------------------------------
+        temp_wheel_base = self.mapManager.default_wheel_base
+        self.updateMap_pick(temp_wheel_base)
+        if (controlMain.robot_.SwitchMoveMode(1, temp_wheel_base) == False):
+            return False
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
 
-        threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.Navigatting,
-                          export_id, "F_exportLink", True, wheel_base)
-        threadPool.submit(control2.robot_.Navigatting,
-                          export_id, "B_exportLink", True, wheel_base)
-        threadPool.shutdown(wait=True)
+        space_streetNode = export_id.replace("S_", "R_")
+        controlMain.robot_.GeneratePath(export_id, space_streetNode, map_name)
+        if controlMain.robot_.Navigatting(export_id, space_streetNode, True, temp_wheel_base, task_flag) == False:
+            return False
 
+        # 双单车出库
+        # controlMain, control1, control2 = self.GetMainFrontBackAGV()
+        # self.ReLoadMap(frontMap, "./map.json")
+        # self.updateMap_pick(export_id, frontMap)
+        # control1.robot_.GeneratePath(export_id, "F_exportLink", frontMap)
+        #
+        # self.ReLoadMap(backMap, "./map.json")
+        # self.updateMap_pick(export_id, backMap)
+        # control2.robot_.GeneratePath(export_id, "B_exportLink", backMap)
+        #
+        # threadPool = ThreadPoolExecutor(2)
+        # features=[
+        # threadPool.submit(control1.robot_.Navigatting,export_id, "F_exportLink", True, wheel_base, task_flag),
+        # threadPool.submit(control2.robot_.Navigatting,export_id, "B_exportLink", True, wheel_base, task_flag)
+        # ]
+        # if self.asyncExecute(features) == False:
+        #     print(" Pick Failed ")
+        #     return False
+
+        print("  over publish...")
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
                                 tf.MessageToString(pick_table, as_utf8=True))
         print("Publish_park_response------------------\n", pick_table)
         return True
 
-
-    def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
+    def ComputeStreetNode(self, mapName, s_id, s_x, s_y, s_theta):
         """
-
         """
-        n_x, n_y = 0, 0
-        if s_id == "S1101":
-            n_y = self.mapManager.maps["Base"].graph_.points["C_OutLink"][1]
+        id, n_x, n_y = "", 0, 0
+        if s_id == "S_1101":
+            id = "R_1101"
+            n_y = self.mapManager.maps[mapName].graph_.points[id][1]
             k = math.tan(s_theta)
             n_x = (n_y - s_y) / k + s_x  # 弧度
 
-            n_yF = self.mapManager.maps["Base"].graph_.points["F_OutLink"][1]
-            n_xF = (n_yF - s_y) / k + s_x  # 弧度
-
-            n_yB = self.mapManager.maps["Base"].graph_.points["B_OutLink"][1]
-            n_xB = (n_yB - s_y) / k + s_x  # 弧度
-
-        # print(n_x, n_y)
-        return [[n_xF, n_yF], [n_x, n_y], [n_xB, n_yB]]
+        print(id, n_x, n_y)
+        return [id, n_x, n_y]
 
     def readJson(self, file):
         with open(file, 'r', encoding='utf-8') as fp:
@@ -623,6 +617,7 @@ class MainWindow(QMainWindow):
         self.move(int((screen.width() - self_size.width()) / 2), int((screen.height() - self_size.height()) / 2))
 
     def closeEvent(self, QCloseEvent):
+        self.Cancel_ = True
         self.gl.close()
         print("close...")
 
@@ -645,7 +640,7 @@ class MainWindow(QMainWindow):
         splitter_main.addWidget(self.gl)
 
         splitter_main.setStretchFactor(0, 1)
-        splitter_main.setStretchFactor(2, 4)
+        splitter_main.setStretchFactor(1, 10)
 
         return splitter_main
 
@@ -655,17 +650,6 @@ if __name__ == "__main__":
     win = MainWindow()
     win.show()
     sys.exit(app.exec_())
-
-# ===============================================================================
-# Main
-# ===============================================================================
-
-'''app = QApplication(sys.argv)
-mainWindow = MapGLWidget()
-mainWindow.show()
-mainWindow.raise_() # Need this at least on OS X, otherwise the window ends up in background
-sys.exit(app.exec_())'''
-
 # ===============================================================================
 #
 # Local Variables: