1.地图保留四份，全部地图base，整车地图Main,前方地图Front，后方地图Back

2.手动操作更新轴距
3.界面添加地图切换

ControllWidget.py

@@ -12,7 +12,7 @@ import message_pb2
 from RobotData import Robot
 import dijkstra.Map as mp
 from custom_define import ControllerStatus
-
+import MapGLWidget as viewer
 
 class ControlFrame(QFrame):
     threadPool_ = ThreadPoolExecutor(5)
@@ -148,7 +148,7 @@ class ControlFrame(QFrame):
             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([x,y])
+                [label,pt]=mp.MapManager().findNeastNode("Base",[x,y])
                 self.begQc.setCurrentText(label)
 
         statu = "min-width: 32px; min-height: 32px;max-width:32px;\
@@ -195,8 +195,8 @@ class ControlFrame(QFrame):
             self.begId_ = self.robot_.begId_
             self.targetId_ = self.robot_.targetId_
             djks_map = mp.MapManager()
-            beg_node = djks_map.GetVertex(self.begId_)
-            end_node = djks_map.GetVertex(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:
@@ -231,6 +231,7 @@ class ControlFrame(QFrame):
         if self.robot_.IsMainModeStatu() == False:
             leng = float(self.WheelBaseEdit.text())
             self.threadPool_.submit(self.robot_.SwitchMoveMode, 1, leng)
+            self.robot_.L_ = leng
         else:
             self.threadPool_.submit(self.robot_.SwitchMoveMode, 0, 0)
 
@@ -238,7 +239,7 @@ class ControlFrame(QFrame):
         id1 = self.begQc.currentText()
         id2 = self.endStreetQc.currentText()
         if not id1 == "------" and not id2 == "------":
-            self.robot_.GeneratePath(id1, id2)
+            self.robot_.GeneratePath(id1, id2,"Base")
             self.begId_=id1
             self.targetId_=id2
 
@@ -246,7 +247,7 @@ class ControlFrame(QFrame):
         id1 = self.begQc.currentText()
         id2 = self.endSpaceQc.currentText()
         if not id1 == "------" and not id2 == "------":
-            self.robot_.GeneratePath(id1, id2)
+            self.robot_.GeneratePath(id1, id2,"Base")
             self.begId_=id1
             self.targetId_=id2
 
@@ -262,7 +263,7 @@ class ControlFrame(QFrame):
             if self.btnAutoDirect.checkState()==Qt.Checked:
                 print("自由方向")
                 autoDirect=True
-            self.robot_.GeneratePath(self.begId_, self.targetId_)
+            self.robot_.GeneratePath(self.begId_, self.targetId_,"Base")
             self.threadPool_.submit(self.robot_.Navigatting,
                                     self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()))
 
@@ -290,6 +291,36 @@ class CountFrame(QFrame):
         self.static2.setText("单车导航次数 : /")
         self.static2.setGeometry(5, 40, 200, 30)
 
+        self.btnFrontCheck = QCheckBox("前车地图", self)
+        self.btnFrontCheck.setGeometry(210, 5, 100, 30)
+        self.btnFrontCheck.clicked.connect(self.FrontChecked)
+
+        self.btnBackCheck = QCheckBox("后车地图", self)
+        self.btnBackCheck.setGeometry(310, 5, 100, 30)
+        self.btnBackCheck.clicked.connect(self.FrontChecked)
+
+        self.btnMainCheck = QCheckBox("整车地图", self)
+        self.btnMainCheck.setGeometry(210, 40, 100, 30)
+        self.btnMainCheck.clicked.connect(self.FrontChecked)
+
+        self.btnBaseCheck = QCheckBox("原始地图", self)
+        self.btnBaseCheck.setGeometry(310, 40, 100, 30)
+        self.btnBaseCheck.clicked.connect(self.FrontChecked)
+
+
+    def FrontChecked(self):
+        maps = []
+        if self.btnFrontCheck.checkState() == Qt.Checked:
+            maps.append("Front")
+        if self.btnBackCheck.checkState() == Qt.Checked:
+            maps.append("Back")
+        if self.btnMainCheck.checkState() == Qt.Checked:
+            maps.append("Main")
+        if self.btnBaseCheck.checkState() == Qt.Checked:
+            maps.append("Base")
+        view = viewer.MapGLWidget()
+        view.SetEnableMaps(maps)
+
     def UpdateUI(self):
         load_str = "重载导航次数 : %d" % (Count.TestCount().loadCount())
         single_str = "单车导航次数 : %d" % (Count.TestCount().singleCount())

