1.FCB地图结构优化

ControllWidget.py

@@ -13,7 +13,7 @@ from RobotData import Robot
 import dijkstra.Map as mp
 from custom_define import ControllerStatus
 import MapGLWidget as viewer
-
+import dijkstra.Map as Map
 class ControlFrame(QFrame):
     threadPool_ = ThreadPoolExecutor(5)
     last_beg = "**"
@@ -309,10 +309,12 @@ class CountFrame(QFrame):
 
 
     def FrontChecked(self):
+        wheelBase=2.8
         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")

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

@@ -330,6 +330,7 @@ class Robot(MqttAsync):
                     pathNode = message.PathNode()
                     pathNode.x = node.x_
                     pathNode.y = node.y_
+                    pathNode.id=node.id_
 
                     newAction.type = 4
                     if autoDirect:

count.json

@@ -1 +1 @@
-{"load_count": 885, "single_count": 2722}
+{"load_count": 904, "single_count": 2795}

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
@@ -62,7 +62,6 @@ class DijikstraMap(object):
     def __init__(self):
         self.nodes_ = {} #dict ,{id: StreetNode/SpaceNode}
         self.graph_ = Graph()
-
     def GetVertex(self, id):
         return self.nodes_.get(id)
 
@@ -83,6 +82,7 @@ class DijikstraMap(object):
         self.nodes_[node.id_].x_ = node.x_
         self.nodes_[node.id_].y_ = node.y_
 
+
     def AddEdge(self, id1, id2, direct=False):
         if self.nodes_.get(id1) == None or self.nodes_.get(id2) == None:
             print("Exceptin: Add edge failed")
@@ -288,12 +288,50 @@ 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()
+        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,1.5)
+        self.ResetMap("Back",0,-1.5)
+
+    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)
@@ -303,9 +341,6 @@ class MapManager(object):
     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):
         self.maps[mapName] = DijikstraMap()
 

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,56 @@
 {
   "street_nodes": {
-    "F_Input_R145": [
-      -8.57, -5.61
+    "R_65": [
+      -16.665, -7.16
     ],
-    "C_Input_R145": [
+    "R_145": [
       -8.57, -7.16
     ],
-    "B_Input_R145": [
-      -8.57, -8.46
-    ],
-    "F_Input_R147": [
-      -10.97, -5.61
-    ],
-    "C_Input_R147": [
+    "R_147": [
       -10.97, -7.16
     ],
-    "B_Input_R147": [
-      -10.97, -8.46
-    ],
-     "F_OutLink": [
-      -0.32, -5.61
-    ],
-    "C_OutLink": [
+    "R_1101": [
       -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
     ]
   },
 
   "space_nodes": {
-    "S147": [
-      -10.96, -13.48, -90
-    ],
-    "S1100": [
-      -0.32, 1.0, 90
-    ],
-    "S65": [
+    "S_65": [
       -16.665, -0.980, 89.15
     ],
-    "S145": [
+    "S_145": [
       -8.57, -13.48, -89.15
-    ]
-  },
-  "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"
+        "S_147": [
+      -10.96, -13.48, -90
     ],
+    "S_1101": [
+      -0.32, 1.0, 90
+    ]
+  },
 
-    "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"
+    ],
+    "R65_S65": [
+      "R_65",
+      "S_65"
+    ],
+    "R145_S145": [
+      "S_145",
+      "R_145"
+    ],
+    "R147_S147": [
+      "R_147",
+      "S_147"
+    ],
+    "R1101_S1101": [
+      "R_1101",
+      "S_1101"
     ]
   }
 }

message.proto

@@ -53,7 +53,7 @@ message PathNode  //导航路径点及精度
   float l=3;    //目标点旋转矩形方程，代表精度范围
   float w=4;
   float theta=5;
-  string id=6;
+  optional string id=6;
 }
 
 message Trajectory

mytool/RabbitMq_helper/async_communication.py

@@ -90,8 +90,11 @@ 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 callback(message.body.decode())==True:
+                        await asyncio.sleep(0.5)
+                        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
@@ -57,6 +58,8 @@ from mytool.RabbitMq_helper import async_communication as rabitmq
 
 import concurrent.futures
 
+from queue import Queue
+
 
 # ===============================================================================
 
@@ -71,11 +74,19 @@ class MainWindow(QMainWindow):
     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.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"]
         rpc_2 = "192.168.0.71:9090"
@@ -106,6 +117,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_,
@@ -117,6 +130,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)
@@ -140,8 +154,11 @@ 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 self.AutoParkTask(park_table)
+        return False
 
     def recv_pick_request(self, msg):
         print("Recv_pick_request------------------\n", msg)
@@ -151,124 +168,31 @@ class MainWindow(QMainWindow):
             tf.Parse(msg, pick_table)
         except Exception as e:
             print("Parse  error\n")
+            return False
+
+        # 加入自动任务列表
+        '''if self.isAutoDispatchOnline:
+            temp = time.time()
+            self.auto_task_queue.put((2, pick_table, temp))'''
+
         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 pt1.find("OutLink") >= 0 or pt2.find("OutLink") >= 0:
-                            continue
-                        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 self.AutoPickTask(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))
 
