1.调整controler控制序列

RobotData.py

@@ -603,7 +603,7 @@ class Robot(MqttAsync):
     def ManualTask(self, current_operation, step_acc=0, speed=0):
         if self.IsNavigating():
             print("AGV is navigating!!!")
-            return
+            # return False
 
         # slowly stop
         if current_operation == ManualOperationType.eReady and \

map.json

@@ -1,5 +1,14 @@
 {
   "street_nodes": {
+    "FInput_R145": [
+      -8.57, -5.66
+    ],
+    "CInput_R145": [
+      -8.57, -7.16
+    ],
+    "BInput_R145": [
+      -8.57, -8.66
+    ],
     "FInput_R147": [
       -10.98, -5.66
     ],
@@ -17,6 +26,24 @@
     ],
      "BInput_R1100": [
       -0.32, -8.66
+    ],
+    "FInput_R65": [
+      -16.665, -5.66
+    ],
+    "CInput_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
     ]
   },
 
@@ -29,21 +56,57 @@
     ],
     "S65": [
       -16.665, -1.120, 89.15
+    ],
+    "S145": [
+      -8.57, -13.48, -89.15
     ]
   },
   "roads": {
     "road_Input": [
+      "CInput_R145",
       "CInput_R147",
-      "CInput_R1100"
+      "CInput_R1100",
+      "CInput_R65"
     ],
     "road_InputF": [
+      "FInput_R145",
       "FInput_R147",
-      "FInput_R1100"
+      "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": [
+      "S145",
+      "CInput_R145"
+    ],
+    "road_145F": [
+      "S145",
+      "FInput_R145"
+    ],
+    "road_145B": [
+      "S145",
+      "BInput_R145"
+    ],
+
     "road_147C": [
       "S147",
       "CInput_R147"
@@ -55,6 +118,19 @@
     "road_147B": [
       "S147",
       "BInput_R147"
+    ],
+
+    "road_65C": [
+      "S65",
+      "CInput_R65"
+    ],
+    "road_65F": [
+      "S65",
+      "FInput_R65"
+    ],
+    "road_65B": [
+      "S65",
+      "BInput_R65"
     ]
   }
 }

test.py

@@ -55,21 +55,6 @@ from mytool.txt_helper.txt_operation import TXTOperation
 from custom_define import RobotName
 from mytool.RabbitMq_helper import async_communication as rabitmq
 
-'''
-"road_1100F": [
-      "S1100",
-      "FInput_R1100"
-    ],
-    "road_1100": [
-      "S1100",
-      "CInput_R1100"
-    ],
-    "road_1100B": [
-      "S1100",
-      "BInput_R1100"
-    ]
-'''
-
 
 # ===============================================================================
 
@@ -156,6 +141,7 @@ class MainWindow(QMainWindow):
         # result = [float(split_msg[0]), float(split_msg[1]), float(split_msg[2])]
         if self.isAutoDispatchOnline:
             self.AutoParkTask(park_table)
+        # self.g_rabbitmq.publish()
 
     def recv_pick_request(self, msg):
         print("Recv_pick_request------------------\n", msg)
@@ -205,17 +191,24 @@ class MainWindow(QMainWindow):
                 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"])
-
+    def GetMainFrontBackAGV(self):
+        controlMain = self.Controller1
+        nagtive = False
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            nagtive = True
+        control1 = None
+        control2 = None
+        if nagtive == False:
+            control1 = self.Controller1
+            control2 = self.Controller2
+        else:
+            control2 = self.Controller1
+            control1 = self.Controller2
+        return [controlMain,control1,control2]
     def AutoParkTask(self, park_table: message.park_table = None):
         print("AutoParkTask:---------------------\n")
-        controlMain=self.Controller1
-
-        control1=self.Controller1
-        control2=self.Controller2
-        if control1.robot_.timedRobotStatu_.statu.theta<0 :
-            control1 = self.Controller2
-            control2 = self.Controller1
 
+        [controlMain,control1,control2]=self.GetMainFrontBackAGV()
 
         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,
@@ -223,9 +216,12 @@ class MainWindow(QMainWindow):
                                                   (
                                                           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.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
         trans_a = entrance_theta + da - math.pi
         while trans_a < 0:
             trans_a += math.pi
@@ -242,7 +238,12 @@ 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)
 
         cur_pose1 = control1.robot_.timedRobotStatu_.statu
@@ -255,7 +256,7 @@ class MainWindow(QMainWindow):
 
         cur_pose2 = control2.robot_.timedRobotStatu_.statu
         [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
-        self.Controller2.robot_.GeneratePath(label, entrance_id)
+        control2.robot_.GeneratePath(label, entrance_id)
         print("Child: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
 
         autoDirect = True
@@ -293,6 +294,7 @@ class MainWindow(QMainWindow):
         threadPool.shutdown(wait=True)
 
         # 双单车出库
+        controlMain, control1, control2 = self.GetMainFrontBackAGV()
         self.djks_map_.Reset()  # 重置地图
         control1.robot_.GeneratePath(target_id, "FInput_R147")
         control2.robot_.GeneratePath(target_id, "BInput_R147")
@@ -305,38 +307,16 @@ class MainWindow(QMainWindow):
 
         threadPool.shutdown(wait=True)
 
-        # # 主车进入口
-        #
-        #
-        # node = mp.MapManager().GetVertex(label)# StreetNode
-        # self.Controller1.robot_.Navigatting(label, entrance_id, True, wheel_base)
-        # # 副车进入口
-        # cur_child_pose = self.Controller2.robot_.timedRobotStatu_.statu
-        # [label, pt] = mp.MapManager().findNeastNode([cur_child_pose.x, cur_child_pose.y])
-        # node = mp.MapManager().GetVertex(label)# StreetNode
-        # self.Controller2.robot_.Navigatting(label, entrance_id, True, wheel_base)
-        # # 切换整车模式
-        # self.Controller1.robot_.MainAgvchangecb()
-        # # 整车夹持
-        # self.Controller1.robot_.ClampClose()
-        # # 整车进目标车位
-        # self.Controller1.robot_.Navigatting(entrance_id, target_id, True, wheel_base)
-        # #
-        # # ---------------------------------------------------------------
-        # 恢复
-        # self.djks_map_.Reset()
-
-    def updateMap_pick(self, export_id, type):# 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
-        print("5555555")
+    def updateMap_pick(self, export_id, type):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
 
         self.djks_map_.Reset()  # 重置地图
         # export_id = "S1103"
         # 添加出口车位点
         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_.map_t.graph_.points["S1100"][1], 90))  # 更新库位点对应马路点
+        if type == 0:
             self.djks_map_.AddEdge_t(export_id, "FInput_R1100")
-        elif type==1:
+        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]
@@ -351,20 +331,10 @@ class MainWindow(QMainWindow):
                     self.djks_map_.AddEdge_t("CInput_R1103", node_id)
             print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1103"])
 
