1.界面确定路径时添加地图选择

2.存取车流程AGV整车出库时，以默认轴距2.6m动作

ControllWidget.py

@@ -14,6 +14,8 @@ 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 = "**"
@@ -31,15 +33,16 @@ 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 +113,7 @@ class ControlFrame(QFrame):
         self.wheelbasestatic.setText("轴距:")
         self.wheelbasestatic.setGeometry(260, 100, 40, 30)
         self.WheelBaseEdit = QLineEdit(self)
-        self.WheelBaseEdit.setText("2.8")
+        self.WheelBaseEdit.setText(str(self.default_wheel_base))
         self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
         self.btnModelCheck = QCheckBox("整体协调", self)
@@ -140,15 +143,20 @@ class ControlFrame(QFrame):
         self.btnAutoBegChanged.setGeometry(260, 10, 100, 20)
         self.btnAutoBegChanged.setCheckState(Qt.Checked)
 
-    def Update(self):
+        self.select_map_Qc = QComboBox(self)
+        self.select_map_Qc.setGeometry(360, 10, 100, 20)
+        maps = ["Front", "Back", "Base"]
+        self.select_map_Qc.addItems(maps)
+        self.select_map_Qc.activated.connect(self.select_map_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 +203,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 +219,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,31 +248,31 @@ class ControlFrame(QFrame):
         id1 = self.begQc.currentText()
         id2 = self.endStreetQc.currentText()
         if not id1 == "------" and not id2 == "------":
-            self.robot_.GeneratePath(id1, id2,"Back")
-            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,"Back")
-            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_,"Back")
+                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()))
 
@@ -272,11 +281,16 @@ class ControlFrame(QFrame):
         self.threadPool_.submit(self.robot_.CancelNavTask)
         self.btnAuto.setCheckState(Qt.Unchecked)
 
+    def select_map_QcChanged(self):
+        self.select_map = self.select_map_Qc.currentText()
+        print("changed selected map to" + self.select_map)
+
 
 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,14 +321,12 @@ class CountFrame(QFrame):
         self.btnBaseCheck.setGeometry(310, 40, 100, 30)
         self.btnBaseCheck.clicked.connect(self.FrontChecked)
 
-
     def FrontChecked(self):
-        wheelBase=2.8
+        # wheelBase = self.default_wheel_base
         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")

dijkstra/Map.py

@@ -60,8 +60,9 @@ 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):
         return self.nodes_.get(id)
 
@@ -82,7 +83,6 @@ 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,15 +288,18 @@ class DijikstraMap(object):
 @singleton
 class MapManager(object):
     def __init__(self):
-        self.maps={}
+        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):
+
+    def LoadJson(self, file):
 
         self.maps["Base"] = DijikstraMap()
         self.maps["Front"] = DijikstraMap()
@@ -321,41 +324,39 @@ class MapManager(object):
                 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)
+        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"])
+    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))
+            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):
+    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 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)
-

test.py

@@ -202,6 +202,7 @@ class MainWindow(QMainWindow):
         [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))
@@ -209,6 +210,7 @@ class MainWindow(QMainWindow):
         [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")
@@ -279,7 +281,7 @@ class MainWindow(QMainWindow):
                                                   (
                                                           90 + park_table.entrance_measure_info.measure_info_to_plc_forward.theta) / 180 * math.pi]
         # 变换到地图坐标系
-        [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
@@ -327,7 +329,7 @@ class MainWindow(QMainWindow):
             return False
         controlMain.robot_.ClampClose()
 
-        # 整车入库---------------------------------
+        # 整车去车位---------------------------------
         map_name = "Front"
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
@@ -344,12 +346,17 @@ class MainWindow(QMainWindow):
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
 
+        # 整车出库---------------------------------
+        # 限定轴距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
         streetNode = target_id.replace("S_", "R_")
         controlMain.robot_.GeneratePath(target_id, streetNode, map_name)
-        if controlMain.robot_.Navigatting(target_id, streetNode, True, wheel_base) == False:
+        if controlMain.robot_.Navigatting(target_id, streetNode, True, temp_wheel_base) == False:
             return False
 
-
         #
         # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
         #     return False
@@ -426,7 +433,6 @@ class MainWindow(QMainWindow):
             map_name = "Back"
         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:
             return False
 
@@ -434,7 +440,11 @@ class MainWindow(QMainWindow):
 
         if controlMain.robot_.ClampOpen() == False:
             return False
-        if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == 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:
@@ -442,7 +452,7 @@ class MainWindow(QMainWindow):
 
         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, temp_wheel_base) == False:
             return False
 
         # 双单车出库