-    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])
+        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)]
@@ -299,16 +223,13 @@ 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"
+        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]
         trans_x = -0.999825716019 * entrance_x - 0.018668506294 * entrance_y + dx
         trans_y = 0.018668506294 * entrance_x - 0.999825716019 * entrance_y + dy
@@ -317,7 +238,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)
 
         # 轴距
@@ -330,24 +251,19 @@ class MainWindow(QMainWindow):
 
         if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
             return False
-
+        autoDirect = True
         # # 双单车入库-----------------------------------------------------
-        self.ReLoadMap(frontMap, "./map.json")
-        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], frontMap)
+        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("Main: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        print("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, 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))
-
-        autoDirect = True
+        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),
@@ -358,7 +274,6 @@ class MainWindow(QMainWindow):
             return False
 
         print(" input entrance completed!!!!")
-
         if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
             return False
         controlMain.robot_.ClampClose()
@@ -367,12 +282,8 @@ class MainWindow(QMainWindow):
         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))
-
         controlMain.robot_.Navigatting(entrance_id, target_id, autoDirect, wheel_base)
 
         print("-------------------------")
@@ -384,19 +295,16 @@ class MainWindow(QMainWindow):
             return False
 
         # 双单车出库
-        self.ReLoadMap(frontMap, "./map.json")
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
-        control1.robot_.GeneratePath(target_id, "F_Input_R147", frontMap)
-
-        self.ReLoadMap(backMap, "./map.json")
-        control2.robot_.GeneratePath(target_id, "B_Input_R147", backMap)
+        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, "F_Input_R147", autoDirect, wheel_base)
+                          target_id, end_street, autoDirect, wheel_base)
         threadPool.submit(control2.robot_.Navigatting,
-                          target_id, "B_Input_R147", autoDirect, wheel_base)
-
+                          target_id, end_street, autoDirect, wheel_base)
         threadPool.shutdown(wait=True)
 
         # 反馈
@@ -405,73 +313,44 @@ 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")
 
+
+
         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))
-
+        self.updateMap_pick(wheel_base)
         # 双单车入库
-        if(controlMain.robot_.SwitchMoveMode(0, wheel_base)==False):
+        if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
             return False
-
-        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))
+        print("Front: 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] = 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))
+        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),
-                    threadPool.submit(control2.robot_.Navigatting,label, space_id, autoDirect, wheel_base)]
+        features = [threadPool.submit(control1.robot_.Navigatting, label, space_id, autoDirect, wheel_base),
+                    threadPool.submit(control2.robot_.Navigatting, label, space_id, autoDirect, wheel_base)]
         if self.asyncExecute(features) == False:
             print(" Pick Failed ")
             return False
@@ -480,84 +359,71 @@ class MainWindow(QMainWindow):
         print(" input space completed!!!!")
         if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
             return False
-        if controlMain.robot_.ClampClose()==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))
 
-
-        if controlMain.robot_.Navigatting(space_id, export_id, True, wheel_base)==False:
+        if controlMain.robot_.Navigatting(space_id, export_id, True, wheel_base) == False:
             return False
 
         # 整车松夹池------------------------------------
 
-        if controlMain.robot_.ClampOpen()==False:
+        if controlMain.robot_.ClampOpen() == False:
             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"
-        self.ReLoadMap(map_name, "./map.json")
-        self.updateMap_pick(export_id, map_name)
-        streetNode= "F_exportLink" if map_name=="Front" else "B_exportLink"
+
+        streetNode = export_id.replace("S_", "R_")
         controlMain.robot_.GeneratePath(export_id, streetNode, map_name)
-        if controlMain.robot_.Navigatting(export_id,streetNode,True,wheel_base)==False:
+        if controlMain.robot_.Navigatting(export_id, streetNode, True, wheel_base) == 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),
-                    threadPool.submit(control2.robot_.Navigatting,export_id, "B_exportLink", True, wheel_base)]
-        if self.asyncExecute(features) == False:
-            print(" Pick Failed ")
-            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),
+        #             threadPool.submit(control2.robot_.Navigatting,export_id, "B_exportLink", True, wheel_base)]
+        # 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:
@@ -608,7 +474,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