1.添加自动流程

ControllWidget.py

@@ -261,6 +261,7 @@ class ControlFrame(QFrame):
             if self.btnAutoDirect.checkState()==Qt.Checked:
                 print("自由方向")
                 autoDirect=True
+            self.robot_.GeneratePath(self.begId_, self.targetId_)
             self.threadPool_.submit(self.robot_.Navigatting,
                                     self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()))
 

RobotData.py

@@ -187,7 +187,7 @@ class Robot(MqttAsync):
 
     def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type="None"):
         print("nav")
-        self.GeneratePath(begId, targetId, task_type=task_type)
+
         # self.ExecPathNodes(autoDirect)
         self.ExecNavtask(autoDirect, wheelbase)
 
@@ -815,7 +815,42 @@ class Robot(MqttAsync):
 
     def GenerateActionMPC(self, beg_name, end_name, autoDirect, wheelbase=0, task_type="None"):
         self.GeneratePath(beg_name, end_name, task_type=task_type)
+        actions = list()
+
+        split_path = self.SplitPath(self.currentNavPathNodes_)
+        for action in split_path:
+            [type, nodes] = action
+            if type == "input":
+                [street, space] = nodes
+                protoSpace = self.generateProtoNode(space)
+                protoStreet = self.generateProtoNode(street)
+                act = message.NewAction()
+                act.type = 1
+                act.wheelbase = wheelbase
+                act.spaceNode.CopyFrom(protoSpace)
+                act.streetNode.CopyFrom(protoStreet)
+
+                actions.append(act)
+            if type == "output":
+                [space, street] = nodes
+                protoSpace = self.generateProtoNode(space)
+                protoStreet = self.generateProtoNode(street)
+                act = message.NewAction()
+                act.type = 2
+                act.wheelbase = wheelbase
+                act.spaceNode.CopyFrom(protoSpace)
+                act.streetNode.CopyFrom(protoStreet)
 
+                actions.append(act)
+            if type == "nav":
+                act = self.CreateNavPathNodesAction(nodes, autoDirect)
+                if act is None:
+                    print("Nav cmd is None")
+                    return False
+                else:
+                    actions.append(act)
+
+        return actions
 
 
 

map.json

