1.添加手动控制模块(绕过板子MPC程序直接控制)

ManualOperationWidget.py

@@ -0,0 +1,96 @@
+import enum
+import time
+from concurrent.futures import ThreadPoolExecutor
+
+from PyQt5.QtCore import QTimer
+from PyQt5.QtWidgets import QFrame
+
+from UI.manual_operation import Ui_manual_operation_widget
+
+
+class ManualOperationType(enum.Enum):
+    eReady = 0x0000
+
+    eCounterclockwiseRotate = 0x0001
+    eClockwiseRotate = 0x0002
+
+    eForwardMove = 0x0010,
+    eBackwardMove = 0x0020,
+    eLeftMove = 0x0040,
+    eRightMove = 0x0080,
+
+
+class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
+    threadPool_ = ThreadPoolExecutor(1)
+
+    def __init__(self, robot_dict:dict):
+        super().__init__()
+        self.setupUi(self)
+
+        self.robot_dict = robot_dict
+        self.selected_robot_name = None
+        self.current_operation = ManualOperationType.eReady
+        self.operation_count = dict()
+        self.operation_count[ManualOperationType.eReady] = 0
+
+        # 所选robot
+        for robot in self.robot_dict.values():
+            self.select_box.addItem(robot.name_)
+        self.select_box.activated.connect(self.SelectRobot)
+        self.selected_robot_name = self.select_box.currentText()
+        # 逆/顺时针旋转
+        self.Counterclockwise_Rotate_btn.pressed.connect(self.Counterclockwise_Rotate)
+        self.Counterclockwise_Rotate_btn.released.connect(self.Cancel)
+        self.Clockwise_Rotate_btn.pressed.connect(self.Clockwise_Rotate)
+        self.Clockwise_Rotate_btn.released.connect(self.Cancel)
+        # 前后左右平移
+        self.Forward_Move_btn.pressed.connect(self.Forward_Move)
+        self.Forward_Move_btn.released.connect(self.Cancel)
+        self.Backward_Move_btn.pressed.connect(self.Backward_Move)
+        self.Backward_Move_btn.released.connect(self.Cancel)
+        self.Left_Move_btn.pressed.connect(self.Left_Move)
+        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.timer = QTimer()
+        self.timer.timeout.connect(self.DownCmd)
+        self.timer.start(200)
+
+    def SelectRobot(self):
+        self.selected_robot_name = self.select_box.currentText()
+    def Counterclockwise_Rotate(self):
+        self.current_operation = ManualOperationType.eCounterclockwiseRotate
+
+    def Clockwise_Rotate(self):
+        self.current_operation = ManualOperationType.eClockwiseRotate
+
+    def Forward_Move(self):
+        self.current_operation = ManualOperationType.eForwardMove
+
+    def Backward_Move(self):
+        self.current_operation = ManualOperationType.eBackwardMove
+
+    def Left_Move(self):
+        self.current_operation = ManualOperationType.eLeftMove
+
+    def Right_Move(self):
+        self.current_operation = ManualOperationType.eRightMove
+
+    def Cancel(self):
+        self.current_operation = ManualOperationType.eReady
+        self.operation_count[ManualOperationType.eReady] += 1
+
+    def DownCmd(self):
+        if self.current_operation != ManualOperationType.eReady:
+            self.threadPool_.submit(self.robot_dict[self.selected_robot_name].ManualTask, self.current_operation)
+        elif 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
+        else:
+            self.current_operation = ManualOperationType.eReady
+            self.operation_count[ManualOperationType.eReady] = 0
+

RobotData.py

@@ -4,6 +4,7 @@ import time
 import message_pb2 as message
 import grpc
 import message_pb2_grpc as msg_rpc
+from ManualOperationWidget import ManualOperationType
 from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
 from dijkstra.Map import DijikstraMap, SpaceNode, StreetNode
@@ -37,15 +38,16 @@ class TimeStatu:
 
 class Robot(MqttAsync):
 
-    def __init__(self,rpc_ipport,name=""):
+    def __init__(self, rpc_ipport, name=""):
         MqttAsync.__init__(self)
         self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
         self.timedNavStatu_ = TimeStatu(message.NavStatu, 0)
         self.dataTopic_ = {}
