1.心跳线程

2.界面默认轴距2.8，地图Back
3.任务队列方式执行Task。
4.添加车库点位

ControllWidget.py

@@ -110,7 +110,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("2.8")
         self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
         self.btnModelCheck = QCheckBox("整体协调", self)
@@ -239,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,"Base")
+            self.robot_.GeneratePath(id1, id2,"Back")
             self.begId_=id1
             self.targetId_=id2
 
@@ -247,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,"Base")
+            self.robot_.GeneratePath(id1, id2,"Back")
             self.begId_=id1
             self.targetId_=id2
 
@@ -263,7 +263,7 @@ class ControlFrame(QFrame):
             if self.btnAutoDirect.checkState()==Qt.Checked:
                 print("自由方向")
                 autoDirect=True
-            self.robot_.GeneratePath(self.begId_, self.targetId_,"Base")
+            self.robot_.GeneratePath(self.begId_, self.targetId_,"Back")
             self.threadPool_.submit(self.robot_.Navigatting,
                                     self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()))
 

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

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": 904, "single_count": 2795}
+{"load_count": 932, "single_count": 2900}

map.json

@@ -1,56 +1,44 @@
 {
   "street_nodes": {
-    "R_65": [
-      -16.665, -7.16
-    ],
-    "R_145": [
-      -8.57, -7.16
-    ],
-    "R_147": [
-      -10.97, -7.16
-    ],
-    "R_1101": [
-      -0.32, -7.16
-    ]
+    "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": {
-    "S_65": [
-      -16.665, -0.980, 89.15
-    ],
-    "S_145": [
-      -8.57, -13.48, -89.15
-    ],
-        "S_147": [
-      -10.96, -13.48, -90
-    ],
-    "S_1101": [
-      -0.32, 1.0, 90
-    ]
+    "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": {
-    "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"
-    ]
+    "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"]
   }
 }

mytool/RabbitMq_helper/async_communication.py

@@ -90,8 +90,7 @@ class RabbitAsyncCommunicator(threading.Thread):
         async with queue.iterator() as queue_iter:
             async for message in queue_iter:
                 if not callback==None:
-                    if callback(message.body.decode())==True:
-                        await asyncio.sleep(0.5)
+                    if await callback(message.body.decode())==True:
                         print("ask")
                         await message.ack()
                         print("ask over")

test.py

@@ -57,7 +57,7 @@ from custom_define import RobotName
 from mytool.RabbitMq_helper import async_communication as rabitmq
 
 import concurrent.futures
-
+import TaskTable
 from queue import Queue
 
 
@@ -70,6 +70,7 @@ 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)
@@ -79,6 +80,11 @@ class MainWindow(QMainWindow):
         self.isLocalTest = False
         self.isAutoDispatchOnline = True
 
+        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"):
@@ -146,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()
@@ -157,10 +177,10 @@ class MainWindow(QMainWindow):
             return False
 
         if self.isAutoDispatchOnline:
-            return 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()
 
@@ -170,13 +190,8 @@ class MainWindow(QMainWindow):
             print("Parse  error\n")
             return False
 
-        # 加入自动任务列表
-        '''if self.isAutoDispatchOnline:
-            temp = time.time()
-            self.auto_task_queue.put((2, pick_table, temp))'''
-
         if self.isAutoDispatchOnline:
-            return self.AutoPickTask(pick_table)
+            return await self.PushPickTask(pick_table)
         return False
 
     def updateMap_park(self, entrance_id, pose, wheelbase):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
@@ -219,6 +234,40 @@ 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")
 
@@ -288,24 +337,35 @@ class MainWindow(QMainWindow):
 
         print("-------------------------")
         # 整车松夹池------------------------------------
-        threadPool = ThreadPoolExecutor(1)
-        threadPool.submit(controlMain.robot_.ClampOpen)
-        threadPool.shutdown(wait=True)
-        if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+        if controlMain.robot_.ClampOpen()==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)
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
 
-        threadPool = ThreadPoolExecutor(2)
-        threadPool.submit(control1.robot_.Navigatting,
-                          target_id, end_street, autoDirect, wheel_base)
-        threadPool.submit(control2.robot_.Navigatting,
-                          target_id, end_street, autoDirect, wheel_base)
-        threadPool.shutdown(wait=True)
+        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:
+            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)
+        # threadPool.submit(control2.robot_.Navigatting,
+        #                   target_id, end_street, autoDirect, wheel_base)
+        # threadPool.shutdown(wait=True)
 
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
@@ -320,8 +380,6 @@ class MainWindow(QMainWindow):
     def AutoPickTask(self, pick_table: message.pick_table = None):
         print("AutoPickTask:---------------------\n")
 
-
-
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
         mainMap, frontMap, backMap = ["Main", "Front", "Back"]
 
@@ -404,7 +462,6 @@ class MainWindow(QMainWindow):
         #     print(" Pick Failed ")
         #     return False
 
-
         print("  over publish...")
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
@@ -452,6 +509,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...")