1.优化提升控制

2.添加页面切换

ControllWidget.py

@@ -109,7 +109,7 @@ class ControlFrame(QFrame):
         self.wheelbasestatic.setText("轴距:")
         self.wheelbasestatic.setGeometry(260, 100, 40, 30)
         self.WheelBaseEdit = QLineEdit(self)
-        self.WheelBaseEdit.setText("2.78")
+        self.WheelBaseEdit.setText("0")
         self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
         self.btnModelCheck = QCheckBox("整体协调", self)
@@ -163,11 +163,13 @@ class ControlFrame(QFrame):
             self.endSpaceQc.setEnabled(False)
             self.btnModelCheck.setEnabled(False)
             self.btnClampCheck.setEnabled(False)
+            self.btnLifterCheck.setEnabled(False)
         else:
             self.endStreetQc.setEnabled(True)
             self.endSpaceQc.setEnabled(True)
             self.btnModelCheck.setEnabled(True)
             self.btnClampCheck.setEnabled(True)
+            self.btnLifterCheck.setEnabled(True)
         self.LedLabel.setStyleSheet(statu)
 
         self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
@@ -183,6 +185,11 @@ class ControlFrame(QFrame):
         else:
             self.btnClampCheck.setCheckState(Qt.Unchecked)
 
+        if self.robot_.IsLiferRose():
+            self.btnLifterCheck.setCheckState(Qt.Checked)
+        else:
+            self.btnLifterCheck.setCheckState(Qt.Unchecked)
+
         if self.robot_.IsNavigating():
             self.begId_ = self.robot_.begId_
             self.targetId_ = self.robot_.targetId_
@@ -214,10 +221,10 @@ class ControlFrame(QFrame):
             self.threadPool_.submit(self.robot_.ClampOpen)
 
     def LifterClick(self):
-        if self.robot_.IsLiferDowned() == False:
-            self.threadPool_.submit(self.robot_.LiferDown)
-        else:
+        if self.robot_.IsLiferRose() == False:
             self.threadPool_.submit(self.robot_.LiferRise)
+        else:
+            self.threadPool_.submit(self.robot_.LiferDown)
 
     def MainAgvchangecb(self):
         if self.robot_.IsMainModeStatu() == False:

ManualOperationWidget.py

@@ -1,18 +1,19 @@
+import inspect
+import sys
+import time
 from concurrent.futures import ThreadPoolExecutor
 
-from PyQt5.QtCore import QTimer
+from PyQt5.QtCore import QTimer, Qt
 from PyQt5.QtWidgets import QFrame
 
 from UI.manual_operation import Ui_manual_operation_widget
-
-
 from custom_define import ManualOperationType, RobotName
 
 
 class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
     threadPool_ = ThreadPoolExecutor(1)
 
-    def __init__(self, robot_dict:dict):
+    def __init__(self, robot_dict: dict):
         super().__init__()
         self.setupUi(self)
 
@@ -41,15 +42,29 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
         self.Left_Move_btn.released.connect(self.Cancel)
         self.Right_Move_btn.pressed.connect(self.Right_Move)
         self.Right_Move_btn.released.connect(self.Cancel)
+        # 夹持/抬升
+        self.Clamp_Close_btn.pressed.connect(self.Clamp_Close)
+        self.Clamp_Close_btn.released.connect(self.Cancel)
+        self.Clamp_Open_btn.pressed.connect(self.Clamp_Open)
+        self.Clamp_Open_btn.released.connect(self.Cancel)
+        self.Lifter_Rise_btn.pressed.connect(self.Lifter_Rise)
+        self.Lifter_Rise_btn.released.connect(self.Cancel)
+        self.Lifter_Down_btn.pressed.connect(self.Lifter_Down)
+        self.Lifter_Down_btn.released.connect(self.Cancel)
+        # 设定速度
+        self.speed_lineEdit.textChanged.connect(self.speed_lineEdit_change)
 
         # 加减速阶梯
         self.step_acc: int = 0
 
         # 启动界面刷新定时器
         self.timer = QTimer()
-        self.timer.timeout.connect(self.DownCmd)
+        self.timer.timeout.connect(self.Update)
         self.timer.start(50)
 