Count.py

@@ -4,7 +4,6 @@ import threading
 import time
 from dijkstra.Map import singleton
 import json
-
 @singleton
 class TestCount():
     conf_file_="./count.json"

JointContrallerWidget.py

@@ -1,11 +1,11 @@
 import time
 from concurrent.futures import ThreadPoolExecutor
 
-from PyQt5.QtCore import QTimer
-from PyQt5.QtWidgets import QFrame, QLabel, QPushButton
+from PyQt5.QtCore import QTimer,Qt
+from PyQt5.QtWidgets import QFrame, QLabel, QPushButton,QCheckBox
 
 import Count
-
+import MapGLWidget as viewer
 
 class JointControlFrame(QFrame):
     threadPool_ = ThreadPoolExecutor(5)
@@ -86,14 +86,43 @@ class CountFrame(QFrame):
     def InitUI(self):
         self.static1 = QLabel(self)
         self.static1.setText("重载导航次数 : /")
-        self.static1.setGeometry(5, 5, 200, 30)
+        #self.static1.setGeometry(5, 5, 100, 30)
 
         self.static2 = QLabel(self)
         self.static2.setText("单车导航次数 : /")
-        self.static2.setGeometry(5, 40, 200, 30)
+        #self.static2.setGeometry(5, 40, 100, 30)
+
+        self.btnFrontCheck = QCheckBox("前车地图", self)
+        self.btnFrontCheck.setGeometry(5, 5, 40, 30)
+        self.btnFrontCheck.clicked.connect(self.FrontChecked)
+
+        self.btnBackCheck = QCheckBox("后车地图", self)
+        self.btnBackCheck.setGeometry(260, 40, 40, 30)
+        self.btnBackCheck.clicked.connect(self.FrontChecked)
+
+        self.btnMainCheck = QCheckBox("整车地图", self)
+        self.btnMainCheck.setGeometry(310, 40, 40, 30)
+        self.btnMainCheck.clicked.connect(self.FrontChecked)
+
+        self.btnBaseCheck = QCheckBox("原始地图", self)
+        self.btnBaseCheck.setGeometry(360, 40, 40, 30)
+        self.btnBaseCheck.clicked.connect(self.FrontChecked)
 
     def UpdateUI(self):
         load_str = "重载导航次数 : %d" % (Count.TestCount().loadCount())
         single_str = "单车导航次数 : %d" % (Count.TestCount().singleCount())
         self.static1.setText(load_str)
         self.static2.setText(single_str)
+
+    def FrontChecked(self):
+        maps=[]
+        if self.btnFrontCheck.checkState() == Qt.Checked:
+            maps.append("Front")
+        if self.btnBackCheck.checkState() == Qt.Checked:
+            maps.append("Back")
+        if self.btnMainCheck.checkState() == Qt.Checked:
+            maps.append("Main")
+        if self.btnBaseCheck.checkState() == Qt.Checked:
+            maps.append("Base")
+        view=viewer.MapGLWidget()
+        view.SetMaps(maps)

MapGLWidget.py

@@ -5,31 +5,59 @@ from PyGLWidget import PyGLWidget
 from OpenGL.GL import *
 import RobotData as RD
 import message_pb2 as message
+from dijkstra import Map
 from OpenGL.GLUT import glutStrokeCharacter
 from OpenGL import GLUT
-class MapGLWidget(PyGLWidget):
 
-    nodes = {}
-    roads = {}
 
+def singleton(cls):
+    _instance = {}
+
+    def inner():
+        if cls not in _instance:
+            _instance[cls] = cls()
+        return _instance[cls]
+
+    return inner
+@singleton
+class MapGLWidget(PyGLWidget):
+    maps_={}
+    pointColors={"Front":[0,0.5,0.5],
+            "Back":[0.5,0,0],
+            "Main":[0.5,0,0.5],
+            "Base":[0,0.5,0]}
+    edgeColors = {"Front": [0, 0.5, 0.5],
+                   "Back": [0.5, 0, 0],
+                   "Main": [0.5, 0, 0.5],
+                   "Base": [0, 0.5, 0]}
+    pointSizes={"Front":20,
+            "Back":20,
+            "Main":20,
+            "Base":20
+    }
+    edgeLineWidth = {"Front": 5,
+                  "Back": 5,
+                  "Main": 5,
+                  "Base": 2
+                  }
+    enableMaps=["Front","Back","Main"]
     robot1_ = None
     robot2_ = None
