1.下发流程码（0：默认，1：存车，2：取车）

ControllWidget.py

@@ -41,6 +41,7 @@ class ControlFrame(QFrame):
 
         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)
@@ -144,13 +145,29 @@ class ControlFrame(QFrame):
         self.btnAutoBegChanged.setCheckState(Qt.Checked)
 
         # 选择地图
+        self.mapstatic = QLabel(self)
+        self.mapstatic.setText("地图")
+        self.mapstatic.setGeometry(360, 5, 80, 30)
+
         self.select_map_Qc = QComboBox(self)
-        self.select_map_Qc.setGeometry(360, 10, 100, 20)
+        self.select_map_Qc.setGeometry(400, 10, 100, 20)
         maps = ["Front", "Back", "Base"]
         self.select_map_Qc.addItem("------")
         self.select_map_Qc.addItems(maps)
         self.select_map_Qc.activated.connect(self.select_map_QcChanged)
 
+        # 选择流程
+        self.seq_static = QLabel(self)
+        self.seq_static.setText("流程")
+        self.seq_static.setGeometry(360, 35, 80, 30)
+
+        self.select_seq_Qc = QComboBox(self)
+        self.select_seq_Qc.setGeometry(400, 40, 100, 20)
+        sequence = ["Park", "Pick"]
+        self.select_seq_Qc.addItem("------")
+        self.select_seq_Qc.addItems(sequence)
+        self.select_seq_Qc.activated.connect(self.select_seq_QcChanged)
+
     def Update(self):
         if self.robot_.timedRobotStatu_.timeout() == False:
             x = self.robot_.timedRobotStatu_.statu.x
@@ -276,7 +293,8 @@ class ControlFrame(QFrame):
                 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()))
+                                    self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()),
+                                    self.robot_.current_sequence_)
 
     def btnCancelClick(self):
         self.controller_status = ControllerStatus.eCancel
@@ -289,6 +307,14 @@ class ControlFrame(QFrame):
         mp.MapManager().ResetMap("Back", 0, -float(self.WheelBaseEdit.text()) / 2)
         print("changed selected map to" + self.select_map)
 
+    def select_seq_QcChanged(self):
+        print("当前流程:", self.select_seq_Qc.currentText())
+        if self.select_seq_Qc.currentText() == "Park":
+            self.robot_.SetSequence(1)
+        elif self.select_seq_Qc.currentText() == "Pick":
+            self.robot_.SetSequence(2)
+        else:
+            self.robot_.SetSequence(0)
 
 class CountFrame(QFrame):
     def __init__(self):

RobotData.py

@@ -50,6 +50,9 @@ class Robot(MqttAsync):
         self.currentNavPathNodes_ = None
         self.currentNavPath_ = None
 
+        # 当前正在执行的流程默认0，存车1， 取车2
+        self.current_sequence_: int = 0
+
         self.pathColor_ = [1, 1, 0]
         self.robotColor_ = [0.7, 0.2, 0.3]  # agv当前颜色
         self.name_ = name
@@ -152,7 +155,7 @@ class Robot(MqttAsync):
         return True'''
 
     def GeneratePath(self, begID, endID, mapName):
-        self.currentNavPathNodes_ = MapManager().GetShortestPath(mapName,begID, endID)
+        self.currentNavPathNodes_ = MapManager().GetShortestPath(mapName, begID, endID)
 
         # if task_type == "DoubleAGV":
         #     print("AdjustPath------------------")
@@ -185,15 +188,18 @@ class Robot(MqttAsync):
     阻塞直到导航完成
     '''
 
-    def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type="None"):
+    def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type: int = 0):
         if wheelbase < 2.4 or wheelbase > 3.2:
             print("wheelbase is out of range!\n")
             return False
         print("nav")
 
+        self.SetSequence(task_type)
         # self.ExecPathNodes(autoDirect)
-        if self.ExecNavtask(autoDirect, wheelbase)==False:
+        if self.ExecNavtask(autoDirect, wheelbase) == False:
+            self.ResetSequence()
             return False
+        self.ResetSequence()
 
         if self.IsMainModeStatu():
             Count.TestCount().loadCountAdd()
@@ -264,6 +270,7 @@ class Robot(MqttAsync):
         key = str(uuid.uuid4())
         cmd.key = key
         cmd.action = 0
+        cmd.sequence = self.current_sequence_
 
         actions = self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
@@ -330,7 +337,7 @@ class Robot(MqttAsync):
                     pathNode = message.PathNode()
                     pathNode.x = node.x_
                     pathNode.y = node.y_