-        self.rpc_ipport_=rpc_ipport
+        self.rpc_ipport_ = rpc_ipport
         self.messageArrivedSignal = None
         self.currentNavData_ = None
         self.navCmdTopic_ = None
+        self.speedCmdTopic = "monitor_child/speedcmd"
         self.currentNavPathNodes_ = None
         self.currentNavPath_ = None
 
@@ -61,6 +63,7 @@ class Robot(MqttAsync):
         self.l_ = 0.8  # 轮长
         self.L_ = 1.3  # 轴距
         self.W_ = 2.5  # 宽
+        self.heat_ = 0  # 心跳
 
     def Color(self):
         if self.IsMainModeStatu():
@@ -243,7 +246,7 @@ class Robot(MqttAsync):
         protoNode = message.PathNode()
         protoNode.x = node.x_
         protoNode.y = node.y_
-        protoNode.id=node.id_
+        protoNode.id = node.id_
         return protoNode
 
     def ExecNavtask(self, autoDirect):
@@ -261,7 +264,7 @@ class Robot(MqttAsync):
                 protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 1
-                act.wheelbase=2.78
+                act.wheelbase = 2.78
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
 
@@ -272,7 +275,7 @@ class Robot(MqttAsync):
                 protoStreet = self.generateProtoNode(street)
                 act = message.NewAction()
                 act.type = 2
-                act.wheelbase=2.78
+                act.wheelbase = 2.78
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
 
@@ -284,31 +287,33 @@ class Robot(MqttAsync):
                     return False
                 else:
                     cmd.newActions.add().CopyFrom(action)
-
+        print(cmd)
         if cmd is None:
             print("Nav cmd is None")
             return False
 
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
 
         response = stub.Start(cmd)
         print("client received: ", response)
-        if not response.ret==0:
-            print("nav failed :%s"%(response.info))
+        if not response.ret == 0:
+            print("nav failed :%s" % (response.info))
         else:
             print("nav completed !!!")
 
         return True
 
     def CreateNavPathNodesAction(self, path, autoDirect):
-        if path is not None and self.timedRobotStatu_.timeout() == False:
+        # if path is not None and self.timedRobotStatu_.timeout() == False:
+        if 1:
             for node in path:
                 if isinstance(node, (SpaceNode)):
                     print(" nodes must all street node")
                     return None
             statu = self.timedRobotStatu_.statu
-            if statu is not None:
+            # if statu is not None:
+            if 1:
                 newAction = message.NewAction()
                 size = len(path)
                 for i in range(size):
@@ -349,7 +354,7 @@ class Robot(MqttAsync):
         cmd.newActions.add().CopyFrom(action)
 
         print(cmd)
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
         return True
@@ -443,7 +448,7 @@ class Robot(MqttAsync):
         if mode == 2:
             action.wheelbase = wheelbase
         cmd.newActions.add().CopyFrom(action)
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
         return False
@@ -492,7 +497,7 @@ class Robot(MqttAsync):
         act = message.NewAction()
         act.type = 6
         cmd.newActions.add().CopyFrom(act)
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
         return True
@@ -508,7 +513,7 @@ class Robot(MqttAsync):
         act = message.NewAction()
         act.type = 7
         cmd.newActions.add().CopyFrom(act)
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
         response = stub.Start(cmd)
         return True
@@ -517,10 +522,77 @@ class Robot(MqttAsync):
         cmd = message.NavCmd()
         cmd.action = 3
 
-        channel=grpc.insecure_channel(self.rpc_ipport_)
+        channel = grpc.insecure_channel(self.rpc_ipport_)
         stub = msg_rpc.NavExcutorStub(channel)
 
         response = stub.Cancel(cmd)
         print("client received: ", response)
         print(" Cancel task completed!!!")
         return True