@@ -1,39 +1,60 @@
 {
   "street_nodes": {
-    "Input_R147": [
+    "FInput_R147": [
+      -10.98, -5.66
+    ],
+    "CInput_R147": [
       -10.98, -7.16
     ],
-    "Input_R1100": [
-      -0.32, -7.16
+    "BInput_R147": [
+      -10.98, -8.66
+    ],
+     "FInput_R1100": [
+      -0.32, -5.66
     ],
-    "Input_R1101": [
+    "CInput_R1100": [
       -0.32, -7.16
+    ],
+     "BInput_R1100": [
+      -0.32, -8.66
     ]
   },
 
   "space_nodes": {
     "S147": [
-      -10.97, -13.42, -90
-    ],
-    "S1101": [
-      -0.32, 1.0, 90
+      -10.97, -13.48, -90
     ],
     "S1100": [
       -0.32, 1.0, 90
+    ],
+    "S65": [
+      -16.665, -1.120, 89.15
     ]
   },
   "roads": {
     "road_Input": [
-      "Input_R147",
-      "Input_R1100"
+      "CInput_R147",
+      "CInput_R1100"
+    ],
+    "road_InputF": [
+      "FInput_R147",
+      "FInput_R1100"
+    ],
+    "road_InputB": [
+      "BInput_R147",
+      "BInput_R1100"
     ],
-    "road_147": [
+    "road_147C": [
       "S147",
-      "Input_R147"
+      "CInput_R147"
     ],
-    "road_1100": [
-      "S1100",
-      "Input_R1100"
+    "road_147F": [
+      "S147",
+      "FInput_R147"
+    ],
+    "road_147B": [
+      "S147",
+      "BInput_R147"
     ]
   }
-}
+}

test.py

@@ -49,12 +49,27 @@ import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
 import google.protobuf.text_format as tf
-
+from concurrent.futures import ThreadPoolExecutor
 import uuid
 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"
+    ]
+'''
+
 
 # ===============================================================================
 
@@ -144,17 +159,69 @@ class MainWindow(QMainWindow):
 
     def recv_pick_request(self, msg):
         print("Recv_pick_request------------------\n", msg)
-        park_table = message.park_table()
-        return
+        pick_table = message.pick_table()
+        try:
+            tf.Parse(msg, pick_table)
+        except Exception as e:
+            print("Parse  error\n")
+        # split_msg = msg.split(' ')
+        # result = [float(split_msg[0]), float(split_msg[1]), float(split_msg[2])]
+        if self.isAutoDispatchOnline:
+            self.AutoPickTask(pick_table)
+
+    def updateMap_park(self, entrance_id, pose, type):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
+        self.djks_map_.Reset()  # 重置地图
+
+        trans_x, trans_y, trans_a = pose
+        self.djks_map_.AddVertex_t(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))  # 更新库位点
+
+        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_id, value in self.djks_map_.VertexDict().items():
+                if node_id.find("FInput") >= 0:
+                    self.djks_map_.AddEdge_t("FInput_R1101", node_id)
+
+            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)
+
+        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"])
 
     def AutoParkTask(self, park_table: message.park_table = None):
         print("AutoParkTask:---------------------\n")
-        self.djks_map_.Reset()  # 重置地图
+        controlMain=self.Controller1
+
+        control1=self.Controller1
+        control2=self.Controller2
+        if control1.robot_.timedRobotStatu_.statu.theta<0 :
+            control1 = self.Controller2
+            control2 = self.Controller1
+
+
         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]
+                                                          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
@@ -167,30 +234,80 @@ class MainWindow(QMainWindow):
         target_id = "S" + str(park_table.allocated_space_info.table_id)  # "S147"
         print("target:", target_id)
 
+        # 轴距
         wheel_base = park_table.entrance_measure_info.measure_info_to_plc_forward.wheelbase
-        self.Controller1.WheelBaseEdit.setText(str(wheel_base))
-        self.Controller2.WheelBaseEdit.setText(str(wheel_base))
+        control1.WheelBaseEdit.setText(str(wheel_base))
+        control2.WheelBaseEdit.setText(str(wheel_base))
 
         trans_x += wheel_base / 2 * math.cos(trans_a)
         trans_y += wheel_base / 2 * math.sin(trans_a)
-        self.djks_map_.ResetSpaceNode(entrance_id, trans_x, trans_y)  # 更新库位点
 
-        entrance_street_node = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
-        self.djks_map_.ResetStreetNode("Input_R1101", entrance_street_node[0], entrance_street_node[1])  # 更新库位点对应马路点
-        print("entrance_space pose: ", self.djks_map_.map_t.graph_.points[entrance_id])
-        print("entrance_street pose ", self.djks_map_.map_t.graph_.points["Input_R1101"])
+        # # 开始系列子流程-----------------------------------------------------
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 0)
+
+        cur_pose1 = control1.robot_.timedRobotStatu_.statu
+        [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y])
+        control1.robot_.GeneratePath(label, entrance_id)
+        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)
+
+        cur_pose2 = control2.robot_.timedRobotStatu_.statu
+        [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
+        self.Controller2.robot_.GeneratePath(label, entrance_id)
+        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)
+        threadPool.submit(control2.robot_.Navigatting,
+                          label, entrance_id, autoDirect, wheel_base)
+
+        threadPool.shutdown(wait=True)
+
+        print(" input entrance completed!!!!")
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
+        threadPool.shutdown(wait=True)
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.ClampClose)
+        threadPool.shutdown(wait=True)
+
+        # 整车入库---------------------------------
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 2)
+        controlMain.robot_.GeneratePath(entrance_id, target_id)
+        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)
+        threadPool.shutdown(wait=True)
+
+        # 整车松夹池------------------------------------
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.ClampOpen)
+        threadPool.shutdown(wait=True)
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
+        threadPool.shutdown(wait=True)
+
+        # 双单车出库
+        self.djks_map_.Reset()  # 重置地图
+        control1.robot_.GeneratePath(target_id, "FInput_R147")
+        control2.robot_.GeneratePath(target_id, "BInput_R147")
 
-        # 加临时边
-        self.djks_map_.AddEdge_t(entrance_id, "Input_R1101")
-        # 加临时边
-        for node_id, value in self.djks_map_.VertexDict().items():
-            if node_id.find("Input") >= 0:
-                self.djks_map_.AddEdge_t("Input_R1101", node_id)
+        threadPool = ThreadPoolExecutor(2)
+        threadPool.submit(control1.robot_.Navigatting,
+                          target_id, "FInput_R147", autoDirect, wheel_base)
+        threadPool.submit(control2.robot_.Navigatting,
+                          target_id, "BInput_R147", autoDirect, wheel_base)
+
+        threadPool.shutdown(wait=True)
 
-        # # 开始系列子流程-----------------------------------------------------
         # # 主车进入口
-        # cur_main_pose = self.Controller1.robot_.timedRobotStatu_.statu
-        # [label, pt] = mp.MapManager().findNeastNode([cur_main_pose.x, cur_main_pose.y])
+        #
+        #
         # node = mp.MapManager().GetVertex(label)# StreetNode
         # self.Controller1.robot_.Navigatting(label, entrance_id, True, wheel_base)
         # # 副车进入口
@@ -200,14 +317,133 @@ class MainWindow(QMainWindow):
         # 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")
+
+        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_.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"])
 
 
-        # 恢复
-        # self.djks_map_.Reset()
+
+
+    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
+
+
+        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"
+        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(export_id, 2)
+
+
+        # 双单车入库
+
+        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))
+
+        cur_pose2 = control2.robot_.timedRobotStatu_.statu
+        [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
+        control2.robot_.GeneratePath(label, space_id)
+        print("Child: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+
+        autoDirect = True
+        threadPool = ThreadPoolExecutor(2)
+        threadPool.submit(control1.robot_.Navigatting,
+                          label, space_id, autoDirect, wheel_base)
+        threadPool.submit(control2.robot_.Navigatting,
+                          label, space_id, autoDirect, wheel_base)
+        threadPool.shutdown(wait=True)
+        
+
+
+        # 整车夹持------------------------------------
+        print(" input space completed!!!!")
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
+        threadPool.shutdown(wait=True)
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.ClampClose)
+        threadPool.shutdown(wait=True)
+        
+
+        # 整车到出口---------------------------------
+        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)
+        
+
+        # 整车松夹池------------------------------------
+
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.ClampOpen)
+        threadPool.shutdown(wait=True)
+        threadPool = ThreadPoolExecutor(1)
+        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
+        threadPool.shutdown(wait=True)
+
+        # 双单车出库
+
+        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")
+
+        threadPool = ThreadPoolExecutor(2)
+        threadPool.submit(control1.robot_.Navigatting,
+                          export_id, "FInput_R1100", True, wheel_base)
+        threadPool.submit(control2.robot_.Navigatting,
+                          export_id, "BInput_R1100", True, wheel_base)
+        threadPool.shutdown(wait=True)
 
     def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
         """
@@ -215,11 +451,18 @@ class MainWindow(QMainWindow):
         """
         n_x, n_y = 0, 0
         if s_id == "S1101":
-            n_y = self.djks_map_.map_t.graph_.points["Input_R1100"][1]
+            n_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][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_xF = (n_yF - s_y) / k + s_x  # 弧度
+
+            n_yB = self.djks_map_.map_t.graph_.points["BInput_R1100"][1]
+            n_xB = (n_yB - s_y) / k + s_x  # 弧度
+
         # print(n_x, n_y)
-        return [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