+        # 操作计时
+        self.manual_task_beg = 0
+
     def SelectRobot(self):
         if self.robot_dict[RobotName.strAGVMain].IsMainModeStatu():
             self.selected_robot_name = RobotName.strAGVMain
@@ -57,40 +72,133 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
             self.selected_robot_name = self.select_box.currentText()
 
     def Counterclockwise_Rotate(self):
-        self.current_operation = ManualOperationType.eCounterclockwiseRotate
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            print("eCounterclockwiseRotate")
+            self.current_operation = ManualOperationType.eCounterclockwiseRotate
 
     def Clockwise_Rotate(self):
-        self.current_operation = ManualOperationType.eClockwiseRotate
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("Clockwise_Rotate | ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            print("eCounterclockwiseRotate")
+            self.current_operation = ManualOperationType.eClockwiseRotate
 
     def Forward_Move(self):
-        self.current_operation = ManualOperationType.eForwardMove
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eForwardMove
 
     def Backward_Move(self):
-        self.current_operation = ManualOperationType.eBackwardMove
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eBackwardMove
 
     def Left_Move(self):
-        self.current_operation = ManualOperationType.eLeftMove
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eLeftMove
 
     def Right_Move(self):
-        self.current_operation = ManualOperationType.eRightMove
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eRightMove
+
+    def Lifter_Rise(self):
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+        if self.current_operation == ManualOperationType.eReady:
+            print("Lifter_Rise")
+            self.current_operation = ManualOperationType.eLifterRise
+
+    def Lifter_Down(self):
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eLifterDown
+
+    def Clamp_Close(self):
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eClampClose
+
+    def Clamp_Open(self):
+        if self.robot_dict[self.selected_robot_name].IsManualTaskRunning():
+            return print("ManualTask is running...")
+        if self.current_operation == ManualOperationType.eReady:
+            self.current_operation = ManualOperationType.eClampOpen
 
     def Cancel(self):
         self.current_operation = ManualOperationType.eReady
         self.operation_count[ManualOperationType.eReady] += 1
 
+    def Update(self):
+        self.DownCmd()
+        self.UIRefresh()
+
+    def speed_lineEdit_change(self):
+        try:
+            if not 0 <= float(self.speed_lineEdit.text()) <= 99:
+                self.speed_lineEdit.setText("0")
+        except:
+            self.speed_lineEdit.setText("0")
+
+    def speed_lineEdit_check(self):
+        try:
+            if not 0 <= float(self.speed_lineEdit.text()) <= 99:
+                self.speed_lineEdit.setText("0")
+            else:
+                return True
+        except:
+            self.speed_lineEdit.setText("0")
+            return False
+
     def DownCmd(self):
-        if self.current_operation != ManualOperationType.eReady:
-            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc)
+        if ManualOperationType.eCounterclockwiseRotate <= self.current_operation <= ManualOperationType.eClockwiseRotate:
+            speed = 0.0
+            if self.speed_lineEdit_check():
+                speed = float(self.speed_lineEdit.text())
+            speed = min(speed, 3)
+            self.speed_lineEdit.setText(str(speed))
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc, speed=speed)
             self.step_acc += 1
             # self.threadPool_.submit(self.robot_dict[self.selected_robot_name].ManualTask, self.current_operation)
-        elif self.operation_count[ManualOperationType.eReady] != 0:
+        elif ManualOperationType.eForwardMove <= self.current_operation <= ManualOperationType.eRightMove:
+            speed = 0.0
+            if self.speed_lineEdit_check():
+                speed = float(self.speed_lineEdit.text())
+            speed = min(speed, 0.5)
+            self.speed_lineEdit.setText(str(speed))
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc, speed=speed)
+            self.step_acc += 1
+        elif ManualOperationType.eClampClose <= self.current_operation <= ManualOperationType.eClampOpen:
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation)
+        elif ManualOperationType.eLifterRise <= self.current_operation <= ManualOperationType.eLifterDown:
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation)
+        elif self.current_operation == ManualOperationType.eReady and \
+                self.operation_count[ManualOperationType.eReady] != 0:
             # self.threadPool_.shutdown(False)
             self.threadPool_.submit(self.robot_dict[self.selected_robot_name].CancelNavTask)
             self.current_operation = ManualOperationType.eReady
             self.operation_count[ManualOperationType.eReady] = 0
             self.step_acc = 0
         else:
-            self.current_operation = ManualOperationType.eReady
-            self.operation_count[ManualOperationType.eReady] = 0
-            self.step_acc = 0
+            pass
+            # self.current_operation = ManualOperationType.eReady
+            # self.operation_count[ManualOperationType.eReady] = 0
+            # self.step_acc = 0
 
+    def UIRefresh(self):
+        pass

RobotData.py

@@ -21,11 +21,11 @@ class TimeStatu:
     def __init__(self, statu=None, timeout=3):
         self.statu = statu
         self.time = time.time()
-        self.timeout_ms = timeout
+        self.timeout_s = timeout
 
     def timeout(self):
         tm = time.time()
-        return tm - self.time > self.timeout_ms or self.statu == None
+        return tm - self.time > self.timeout_s or self.statu == None
 
 
 class Robot(MqttAsync):
@@ -40,7 +40,7 @@ class Robot(MqttAsync):
         self.messageArrivedSignal = None
         self.currentNavData_ = None
         self.navCmdTopic_ = None
-        if(name == RobotName.strAGVMain):
+        if (name == RobotName.strAGVMain):
             self.speedCmdTopic = "monitor_child/speedcmd"
         else:
             self.speedCmdTopic = "monitor_child/speedcmd"
@@ -61,6 +61,8 @@ class Robot(MqttAsync):
         self.W_ = 2.5  # 宽
         self.heat_ = 0  # 心跳
 
+        self.manual_status = ManualOperationType.eReady  # 正在进行的手动操作
+
     def Color(self):
         if self.IsMainModeStatu():
             return [0, 0, 0]
@@ -231,9 +233,13 @@ class Robot(MqttAsync):
             if runningStatu.statu == 4:
                 return ActType.eMPC
             if runningStatu.statu == 5:
-                return ActType.eClampOpen
-            if runningStatu.statu == 6:
                 return ActType.eClampClose
+            if runningStatu.statu == 6:
+                return ActType.eClampOpen
+            if runningStatu.statu == 7:
+                return ActType.eLifterRise
+            if runningStatu.statu == 8:
+                return ActType.eLifterDown
         else:
             return ActType.eReady
 
@@ -443,6 +449,7 @@ class Robot(MqttAsync):
         if mode == 1:
             action.wheelbase = wheelbase
         cmd.newActions.add().CopyFrom(action)
+        print(cmd)
         channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
@@ -459,18 +466,6 @@ class Robot(MqttAsync):
 
         return False
 
-    def IsLiferDowned(self):
-        if self.timedRobotStatu_.timeout() == False:
-            if self.IsMainModeStatu():
-                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
-                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
-                return (clamp == 1 and clamp_other == 1)
-            else:
-                return self.timedRobotStatu_.statu.agvStatu.clamp == 1
-
-        return False
-
-
     def IsClampRunning(self):
         if self.timedRobotStatu_.timeout() == False:
             if self.IsMainModeStatu():
@@ -525,6 +520,68 @@ class Robot(MqttAsync):
         response = stub.Start(cmd)
         return True
 