+
+    def ManualTask(self, current_operation):
+        cmd = message.ToAgvCmd()
+        self.heat_ += 1
+        self.heat_ %= 255
+        cmd.H = self.heat_
+        if self.IsMainModeStatu():
+            cmd.M = 2
+        else:
+            cmd.M = 1
+
+        if current_operation == ManualOperationType.eCounterclockwiseRotate:
+            cmd.T = 1
+            cmd.V = 0
+            cmd.W = 2.0/180*math.pi
+        if current_operation == ManualOperationType.eClockwiseRotate:
+            cmd.T = 1
+            cmd.V = 0
+            cmd.W = -2.0 / 180 * math.pi
+        if current_operation == ManualOperationType.eForwardMove:
+            cmd.T = 3
+            cmd.V = 0.5
+            cmd.W = 0
+        if current_operation == ManualOperationType.eBackwardMove:
+            cmd.T = 3
+            cmd.V = -0.5
+            cmd.W = 0
+        if current_operation == ManualOperationType.eLeftMove:
+            cmd.T = 2
+            cmd.V = 0.5
+            cmd.W = 0
+        if current_operation == ManualOperationType.eRightMove:
+            cmd.T = 2
+            cmd.V = -0.5
+            cmd.W = 0
+        cmd.L = self.L_
+        cmd.end = 1
+        self.publish(self.speedCmdTopic, jtf.MessageToJson(cmd))
+
+        # 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(cmd)
+        print("ManualOperation {op_type} task completed!!!".format(op_type=current_operation))
+        return True

message.proto

@@ -72,11 +72,19 @@ message NavResponse{
   string info=2;
 }
 
-service NavExcutor{
-  rpc Start(NavCmd) returns(NavResponse){}
-  rpc Cancel(NavCmd) returns(NavResponse){}
+message ManualCmd {
+    string key = 1;
+    int32 operation_type = 2; // 1:旋转, 2:前后平移(X方向), 3:左右平移(Y方向):
+    float velocity = 3; // 正数为正方向，负数为负方向
 }
 
+service NavExcutor {
+    rpc Start (NavCmd) returns (NavResponse) {}
+    rpc Cancel (NavCmd) returns (NavResponse) {}
+    rpc ManualOperation (ManualCmd) returns (NavResponse) {}
+}
+
+
 message NavStatu
 {
   int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关

proto.sh

@@ -1,4 +1,4 @@
 
 #protoc -I=./ message.proto --python_out=./ --grpc_python_out=./
 
-python -m grpc_tools.protoc -I. --python_out=. --pyi_out=. --grpc_python_out=. message.proto
+python3 -m grpc_tools.protoc -I. --python_out=. --pyi_out=. --grpc_python_out=. message.proto

test.py

@@ -43,6 +43,7 @@ import json
 import dijkstra.Map as mp
 import ControllWidget
 import JointContrallerWidget
+import ManualOperationWidget
 import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
@@ -67,9 +68,9 @@ class MainWindow(QMainWindow):
                  "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.224",1883,"admin","zx123456"],
+                 "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"],
+                 # "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",
@@ -79,17 +80,18 @@ class MainWindow(QMainWindow):
                  "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.224",1883,"admin","zx123456"],
+                 "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"],
+                 # "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.Controller1 = ControllWidget.ControlFrame(config1)
         self.Controller2 = ControllWidget.ControlFrame(config2)
-        self.JointContrallerWidget = \
-            JointContrallerWidget.JointControlFrame(self.Controller1.robot_, self.Controller2.robot_)
+        robot_dict = {self.Controller1.robot_.name_: self.Controller1.robot_,
+                      self.Controller2.robot_.name_: self.Controller2.robot_}
+        self.ManualOperationWidget = ManualOperationWidget.ManualOperationWidget(robot_dict)
 
         splitter_main = self.split_()
         self.setCentralWidget(splitter_main)
@@ -136,8 +138,6 @@ class MainWindow(QMainWindow):
             return map
 
     def Update(self):
-        self.JointContrallerWidget.SetRobot1_BaTID(self.Controller1.begId_, self.Controller1.targetId_)
-        self.JointContrallerWidget.SetRobot2_BaTID(self.Controller2.begId_, self.Controller2.targetId_)
         self.gl.update()
 
     #窗口基础属性
@@ -162,7 +162,7 @@ class MainWindow(QMainWindow):
         splitter.addWidget(self.count_frame_)
         splitter.addWidget(self.Controller1)
         splitter.addWidget(self.Controller2)
-        splitter.addWidget(self.JointContrallerWidget)
+        splitter.addWidget(self.ManualOperationWidget)
 
         splitter.setStretchFactor(0,1)
         splitter.setStretchFactor(1,3)