+    
     def __init__(self):
         PyGLWidget.__init__(self)
         GLUT.glutInit()
+    def SetMaps(self,maps):
+        self.maps_=maps
+    def SetMap(self,mapName,map:Map.DijikstraMap):
+        self.maps_[mapName]=map
+    def SetEnableMaps(self,mapNames):
+        self.enableMaps=mapNames
     def SetRobot1Instance(self, robot):
         self.robot1_ = robot
     def SetRobot2Instance(self, robot):
         self.robot2_ = robot
 
-    def AddNode(self, node):
-        [id, type, pose] = node
-        self.nodes[id] = [type, pose]
-
-    def AddRoad(self, road):
-        [key, value] = road
-        self.roads[key] = value
-
     def drawTraj(self, poses, width, color):
         glColor3f(color[0], color[1], color[2])
         glLineWidth(2)
@@ -227,7 +255,7 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(x, y)
         glEnd()
         l=len(label)
-        self.DrawText([x-l/8,y],label,size/10,1,[1,1,1])
+        self.DrawText([x-l/8,y],label,size/200,0.5,[1,1,1])
 
     def drawSpaceNode(self,pose,size,color,label=''):
 
@@ -285,20 +313,22 @@ class MapGLWidget(PyGLWidget):
         PyGLWidget.paintGL(self)
         self.drawAxis([0, 0, 0], 3, 2)
         #绘制地图
-        for road in self.roads.items():
-            [_, value] = road
-            for nodeId in value:
-                for nextId in value:
-                    if not nodeId == nextId:
-                        [_, point1] = self.nodes[nodeId]
-                        [_, point2] = self.nodes[nextId]
-                        self.drawEdge(point1, point2, 20, [1,1,0])
-        for node in self.nodes.items():
-            [name, [type, point]] = node
-            if type == "street_node":
-                self.drawNode(point, 30, [0, 0.5, 0.5],name)
-            if type == "space_node":
-                self.drawNode(point, 20, [0.3, 0.8, 0.7],name)
+        pointSize = 10
+        pointColor = [0.2, 0.3, 0.6]
+        edgeWidth = 10
+        edgeColor = [0.1, 0.6, 0.1]
+        for item in self.maps_.items():
+            key,map=item
+            if self.enableMaps.count(key)<=0:
+                continue
+            for edge in map.Edges():
+                [node1,node2,distance]=edge
+                vertex1=map.GetVertex(node1)
+                vertex2 = map.GetVertex(node2)
+                self.drawEdge([vertex1.x_,vertex1.y_],[vertex2.x_,vertex2.y_],self.edgeLineWidth[key],self.edgeColors[key])
+            for vertex in map.VertexDict().items():
+                nodeID,node=vertex
+                self.drawNode([node.x_,node.y_], self.pointSizes[key], self.pointColors[key], nodeID)
 
         #绘制agv相关数据，路径、轨迹、位姿
         self.DrawRobotData(self.robot1_)

RobotData.py