-
-
-
     def AutoPickTask(self, pick_table: message.pick_table = None):
         print("AutoPickTask:---------------------\n")
 
-        controlMain = self.Controller1
-
-        control1 = self.Controller1
-        control2 = self.Controller2
-        if control1.robot_.timedRobotStatu_.statu.theta < 0:
-            control1 = self.Controller2
-            control2 = self.Controller1
-
+        controlMain, control1, control2 = self.GetMainFrontBackAGV()
 
         self.djks_map_.Reset()  # 重置地图
         space_id = "S" + str(pick_table.actually_space_info.table_id)  # "S147"
@@ -376,13 +346,14 @@ class MainWindow(QMainWindow):
         control1.WheelBaseEdit.setText(str(wheel_base))
         control2.WheelBaseEdit.setText(str(wheel_base))
 
-        # self.updateMap_pick(export_id, 2)
-
-
         # 双单车入库
+        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)
         print("Main: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
@@ -399,8 +370,6 @@ class MainWindow(QMainWindow):
         threadPool.submit(control2.robot_.Navigatting,
                           label, space_id, autoDirect, wheel_base)
         threadPool.shutdown(wait=True)
-        
-
 
         # 整车夹持------------------------------------
         print(" input space completed!!!!")
@@ -410,17 +379,15 @@ class MainWindow(QMainWindow):
         threadPool = ThreadPoolExecutor(1)
         threadPool.submit(controlMain.robot_.ClampClose)
         threadPool.shutdown(wait=True)
-        
 
         # 整车到出口---------------------------------
-        self.updateMap_pick(export_id,2)
+        self.updateMap_pick(export_id, 2)
         controlMain.robot_.GeneratePath(space_id, export_id)
         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)
-        
 
         # 整车松夹池------------------------------------
 
@@ -432,7 +399,7 @@ class MainWindow(QMainWindow):
         threadPool.shutdown(wait=True)
 
         # 双单车出库
-
+        controlMain, control1, control2 = self.GetMainFrontBackAGV()
         self.updateMap_pick(export_id, 0)
         control1.robot_.GeneratePath(export_id, "FInput_R1100")
         self.updateMap_pick(export_id, 1)