-                    pathNode.id=node.id_
+                    pathNode.id = node.id_
 
                     newAction.type = 4
                     if autoDirect:
@@ -643,7 +650,7 @@ class Robot(MqttAsync):
                 cmd.W1 = -speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
             if current_operation == ManualOperationType.eForwardMove:
                 cmd.T1 = 3
-                cmd.V1= speed * sigmoid(step_acc, 0, 1)
+                cmd.V1 = speed * sigmoid(step_acc, 0, 1)
                 cmd.W1 = 0
             if current_operation == ManualOperationType.eBackwardMove:
                 cmd.T1 = 3
@@ -774,26 +781,26 @@ class Robot(MqttAsync):
                 if self.manual_status == ManualOperationType.eCounterclockwiseRotate:
                     cmd.T1 = 1
                     cmd.V1 = 0
-                    cmd.W1 = speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                    cmd.W1 = speed_w / 180 * math.pi * (step_sum - step_acc) / step_sum
                 if self.manual_status == ManualOperationType.eClockwiseRotate:
                     cmd.T1 = 1
                     cmd.V1 = 0
-                    cmd.W1 = -speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                    cmd.W1 = -speed_w / 180 * math.pi * (step_sum - step_acc) / step_sum
                 if self.manual_status == ManualOperationType.eForwardMove:
                     cmd.T1 = 3
-                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eBackwardMove:
                     cmd.T1 = 3
-                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = -speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eLeftMove:
                     cmd.T1 = 2
-                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 if self.manual_status == ManualOperationType.eRightMove:
                     cmd.T1 = 2
-                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.V1 = -speed_v * (step_sum - step_acc) / step_sum
                     cmd.W1 = 0
                 cmd.V2 = cmd.V1
                 cmd.V3 = cmd.V1
@@ -819,44 +826,47 @@ class Robot(MqttAsync):
         print("Manual_Unit_SlowlyStop completed.")
         return True
 
-    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
-
-
-
+    # def GenerateActionMPC(self, beg_name, end_name, autoDirect, wheelbase=0, task_type="None"):
+    #     self.GeneratePath(beg_name, end_name)
+    #     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
+
+    def SetSequence(self, s_type: int):
+        self.current_sequence_ = s_type
+
+    def ResetSequence(self):
+        self.current_sequence_ = 0

message.proto

@@ -79,6 +79,8 @@ message NavCmd
   int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
   string key=2;
   repeated NewAction newActions=5;
+  optional int32 sequence = 6; // 0：无， 1：存车， 2：取车
+
 }
 message NavResponse{
   int32 ret=1;

test.py

@@ -77,8 +77,8 @@ class MainWindow(QMainWindow):
         self.basic()
         self.showMaximized()
         self.count_frame_ = ControllWidget.CountFrame()
-        self.isLocalTest = False
-        self.isAutoDispatchOnline = True
+        self.isLocalTest = True
+        self.isAutoDispatchOnline = False
 
         self.Cancel_ = False
 
@@ -202,7 +202,6 @@ 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))
@@ -210,7 +209,6 @@ 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")
@@ -272,6 +270,7 @@ class MainWindow(QMainWindow):
 
     def AutoParkTask(self, park_table: message.park_table = None):
         print("AutoParkTask:---------------------\n")
+        task_flag = 1  # 存车
 
         [controlMain, control1, control2] = self.GetMainFrontBackAGV()
         mainMap, frontMap, backMap = ["Base", "Front", "Back"]
@@ -300,63 +299,68 @@ class MainWindow(QMainWindow):
         trans_x += wheel_base / 2 * math.cos(trans_a)
         trans_y += wheel_base / 2 * math.sin(trans_a)
 
+        # # # 双单车入库-----------------------------------------------------
+        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
+        #     return False
+        # autoDirect = True
+        # 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("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        #
+        # 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("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, task_flag),
+        #     threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag)
+        # ]
+        #
+        # if self.asyncExecute(features) == False:
+        #     print(" Park Failed ")
+        #     return False
+
+        # # ------------------------------------------
+        # # 双单车驶至入口前巷道点
+        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], wheel_base)
+        entrance_streetNode = entrance_id.replace("S_", "R_")
         if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
             return False