+    def IsLiferDowned(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                lifter = self.timedRobotStatu_.statu.agvStatu.lifter
+                lifter_other = self.timedRobotStatu_.statu.agvStatu.lifter_other
+                return (lifter == 2 and lifter_other == 2)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.lifter == 2
+        return False
+
+    def IsLiferRunning(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                lifter = self.timedRobotStatu_.statu.agvStatu.lifter
+                lifter_other = self.timedRobotStatu_.statu.agvStatu.lifter_other
+                return (lifter == 0 and lifter_other == 0)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.lifter == 0
+        return False
+
+    def IsLiferRose(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                lifter = self.timedRobotStatu_.statu.agvStatu.lifter
+                lifter_other = self.timedRobotStatu_.statu.agvStatu.lifter_other
+                return (lifter == 1 and lifter_other == 1)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.lifter == 1
+        return False
+
+    def LiferRise(self):
+        if self.IsLiferRose() == True:
+            print("lifter has rose")
+            return True
+        cmd = message.NavCmd()
+        cmd.action = 0
+        key = str(uuid.uuid4())
+        cmd.key = (key)
+        act = message.NewAction()
+        act.type = 9
+        cmd.newActions.add().CopyFrom(act)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
+        stub = msg_rpc.NavExcutorStub(channel)
+        response = stub.Start(cmd)
+        return True
+
+    def LiferDown(self):
+        if self.IsLiferDowned() == True:
+            print("lifter has downed")
+            return True
+        cmd = message.NavCmd()
+        cmd.action = 0
+        key = str(uuid.uuid4())
+        cmd.key = (key)
+        act = message.NewAction()
+        act.type = 10
+        cmd.newActions.add().CopyFrom(act)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
+        stub = msg_rpc.NavExcutorStub(channel)
+        response = stub.Start(cmd)
+        return True
+
     def CancelNavTask(self):
         cmd = message.NavCmd()
         cmd.action = 3
@@ -537,11 +594,85 @@ class Robot(MqttAsync):
         print(" Cancel task completed!!!")
         return True
 
-    def ManualTask(self, current_operation, step_acc):
+    def IsManualTaskRunning(self):
+        return self.manual_status != ManualOperationType.eReady
+
+    def ManualTask(self, current_operation, step_acc=0, speed=0):
+        print("speed: ", speed)
         if self.IsNavigating():
             print("AGV is navigating!!!")
             return
+        if self.IsManualTaskRunning():
+            return print("ManualTask | ManualTask is running...")
+        self.manual_status = current_operation
+
+        if ManualOperationType.eReady < current_operation <= ManualOperationType.eRightMove:
+            cmd = message.ToAgvCmd()
+            self.heat_ += 1
+            self.heat_ %= 255
+            cmd.H = self.heat_
+            if self.IsMainModeStatu():
+                cmd.M = 1
+            else:
+                cmd.M = 0
+
+            if current_operation == ManualOperationType.eCounterclockwiseRotate:
+                cmd.T = 1
+                cmd.V = 0
+                cmd.W = speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
+            if current_operation == ManualOperationType.eClockwiseRotate:
+                cmd.T = 1
+                cmd.V = 0
+                cmd.W = -speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
+            if current_operation == ManualOperationType.eForwardMove:
+                cmd.T = 3
+                cmd.V = speed * sigmoid(step_acc, 0, 1)
+                cmd.W = 0
+            if current_operation == ManualOperationType.eBackwardMove:
+                cmd.T = 3
+                cmd.V = -speed * sigmoid(step_acc, 0, 1)
+                cmd.W = 0
+            if current_operation == ManualOperationType.eLeftMove:
+                cmd.T = 2
+                cmd.V = speed * sigmoid(step_acc, 0, 1)
+                cmd.W = 0
+            if current_operation == ManualOperationType.eRightMove:
+                cmd.T = 2
+                cmd.V = -speed * sigmoid(step_acc, 0, 1)
+                cmd.W = 0
+            cmd.L = self.L_
+            cmd.end = 1
+
+            # 处理为json格式
+            cmd_dict = {}
+            for field in cmd.DESCRIPTOR.fields:
+                cmd_dict[field.name] = field.default_value
+            for field, value in cmd.ListFields():
+                cmd_dict[field.name] = value
+            cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
+            self.publish(self.speedCmdTopic, cmd_json)
+
+            print(cmd_json)
+
+            # channel = grpc.insecure_channel(self.rpc_ipport_)
+            # stub = msg_rpc.NavExcutorStub(channel)
+            # response = stub.ManualOperation(cmd_0)
+            # print("client received: ", response)
+            self.manual_status = ManualOperationType.eReady
+            # print("ManualOperation {op_type} task completed!!!".format(op_type=hex(current_operation)))
+            return True
+
+        elif ManualOperationType.eClampClose <= current_operation <= ManualOperationType.eClampOpen:
+            self.Manual_Unit_Clamp(current_operation)
+            return True
 
+        elif ManualOperationType.eLifterRise <= current_operation <= ManualOperationType.eLifterDown:
+            self.Manual_Unit_Lifter(current_operation)
+            return True
+        return False
+
+    def Manual_Unit_Lifter(self, current_operation: ManualOperationType):
+        print(current_operation)
         cmd = message.ToAgvCmd()
         self.heat_ += 1
         self.heat_ %= 255
@@ -550,35 +681,9 @@ class Robot(MqttAsync):
             cmd.M = 1
         else:
             cmd.M = 0
+        cmd.T = self.ManualOperationType2AgvCmdType(current_operation)
 
-        if current_operation == ManualOperationType.eCounterclockwiseRotate:
-            cmd.T = 1
-            cmd.V = 0
-            cmd.W = 4.0 / 180 * math.pi * sigmoid(step_acc, 0, 1)
-        if current_operation == ManualOperationType.eClockwiseRotate:
-            cmd.T = 1
-            cmd.V = 0
-            cmd.W = -4.0 / 180 * math.pi * sigmoid(step_acc, 0, 1)
-        if current_operation == ManualOperationType.eForwardMove:
-            cmd.T = 3
-            cmd.V = 0.6 * sigmoid(step_acc, 0, 1)
-            cmd.W = 0
-        if current_operation == ManualOperationType.eBackwardMove:
-            cmd.T = 3
-            cmd.V = -0.6 * sigmoid(step_acc, 0, 1)
-            cmd.W = 0
-        if current_operation == ManualOperationType.eLeftMove:
-            cmd.T = 2
-            cmd.V = 0.6 * sigmoid(step_acc, 0, 1)
-            cmd.W = 0
-        if current_operation == ManualOperationType.eRightMove:
-            cmd.T = 2
-            cmd.V = -0.6 * sigmoid(step_acc, 0, 1)
-            cmd.W = 0
-        cmd.L = self.L_
-        cmd.end = 1
-
-        # 处理为json格式
+        # 处理为json格式  # jtf.MessageToJson(cmd)
         cmd_dict = {}
         for field in cmd.DESCRIPTOR.fields:
             cmd_dict[field.name] = field.default_value
@@ -586,34 +691,40 @@ class Robot(MqttAsync):
             cmd_dict[field.name] = value
         cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
         self.publish(self.speedCmdTopic, cmd_json)
-
         print(cmd_json)
+        self.manual_status = ManualOperationType.eReady
+        return True
 
-        # cmd_0 = message.ManualCmd()
-        # cmd_0.key = str(uuid.uuid4())
-        # if current_operation == ManualOperationType.eCounterclockwiseRotate:
-        #     cmd_0.operation_type = 1
-        #     cmd_0.velocity = 1
-        # if current_operation == ManualOperationType.eClockwiseRotate:
-        #     cmd_0.operation_type = 1
-        #     cmd_0.velocity = -1
-        # if current_operation == ManualOperationType.eForwardMove:
-        #     cmd_0.operation_type = 2
-        #     cmd_0.velocity = 1
-        # if current_operation == ManualOperationType.eBackwardMove:
-        #     cmd_0.operation_type = 2
-        #     cmd_0.velocity = -1
-        # if current_operation == ManualOperationType.eLeftMove:
-        #     cmd_0.operation_type = 3
-        #     cmd_0.velocity = 1
-        # if current_operation == ManualOperationType.eRightMove:
-        #     cmd_0.operation_type = 3
-        #     cmd_0.velocity = -1
-        #
-        # channel = grpc.insecure_channel(self.rpc_ipport_)
-        # stub = msg_rpc.NavExcutorStub(channel)
-        # response = stub.ManualOperation(cmd_0)
-        # print("client received: ", response)
-
-        print("ManualOperation {op_type} task completed!!!".format(op_type=current_operation))
+    def Manual_Unit_Clamp(self, current_operation: ManualOperationType):
+        print(current_operation)
+        cmd = message.ToAgvCmd()
+        self.heat_ += 1
+        self.heat_ %= 255
+        cmd.H = self.heat_
+        if self.IsMainModeStatu():
+            cmd.M = 1
+        else:
+            cmd.M = 0
+        cmd.T = self.ManualOperationType2AgvCmdType(current_operation)
+
+        # 处理为json格式  # jtf.MessageToJson(cmd)
+        cmd_dict = {}
+        for field in cmd.DESCRIPTOR.fields:
+            cmd_dict[field.name] = field.default_value
+        for field, value in cmd.ListFields():
+            cmd_dict[field.name] = value
+        cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
+        self.publish(self.speedCmdTopic, cmd_json)
+        print(cmd_json)
+        self.manual_status = ManualOperationType.eReady
         return True
+
+    def ManualOperationType2AgvCmdType(self, mo_type: ManualOperationType):
+        if mo_type == ManualOperationType.eLifterRise:
+            return ActType.eLifterRise
+        if mo_type == ManualOperationType.eLifterDown:
+            return ActType.eLifterDown
+        if mo_type == ManualOperationType.eClampClose:
+            return ActType.eClampClose
+        if mo_type == ManualOperationType.eClampOpen:
+            return ActType.eClampOpen

count.json

@@ -1 +1 @@
-{"load_count": 538, "single_count": 1344}
+{"load_count": 661, "single_count": 1926}

custom_define.py

@@ -7,8 +7,10 @@ class ActType(enum.IntEnum):
     eHorizon = 2
     eVertical = 3
     eMPC = 4
-    eClampOpen = 5
-    eClampClose = 6
+    eClampClose = 5
+    eClampOpen = 6
+    eLifterRise = 7
+    eLifterDown = 8
 
 
 class ManualOperationType(enum.IntEnum):
@@ -22,6 +24,12 @@ class ManualOperationType(enum.IntEnum):
     eLeftMove = 0x0040
     eRightMove = 0x0080
 
+    eClampClose = 0x0100
+    eClampOpen = 0x0200
+
+    eLifterRise = 0x0400
+    eLifterDown = 0x0800
+
 
 class RobotName:
     strAGVMain = "AgvMain"

map.json

@@ -1,39 +1,31 @@
 {
   "street_nodes": {
     "R1": [
-      -2.4,
-      -8.1
+      4.25,
+      -2.37
     ],
     "R2": [
-      -2.4,
-      -14.88
-    ],
-    "R3": [
-      -10.62,
-      -14.88
+      20.47,
+      -2.37
     ]
   },
 
   "space_nodes": {
     "S1": [
-      -10.62,
-      -4.35,
+      20.47,
+      6.8,
       1.57
     ]
-  },
 
+  },
   "roads": {
-    "road1": [
+    "road_1": [
       "R1",
       "R2"
     ],
-    "road2": [
-      "R2",
-      "R3"
-    ],
-    "road3": [
-      "R3",
-      "S1"
+    "road_2": [
+      "S1",
+      "R2"
     ]
   }
 }

message.proto

@@ -13,9 +13,8 @@ message AgvStatu{
   float w=2;
   int32 clamp=3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
   int32 clamp_other=4; //另外一节
-  int32 lifter=5;   //提升机构
+  int32 lifter=5;   //提升机构  0中间状态，1上升到位， 2下降到位
   int32 lifter_other = 6;     //另外一节
-
 }
 
 message ToAgvCmd {
@@ -89,7 +88,7 @@ service NavExcutor {
 
 message NavStatu
 {
-  int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
+  int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关,7:提升机构提升，8：提升机构下降
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，0：single，1：双车
   LidarOdomStatu odom=4;

test.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
-#===============================================================================
+# ===============================================================================
 #
 # test.py
 #
@@ -31,7 +31,7 @@
 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #
-#===============================================================================
+# ===============================================================================
 import math
 import time
 import sys
@@ -52,46 +52,54 @@ import uuid
 from custom_define import RobotName
 
 
-#===============================================================================
+# ===============================================================================
 
 
 class MainWindow(QMainWindow):
     """docstring for Mainwindow"""
-    djks_map_=mp.DijikstraMap()
-    ui_nodes={}
-    ui_nodes["street_nodes"]=[]
-    ui_nodes["space_nodes"]=[]
+    djks_map_ = mp.DijikstraMap()
+    ui_nodes = {}
+    ui_nodes["street_nodes"] = []
+    ui_nodes["space_nodes"] = []
 
-    def __init__(self, parent = None):
-        super(MainWindow,self).__init__(parent)
+    def __init__(self, parent=None):
+        super(MainWindow, self).__init__(parent)
         self.basic()
-        self.count_frame_=ControllWidget.CountFrame()
+        self.count_frame_ = ControllWidget.CountFrame()
         self.LoadMap()
-        config1={"label":RobotName.strAGVMain,
-                 "rpc":"192.168.0.70:9090",
-                 # "rpc": "127.0.0.1:9090",
-                 "street_nodes":self.ui_nodes["street_nodes"],
-                 "space_nodes":self.ui_nodes["space_nodes"],
-                 # "mqtt":["pyui1","192.168.0.70",1883,"admin","zx123456"],
-                 # "mqtt":["pyui1","192.168.225.224",1883,"admin","zx123456"],
-                 "mqtt":["pyui-main","127.0.0.1",1883,"admin","zx123456"],
-                 "subTopics":{"agv_main/agv_statu":message.RobotStatu.__name__,
-                              "agv_main/nav_statu":message.NavStatu.__name__},
-                 "cmdTopic":"agv_main/nav_cmd",
-                 "robotColor":[0.7,0.2,0.3]}
-
-        config2={"label":RobotName.strAGV2,
-                 # "rpc":"192.168.0.71:9090",
-                 "rpc": "127.0.0.1:9091",
-                 "street_nodes":self.ui_nodes["street_nodes"],
-                 "space_nodes":self.ui_nodes["space_nodes"],
-                 # "mqtt":["pyui2","192.168.0.71",1883,"admin","zx123456"],
-                 # "mqtt":["pyui2","192.168.225.224",1883,"admin","zx123456"],
-                 "mqtt":["pyui-child","127.0.0.1",1883,"admin","zx123456"],
-                 "subTopics":{"agv_child/agv_statu":message.RobotStatu.__name__,
-                              "agv_child/nav_statu":message.NavStatu.__name__},
-                 "cmdTopic":"agv_child/nav_cmd",
-                 "robotColor":[0.4,0.2,0.8]}
+        self.isLocalTest = True
+        self.isAutoDispatch = False
+
+        rpc_1 = "192.168.0.70:9090"
+        mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
+        rpc_2 = "192.168.0.71:9090"
+        mqtt_2 = ["pyui2", "192.168.0.71", 1883, "admin", "zx123456"]
+        if self.isLocalTest:
+            rpc_1 = "127.0.0.1:9090"
+            mqtt_1 = ["pyui-main", "127.0.0.1", 1883, "admin", "zx123456"]
+            rpc_2 = "127.0.0.1:9091"
+            mqtt_2 = ["pyui-child", "127.0.0.1", 1883, "admin", "zx123456"]
+
+        config1 = {"label": RobotName.strAGVMain,
+                   "rpc": rpc_1,
+                   "street_nodes": self.ui_nodes["street_nodes"],
+                   "space_nodes": self.ui_nodes["space_nodes"],
+                   "mqtt": mqtt_1,
+                   "subTopics": {"agv_main/agv_statu": message.RobotStatu.__name__,
+                                 "agv_main/nav_statu": message.NavStatu.__name__},
+                   "cmdTopic": "agv_main/nav_cmd",
+                   "robotColor": [0.7, 0.2, 0.3]}
+
+        config2 = {"label": RobotName.strAGV2,
+                   "rpc": rpc_2,
+                   # "rpc": "127.0.0.1:9091",
+                   "street_nodes": self.ui_nodes["street_nodes"],
+                   "space_nodes": self.ui_nodes["space_nodes"],
+                   "mqtt": mqtt_2,
+                   "subTopics": {"agv_child/agv_statu": message.RobotStatu.__name__,
+                                 "agv_child/nav_statu": message.NavStatu.__name__},
+                   "cmdTopic": "agv_child/nav_cmd",
+                   "robotColor": [0.4, 0.2, 0.8]}
         self.Controller1 = ControllWidget.ControlFrame(config1)
         self.Controller2 = ControllWidget.ControlFrame(config2)
         robot_dict = {self.Controller1.robot_.name_: self.Controller1.robot_,
@@ -104,62 +112,68 @@ class MainWindow(QMainWindow):
         self.gl.SetRobot1Instance(self.Controller1.robot_)
         self.gl.SetRobot2Instance(self.Controller2.robot_)
 
-        self.timer_=QTimer()
+        self.timer_ = QTimer()
         self.timer_.timeout.connect(self.Update)
         self.timer_.start(100)
 
-
     def LoadMap(self):
-        self.gl = MapGLWidget()   #将opengl例子嵌入GUI
-        map=self.load_map("./map.json")
+        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
+        map = self.load_map("./map.json")
 
         for node in map['street_nodes'].items():
-            [id,point]=node
-            street_node=mp.StreetNode(id,point[0],point[1])
+            [id, point] = node
+            street_node = mp.StreetNode(id, point[0], point[1])
             self.djks_map_.AddVertex(street_node)
-            self.gl.AddNode([id,"street_node",point])
+            self.gl.AddNode([id, "street_node", point])
             self.ui_nodes["street_nodes"].append(id)
         for node in map['space_nodes'].items():
-            [id,point]=node
-            [x,y,yaw]=point
-            space_node=mp.SpaceNode(id,point[0],point[1],yaw)
+            [id, point] = node
+            [x, y, yaw] = point
+            space_node = mp.SpaceNode(id, point[0], point[1], yaw)
             self.djks_map_.AddVertex(space_node)
-            glNode=[id,"space_node",[x,y]]
+            glNode = [id, "space_node", [x, y]]
             self.gl.AddNode(glNode)
             self.ui_nodes["space_nodes"].append(id)
 
         for road in map['roads'].items():
             self.gl.AddRoad(road)
 
-            [_,points]=road
+            [_, points] = road
             for pt1 in points:
                 for pt2 in points:
-                    if not pt1==pt2:
-                        self.djks_map_.AddEdge(pt1,pt2)
+                    if not pt1 == pt2:
+                        self.djks_map_.AddEdge(pt1, pt2)
 
-    def load_map(self,file):
-        with open(file,'r',encoding='utf-8') as fp:
-            map=json.load(fp)
+    def load_map(self, file):
+        with open(file, 'r', encoding='utf-8') as fp:
+            map = json.load(fp)
             return map
 
     def Update(self):
         self.gl.update()
+        if self.isAutoDispatch:
+            self.Controller1.setVisible(False)
+            self.Controller2.setVisible(False)
+        else:
+            self.Controller1.setVisible(True)
+            self.Controller2.setVisible(True)
+
 
-    #窗口基础属性
+    # 窗口基础属性
     def basic(self):
-        #设置标题，大小，图标
+        # 设置标题，大小，图标
         self.setWindowTitle("GT")
-        self.resize(1100,650)
+        self.resize(1100, 650)
         self.setWindowIcon(QIcon("./image/Gt.png"))
-        #居中显示
+        # 居中显示
         screen = QDesktopWidget().geometry()
         self_size = self.geometry()
-        self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))
+        self.move(int((screen.width() - self_size.width()) / 2), int((screen.height() - self_size.height()) / 2))
 
     def closeEvent(self, QCloseEvent):
         self.gl.close()
 
-    #分割窗口
+    # 分割窗口
     def split_(self):
         splitter_main = QSplitter(Qt.Horizontal)
         splitter = QSplitter(Qt.Vertical)
@@ -169,29 +183,29 @@ class MainWindow(QMainWindow):
         splitter.addWidget(self.Controller2)
         splitter.addWidget(self.ManualOperationWidget)
 
-        splitter.setStretchFactor(0,1)
-        splitter.setStretchFactor(1,3)
-        splitter.setStretchFactor(2,3)
-        splitter.setStretchFactor(3,1)
+        splitter.setStretchFactor(0, 1)
+        splitter.setStretchFactor(1, 3)
+        splitter.setStretchFactor(2, 3)
+        splitter.setStretchFactor(3, 1)
 
         splitter_main.addWidget(splitter)
         splitter_main.addWidget(self.gl)
 
-        splitter_main.setStretchFactor(0,1)
-        splitter_main.setStretchFactor(2,4)
+        splitter_main.setStretchFactor(0, 1)
+        splitter_main.setStretchFactor(2, 4)
 
         return splitter_main
 
+
 if __name__ == "__main__":
     app = QApplication(sys.argv)
     win = MainWindow()
     win.show()
     sys.exit(app.exec_())
 
-
-#===============================================================================
+# ===============================================================================
 # Main
-#===============================================================================
+# ===============================================================================
 
 '''app = QApplication(sys.argv)
 mainWindow = MapGLWidget()
@@ -199,11 +213,11 @@ mainWindow.show()
 mainWindow.raise_() # Need this at least on OS X, otherwise the window ends up in background
 sys.exit(app.exec_())'''
 
-#===============================================================================
+# ===============================================================================
 #
 # Local Variables:
 # mode: Python
 # indent-tabs-mode: nil
 # End:
 #
-#===============================================================================
+# ===============================================================================