@@ -151,8 +151,8 @@ class Robot(MqttAsync):
                 return False
         return True'''
 
-    def GeneratePath(self, begID, endID, task_type="None"):
-        self.currentNavPathNodes_ = MapManager().GetShortestPath(begID, endID)
+    def GeneratePath(self, begID, endID, mapName):
+        self.currentNavPathNodes_ = MapManager().GetShortestPath(mapName,begID, endID)
 
         # if task_type == "DoubleAGV":
         #     print("AdjustPath------------------")

dijkstra/Map.py

@@ -86,6 +86,7 @@ class DijikstraMap(object):
     def AddEdge(self, id1, id2, direct=False):
         if self.nodes_.get(id1) == None or self.nodes_.get(id2) == None:
             print("Exceptin: Add edge failed")
+            print(id1, id2)
             raise ("Add edge failed %s or %s node is not exist" % (id1, id2))
         print("Add Edge :%s-%s" % (id1, id2))
         self.graph_.AddEdge(id1, id2)
@@ -287,47 +288,39 @@ class DijikstraMap(object):
 @singleton
 class MapManager(object):
     def __init__(self):
-        self.map = DijikstraMap()
-        self.map_t = DijikstraMap()
 
-    def GetVertex(self, id):
-        return self.map_t.GetVertex(id)
+        self.maps={}
+        self.maps["Base"]=DijikstraMap()
+        self.maps["Main"]=DijikstraMap()
+        self.maps["Front"]=DijikstraMap()
+        self.maps["Back"]=DijikstraMap()
 
-    def AddVertex(self, node):
-        self.map.AddVertex(node)
-        self.map_t.AddVertex(node)
+    def GetVertex(self, mapName,id):
+        return self.maps[mapName].GetVertex(id)
+    def AddVertex(self,mapName, node):
+        self.maps[mapName].AddVertex(node)
 
-    def AddEdge(self, id1, id2, direct=False):
-        self.map.AddEdge(id1, id2, direct)
-        self.map_t.AddEdge(id1, id2, direct)
+    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 AddEdge_t(self, id1, id2, direct=False):
-        self.map_t.AddEdge(id1, id2, direct)
+    def Reset(self,mapName):
+        self.maps[mapName] = DijikstraMap()
 
-    def Reset(self):
-        self.map_t = deepcopy(self.map)
+    def VertexDict(self,mapName):
+        return self.maps[mapName].VertexDict()
 
-    def VertexDict(self):
-        return self.map_t.VertexDict()
+    def Edges(self,mapName):
+        return self.maps[mapName].Edges()
 
-    def Edges(self):
-        return self.map_t.Edges()
-
-    def findNeastNode(self, pt):
-        return self.map_t.findNeastNode(pt)
 
-    def GetShortestPath(self, beg, end):
-        print("beg: ", self.map_t.graph_.points[beg])
-        print("end: ", self.map_t.graph_.points[end])
-        return self.map_t.GetShortestPath(beg, end)
+    def findNeastNode(self, mapName,pt):
+        return self.maps[mapName].findNeastNode(pt)
 
-    def ResetSpaceNode(self, space_id, x, y):
-        space = SpaceNode(space_id, x, y, 0)
-        return self.map_t.ResetVertexValue(space)
+    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)
 
-    def ResetStreetNode(self, space_id, x, y):
-        street = StreetNode(space_id, x, y)
-        return self.map_t.ResetVertexValue(street)

map.json

@@ -1,49 +1,40 @@
 {
   "street_nodes": {
-    "FInput_R145": [
-      -8.57, -5.66
+    "F_Input_R145": [
+      -8.57, -5.61
     ],
-    "CInput_R145": [
+    "C_Input_R145": [
       -8.57, -7.16
     ],
-    "BInput_R145": [
-      -8.57, -8.66
+    "B_Input_R145": [
+      -8.57, -8.46
     ],
-    "FInput_R147": [
-      -10.98, -5.66
+    "F_Input_R147": [
+      -10.98, -5.61
     ],
-    "CInput_R147": [
+    "C_Input_R147": [
       -10.98, -7.16
     ],
-    "BInput_R147": [
-      -10.98, -8.66
+    "B_Input_R147": [
+      -10.98, -8.46
     ],
-     "FInput_R1100": [
-      -0.32, -5.66
+     "F_OutLink": [
+      -0.32, -5.61
     ],
-    "CInput_R1100": [
+    "C_OutLink": [
       -0.32, -7.16
     ],
-     "BInput_R1100": [
-      -0.32, -8.66
+     "B_OutLink": [
+      -0.32, -8.46
     ],
-    "FInput_R65": [
-      -16.665, -5.66
+    "F_Input_R65": [
+      -16.665, -5.61
     ],
-    "CInput_R65": [
+    "C_Input_R65": [
       -16.665, -7.16
     ],
-    "BInput_R65": [
-      -16.665, -8.66
-    ],
-    "FColumn1_R10001": [
-      -0.32, -11.98
-    ],
-    "CColumn1_R10001": [
-     -0.32, -13.48
-    ],
-    "BColumn1_R10001": [
-      -0.32, -14.98
+    "B_Input_R65": [
+      -16.665, -8.46
     ]
   },
 
@@ -62,75 +53,60 @@
     ]
   },
   "roads": {
-    "road_Input": [
-      "CInput_R145",
-      "CInput_R147",
-      "CInput_R1100",
-      "CInput_R65"
-    ],
-    "road_InputF": [
-      "FInput_R145",
-      "FInput_R147",
-      "FInput_R1100",
-      "FInput_R65"
-    ],
-    "road_InputB": [
-      "BInput_R145",
-      "BInput_R147",
-      "BInput_R1100",
-      "BInput_R65"
-    ],
-
-    "road_Column1": [
-      "CColumn1_R10001",
-      "CInput_R1100"
-    ],
-    "road_Column1F": [
-      "FColumn1_R10001",
-      "FInput_R1100"
-    ],
-    "road_Column1B": [
-      "BColumn1_R10001",
-      "BInput_R1100"
-    ],
-
-    "road_145C": [
+    "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",
-      "CInput_R145"
+      "C_Input_R145"
     ],
-    "road_145F": [
+    "F_road_145": [
       "S145",
-      "FInput_R145"
+      "F_Input_R145"
     ],
-    "road_145B": [
+    "B_road_145": [
       "S145",
-      "BInput_R145"
+      "B_Input_R145"
     ],
-
-    "road_147C": [
+    "C_road_147": [
       "S147",
-      "CInput_R147"
+      "C_Input_R147"
     ],
-    "road_147F": [
+    "F_road_147": [
       "S147",
-      "FInput_R147"
+      "F_Input_R147"
     ],
-    "road_147B": [
+    "B_road_147": [
       "S147",
-      "BInput_R147"
+      "B_Input_R147"
     ],
 
-    "road_65C": [
+    "C_road_65": [
       "S65",
-      "CInput_R65"
+      "C_Input_R65"
     ],
-    "road_65F": [
+    "F_road_65": [
       "S65",
-      "FInput_R65"
+      "F_Input_R65"
     ],
-    "road_65B": [
+    "B_road_65": [
       "S65",
-      "BInput_R65"
+      "B_Input_R65"
     ]
   }
 }

test.py

@@ -57,11 +57,11 @@ from mytool.RabbitMq_helper import async_communication as rabitmq
 
 
 # ===============================================================================
-# test 12_19_10_27
+
 
 class MainWindow(QMainWindow):
     """docstring for Mainwindow"""
-    djks_map_ = mp.MapManager()
+    mapManager = mp.MapManager()
     ui_nodes = {}
     ui_nodes["street_nodes"] = []
     ui_nodes["space_nodes"] = []
@@ -70,7 +70,7 @@ class MainWindow(QMainWindow):
         super(MainWindow, self).__init__(parent)
         self.basic()
         self.count_frame_ = ControllWidget.CountFrame()
-        self.LoadMap()
+        self.loadJsonMap("./map.json")
         self.isLocalTest = False
         self.isAutoDispatchOnline = True
 
@@ -132,6 +132,7 @@ class MainWindow(QMainWindow):
 
     def recv_park_request(self, msg):
         print("Recv_park_request------------------\n", msg)
+
         park_table = message.park_table()
         try:
             tf.Parse(msg, park_table)
@@ -151,57 +152,135 @@ class MainWindow(QMainWindow):
         if self.isAutoDispatchOnline:
             self.AutoPickTask(pick_table)
 
-    def updateMap_park(self, entrance_id, pose, type):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
-        self.djks_map_.Reset()  # 重置地图
+    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
 
-        trans_x, trans_y, trans_a = pose
-        self.djks_map_.AddVertex_t(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))  # 更新库位点
+        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)
 
-        entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
-        print("entrance_space pose: ", self.djks_map_.map_t.graph_.points[entrance_id])
-        if type == 0:
-            self.djks_map_.AddVertex_t(mp.StreetNode("FInput_R1101", entrance_street_nodes[0][0],
-                                                     entrance_street_nodes[0][1]))  # 更新库位点对应马路点
-            self.djks_map_.AddEdge_t(entrance_id, "FInput_R1101")
+        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 node_id, value in self.djks_map_.VertexDict().items():
-                if node_id.find("FInput") >= 0:
-                    self.djks_map_.AddEdge_t("FInput_R1101", node_id)
+        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)
 
-            print("F entrance_street pose ", self.djks_map_.map_t.graph_.points["FInput_R1101"])
-        elif type == 1:
-            self.djks_map_.AddVertex_t(mp.StreetNode("BInput_R1101", entrance_street_nodes[2][0],
-                                                     entrance_street_nodes[2][1]))  # 更新库位点对应马路点
-            self.djks_map_.AddEdge_t(entrance_id, "BInput_R1101")
-            # 加临时边
-            for node_id, value in self.djks_map_.VertexDict().items():
-                if node_id.find("BInput") >= 0:
-                    self.djks_map_.AddEdge_t("BInput_R1101", node_id)
+    def loadJsonMap(self, file):
+        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
+        map = self.readJson(file)
 
-        else:
-            self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1101", entrance_street_nodes[1][0],
-                                                     entrance_street_nodes[1][1]))  # 更新库位点对应马路点
-            self.djks_map_.AddEdge_t(entrance_id, "CInput_R1101")
-            for node_id, value in self.djks_map_.VertexDict().items():
-                if node_id.find("CInput") >= 0:
-                    self.djks_map_.AddEdge_t("CInput_R1101", node_id)
-            print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1101"])
+        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)
+
+    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.djks_map_.AddVertex(street_node)
+                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.djks_map_.AddVertex(space_node)
+                self.mapManager.AddVertex(space_node)
                 glNode = [id, "space_node", [x, y]]
                 self.gl.AddNode(glNode)
                 self.ui_nodes["space_nodes"].append(id)
@@ -213,7 +292,7 @@ class MainWindow(QMainWindow):
                 for pt1 in points:
                     for pt2 in points:
                         if not pt1 == pt2:
-                            self.djks_map_.AddEdge(pt1, pt2)
+                            self.mapManager.AddEdge(pt1, pt2)
         }
 
 
@@ -246,7 +325,7 @@ class MainWindow(QMainWindow):
         print("AutoParkTask:---------------------\n")
 
         [controlMain, control1, control2] = self.GetMainFrontBackAGV()
-
+        mainMap, frontMap, backMap = ["Main", "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,
@@ -275,28 +354,30 @@ 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.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 0)
-
+        # # 双单车入库-----------------------------------------------------
+        self.ReLoadMap(frontMap, "./map.json")
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], frontMap)
         cur_pose1 = control1.robot_.timedRobotStatu_.statu
-        [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y])
-        control1.robot_.GeneratePath(label, entrance_id)
+        [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))
 
         # 后车 ------------------------------------------------------
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 1)
-
+        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] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
-        control2.robot_.GeneratePath(label, entrance_id)
-        print("Child: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        [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))
 
         autoDirect = True
+
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.Navigatting,
                           label, entrance_id, autoDirect, wheel_base)
@@ -314,9 +395,16 @@ class MainWindow(QMainWindow):
         threadPool.shutdown(wait=True)
 
         # 整车入库---------------------------------
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 2)
-        controlMain.robot_.GeneratePath(entrance_id, target_id)
+        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))
+
+
         threadPool = ThreadPoolExecutor(1)
         threadPool.submit(controlMain.robot_.Navigatting,
                           entrance_id, target_id, autoDirect, wheel_base)
@@ -331,16 +419,18 @@ class MainWindow(QMainWindow):
         threadPool.shutdown(wait=True)
 
         # 双单车出库
+        self.ReLoadMap(frontMap, "./map.json")
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
-        self.djks_map_.Reset()  # 重置地图
-        control1.robot_.GeneratePath(target_id, "FInput_R147")
-        control2.robot_.GeneratePath(target_id, "BInput_R147")
+        control1.robot_.GeneratePath(target_id, "F_Input_R147", frontMap)
+
+        self.ReLoadMap(backMap, "./map.json")
+        control2.robot_.GeneratePath(target_id, "B_Input_R147", backMap)
 
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.Navigatting,
-                          target_id, "FInput_R147", autoDirect, wheel_base)
+                          target_id, "F_Input_R147", autoDirect, wheel_base)
         threadPool.submit(control2.robot_.Navigatting,
-                          target_id, "BInput_R147", autoDirect, wheel_base)
+                          target_id, "B_Input_R147", autoDirect, wheel_base)
 
         threadPool.shutdown(wait=True)
 
@@ -350,62 +440,74 @@ class MainWindow(QMainWindow):
         print("Publish_park_response------------------\n", park_table)
         return True
 
-    def updateMap_pick(self, export_id, type):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
+    def updateMap_pick(self, export_id, mapName):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
+        self.ReLoadMap(mapName, "./map.json")
 
-        self.djks_map_.Reset()  # 重置地图
-        # export_id = "S1103"
+        # export_id = "S1101"
         # 添加出口车位点
-        self.djks_map_.AddVertex_t(mp.SpaceNode(export_id, self.djks_map_.map_t.graph_.points["S1100"][0],
-                                                self.djks_map_.map_t.graph_.points["S1100"][1], 90))  # 更新库位点对应马路点
-        if type == 0:
-            self.djks_map_.AddEdge_t(export_id, "FInput_R1100")
-        elif type == 1:
-            self.djks_map_.AddEdge_t(export_id, "BInput_R1100")
-        elif type == 2:
-            export_street_node_x = self.djks_map_.map_t.graph_.points["CInput_R1100"][0]
-            export_street_node_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][1]
-            self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1103", export_street_node_x,
-                                                     export_street_node_y))  # 更新库位点对应马路点
-
-            self.djks_map_.AddEdge_t(export_id, "CInput_R1103")
-
-            for node_id, value in self.djks_map_.VertexDict().items():
-                if node_id.find("CInput") >= 0:
-                    self.djks_map_.AddEdge_t("CInput_R1103", node_id)
-            print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1103"])
+        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 AutoPickTask(self, pick_table: message.pick_table = None):
         print("AutoPickTask:---------------------\n")
 
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
+        mainMap, frontMap, backMap = ["Main", "Front", "Back"]
 
-        self.djks_map_.Reset()  # 重置地图
         space_id = "S" + str(pick_table.actually_space_info.table_id)  # "S147"
 
-        export_id = "S" + str(pick_table.terminal_id)  # "S1103"
+        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)
 
-        cur_pose1 = control1.robot_.timedRobotStatu_.statu
 
-        [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y])
-        control1.robot_.GeneratePath(label, space_id)
+        self.ReLoadMap(frontMap, "./map.json")
+        self.updateMap_pick(export_id, frontMap)
+        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))
 
+        self.ReLoadMap(backMap, "./map.json")
+        self.updateMap_pick(export_id, backMap)
         cur_pose2 = control2.robot_.timedRobotStatu_.statu
-        [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
-        control2.robot_.GeneratePath(label, space_id)
+        [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))
 
+
         autoDirect = True
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.Navigatting,
@@ -424,14 +526,23 @@ class MainWindow(QMainWindow):
         threadPool.shutdown(wait=True)
 
         # 整车到出口---------------------------------
-        self.updateMap_pick(export_id, 2)
-        controlMain.robot_.GeneratePath(space_id, export_id)
+        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)
 
+
         # 整车松夹池------------------------------------
 
         threadPool = ThreadPoolExecutor(1)
@@ -443,16 +554,20 @@ class MainWindow(QMainWindow):
 
         # 双单车出库
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
-        self.updateMap_pick(export_id, 0)
-        control1.robot_.GeneratePath(export_id, "FInput_R1100")
-        self.updateMap_pick(export_id, 1)
-        control2.robot_.GeneratePath(export_id, "BInput_R1100")
+        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)
         threadPool.submit(control1.robot_.Navigatting,
-                          export_id, "FInput_R1100", True, wheel_base)
+                          export_id, "F_exportLink", True, wheel_base)
         threadPool.submit(control2.robot_.Navigatting,
-                          export_id, "BInput_R1100", True, wheel_base)
+                          export_id, "B_exportLink", True, wheel_base)
         threadPool.shutdown(wait=True)
 
         # 反馈
@@ -468,48 +583,20 @@ class MainWindow(QMainWindow):
         """
         n_x, n_y = 0, 0
         if s_id == "S1101":
-            n_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][1]
+            n_y = self.mapManager.maps["Base"].graph_.points["C_OutLink"][1]
             k = math.tan(s_theta)
             n_x = (n_y - s_y) / k + s_x  # 弧度
 
-            n_yF = self.djks_map_.map_t.graph_.points["FInput_R1100"][1]
+            n_yF = self.mapManager.maps["Base"].graph_.points["F_OutLink"][1]
             n_xF = (n_yF - s_y) / k + s_x  # 弧度
 
-            n_yB = self.djks_map_.map_t.graph_.points["BInput_R1100"][1]
+            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]]
 
-    def LoadMap(self):
-        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
-        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.djks_map_.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.djks_map_.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.djks_map_.AddEdge(pt1, pt2)
-
-    def load_map(self, file):
+    def readJson(self, file):
         with open(file, 'r', encoding='utf-8') as fp:
             map = json.load(fp)
             return map