-        autoDirect = True
-        # # 双单车入库-----------------------------------------------------
-        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("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        control1.robot_.GeneratePath(label, entrance_streetNode, frontMap)
+        print("Front: begin:%s   target:%s    wheelBase:%f" % (label, entrance_streetNode, wheel_base))
 
         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("Back: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))
+        control2.robot_.GeneratePath(label, entrance_streetNode, backMap)
+        print("Back: begin:%s   target:%s    wheelBase:%f" % (label, entrance_streetNode, wheel_base))
 
+        autoDirect = True
         threadPool = ThreadPoolExecutor(2)
-        features = [threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base),
-                    threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base)]
-
+        features = [
+        threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag),
+        threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag)
+        ]
         if self.asyncExecute(features) == False:
             print(" Park Failed ")
             return False
 
-        # # ------------------------------------------
-        # # 双单车驶至入口前巷道点
-        # streetNode = entrance_id.replace("S_", "R_")
-        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
-        #     return False
-        # cur_pose1 = control1.robot_.timedRobotStatu_.statu
-        # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
-        # control1.robot_.GeneratePath(label, streetNode, frontMap)
-        # print("Front: begin:%s   target:%s    wheelBase:%f" % (label, streetNode, wheel_base))
-        #
-        # cur_pose2 = control2.robot_.timedRobotStatu_.statu
-        # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
-        # control2.robot_.GeneratePath(label, streetNode, backMap)
-        # print("Back: begin:%s   target:%s    wheelBase:%f" % (label, streetNode, wheel_base))
-        #
-        # autoDirect = True
-        # threadPool = ThreadPoolExecutor(2)
-        # features = [threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base),
-        #             threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base)]
-        # if self.asyncExecute(features) == False:
-        #     print(" Pick Failed ")
-        #     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"
-        # controlMain.robot_.GeneratePath(streetNode, entrance_id, map_name)
-        # if controlMain.robot_.Navigatting(streetNode, entrance_id, True, wheel_base) == 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"
+        controlMain.robot_.GeneratePath(entrance_streetNode, entrance_id, map_name)
+        if controlMain.robot_.Navigatting(entrance_streetNode, entrance_id, True, wheel_base, task_flag) == False:
+            return False
         # # ---------------------------------
 
         print(" input entrance completed!!!!")
@@ -365,16 +369,33 @@ class MainWindow(QMainWindow):
         controlMain.robot_.ClampClose()
 
         # 整车去车位---------------------------------
+        # map_name = "Front"
+        # if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+        #     map_name = "Back"
+        # 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, task_flag)
+
+        # 整车驶离入口到入口航道点
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
+        controlMain.robot_.GeneratePath(entrance_id, entrance_streetNode, map_name)
+        print("begin:%s   target:%s    wheelBase:%f" % (entrance_id, entrance_streetNode, wheel_base))
+        controlMain.robot_.Navigatting(entrance_id, entrance_streetNode, autoDirect, wheel_base, task_flag)
+
+        # 整车从入口航道点到车位
         map_name = "Front"
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
-        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)
+        controlMain.robot_.GeneratePath(entrance_streetNode, target_id, map_name)
+        print("begin:%s   target:%s    wheelBase:%f" % (entrance_streetNode, target_id, wheel_base))
+        controlMain.robot_.Navigatting(entrance_streetNode, target_id, autoDirect, wheel_base, task_flag)
+
 
         print("-------------------------")
         # 整车松夹池------------------------------------
-        if controlMain.robot_.ClampOpen()==False:
+        if controlMain.robot_.ClampOpen() == False:
             return False
 
         map_name = "Front"
@@ -387,9 +408,9 @@ class MainWindow(QMainWindow):
         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, temp_wheel_base) == False:
+        entrance_streetNode = target_id.replace("S_", "R_")
+        controlMain.robot_.GeneratePath(target_id, entrance_streetNode, map_name)
+        if controlMain.robot_.Navigatting(target_id, entrance_streetNode, True, temp_wheel_base, task_flag) == False:
             return False
 
         #
@@ -404,9 +425,9 @@ class MainWindow(QMainWindow):
         #
         # threadPool = ThreadPoolExecutor(2)
         # threadPool.submit(control1.robot_.Navigatting,
-        #                   target_id, end_street, autoDirect, wheel_base)
+        #                   target_id, end_street, autoDirect, wheel_base, task_flag)
         # threadPool.submit(control2.robot_.Navigatting,
-        #                   target_id, end_street, autoDirect, wheel_base)
+        #                   target_id, end_street, autoDirect, wheel_base, task_flag)
         # threadPool.shutdown(wait=True)
 
         # 反馈
@@ -421,6 +442,7 @@ class MainWindow(QMainWindow):
 
     def AutoPickTask(self, pick_table: message.pick_table = None):
         print("AutoPickTask:---------------------\n")
+        task_flag = 2  # 取车
 
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
         mainMap, frontMap, backMap = ["Main", "Front", "Back"]
@@ -433,61 +455,65 @@ class MainWindow(QMainWindow):
         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):
+        #     return False
+        # 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("Front: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        #
+        # 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("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, task_flag),
+        #             threadPool.submit(control2.robot_.Navigatting, label, space_id, autoDirect, wheel_base, task_flag)]
+        # if self.asyncExecute(features) == False:
+        #     print(" Pick Failed ")
+        #     return False
+
+        # # ------------------------------------------
+        # # 双单车驶至车位前巷道点
         self.updateMap_pick(wheel_base)
-        # 双单车入库
+        space_streetNode = space_id.replace("S_", "R_")
         if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
             return False
         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("Front: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        control1.robot_.GeneratePath(label, space_streetNode, frontMap)
+        print("Front: begin:%s   target:%s    wheelBase:%f" % (label, space_streetNode, wheel_base))
 
         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("Back: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
+        control2.robot_.GeneratePath(label, space_streetNode, backMap)
+        print("Back: begin:%s   target:%s    wheelBase:%f" % (label, space_streetNode, 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_streetNode, autoDirect, wheel_base, task_flag),
+        threadPool.submit(control2.robot_.Navigatting, label, space_streetNode, autoDirect, wheel_base, task_flag)
+        ]
         if self.asyncExecute(features) == False:
             print(" Pick Failed ")
             return False
 
-        # # ------------------------------------------
-        # # 双单车驶至车位前巷道点
-        # streetNode = space_id.replace("S_", "R_")
-        # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
-        #     return False
-        # cur_pose1 = control1.robot_.timedRobotStatu_.statu
-        # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
-        # control1.robot_.GeneratePath(label, streetNode, frontMap)
-        # print("Front: begin:%s   target:%s    wheelBase:%f" % (label, streetNode, wheel_base))
-        #
-        # cur_pose2 = control2.robot_.timedRobotStatu_.statu
-        # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
-        # control2.robot_.GeneratePath(label, streetNode, backMap)
-        # print("Back: begin:%s   target:%s    wheelBase:%f" % (label, streetNode, wheel_base))
-        #
-        # autoDirect = True
-        # threadPool = ThreadPoolExecutor(2)
-        # features = [threadPool.submit(control1.robot_.Navigatting, label, streetNode, autoDirect, wheel_base),
-        #             threadPool.submit(control2.robot_.Navigatting, label, streetNode, autoDirect, wheel_base)]
-        # if self.asyncExecute(features) == False:
-        #     print(" Pick Failed ")
-        #     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"
-        # controlMain.robot_.GeneratePath(streetNode, space_id, map_name)
-        # if controlMain.robot_.Navigatting(streetNode, space_id, True, wheel_base) == 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"
+        controlMain.robot_.GeneratePath(space_streetNode, space_id, map_name)
+        if controlMain.robot_.Navigatting(space_streetNode, space_id, True, wheel_base, task_flag) == False:
+            return False
         # # ---------------------------------
 
         # 整车夹持------------------------------------
@@ -503,7 +529,7 @@ 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:
+        if controlMain.robot_.Navigatting(space_id, export_id, True, wheel_base, task_flag) == False:
             return False
 
         # 整车松夹池------------------------------------
@@ -520,9 +546,9 @@ class MainWindow(QMainWindow):
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
 
-        streetNode = export_id.replace("S_", "R_")
-        controlMain.robot_.GeneratePath(export_id, streetNode, map_name)
-        if controlMain.robot_.Navigatting(export_id, streetNode, True, temp_wheel_base) == False:
+        space_streetNode = export_id.replace("S_", "R_")
+        controlMain.robot_.GeneratePath(export_id, space_streetNode, map_name)
+        if controlMain.robot_.Navigatting(export_id, space_streetNode, True, temp_wheel_base, task_flag) == False:
             return False
 
         # 双单车出库
@@ -536,8 +562,10 @@ class MainWindow(QMainWindow):
         # 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)]
+        # features=[
+        # threadPool.submit(control1.robot_.Navigatting,export_id, "F_exportLink", True, wheel_base, task_flag),
+        # threadPool.submit(control2.robot_.Navigatting,export_id, "B_exportLink", True, wheel_base, task_flag)
+        # ]
         # if self.asyncExecute(features) == False:
         #     print(" Pick Failed ")
         #     return False