1.优化界面控制

ControllWidget.py

@@ -11,6 +11,7 @@ import Count
 import message_pb2
 from RobotData import Robot
 import dijkstra.Map as mp
+from custom_define import ControllerStatus
 
 
 class ControlFrame(QFrame):
@@ -266,6 +267,7 @@ class ControlFrame(QFrame):
                                     self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()))
 
     def btnCancelClick(self):
+        self.controller_status = ControllerStatus.eCancel
         self.threadPool_.submit(self.robot_.CancelNavTask)
         self.btnAuto.setCheckState(Qt.Unchecked)
 

custom_define.py

@@ -34,3 +34,10 @@ class ManualOperationType(enum.IntEnum):
 class RobotName:
     strAGVMain = "AgvMain"
     strAGV2 = "AGV2"
+
+
+class ControllerStatus(enum.IntEnum):
+    eReady = 0
+    eStop = 1
+    eCancel = 2
+    eRunning = 3

data/1111

@@ -0,0 +1,12 @@
+Switch MoveMode --> main(1), wheelBase: 2.700000
+navigation completed!!!
+server received StartCmd: 26f00597-6e14-461a-b08e-a97f4c3c73ac
+Navigation beg...
+unfinished size:1
+ Out space target street node:[x:6.2, y:8.9, theta:0°]
+ impossible to target:6.200000 2.640000 l:0.200000,w:0.100000 theta:-1.570796,  current:6.211456, 9.344073, 1.476038
+ impossible to target:6.200000 2.640000 l:0.200000,w:0.100000 theta:-1.570796,  current:6.211456, 9.344073, 3.046834
+ exec MPC_rotate autoDirect:15
+  MPC_rotate |P[0], input anguar:0.000000, down:0.052360(3.000000), diff:0.091805 anyDirect:15
+  MPC_rotate |P[1], input anguar:0.000000, down:0.104720(6.000000), diff:0.091805 anyDirect:15
+  MPC_rotate |P[2], input anguar:0.000000, down:0.136324(7.810779), diff:0.091805 anyDirect:15

message.proto

@@ -30,7 +30,9 @@ message ToAgvCmd {
     float L1 = 10;  //轴距
     int32 P1 = 11; //车位编号
     float D1 = 12; //距目标点距离
-    int32 end = 13;
+    float Y1=13;    //前车小w
+    float Y2=14;    //后车小w
+    int32 end = 15;
 }
 
 message Pose2d

test.py

@@ -132,26 +132,37 @@ class MainWindow(QMainWindow):
 
     def recv_park_request(self, msg):
         print("Recv_park_request------------------\n", msg)
+        # time.sleep(30)
+        # # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port",
+        # # tf.MessageToString(table, as_utf8=True))
+        # self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
+        #                         msg)
+        # print("Publish_park_response------------------\n", msg)
+        # return True
+
         park_table = message.park_table()
         try:
             tf.Parse(msg, park_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.AutoParkTask(park_table)
-        # self.g_rabbitmq.publish()
 
     def recv_pick_request(self, msg):
         print("Recv_pick_request------------------\n", msg)
         pick_table = message.pick_table()
+        # time.sleep(30)
+        # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port",
+        # tf.MessageToString(table, as_utf8=True))
+        # self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
+        #                         msg)
+        # print("Publish_pick_response------------------\n", msg)
+        # return True
+
         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)
 
@@ -191,6 +202,46 @@ class MainWindow(QMainWindow):
                 if node_id.find("CInput") >= 0:
                     self.djks_map_.AddEdge_t("CInput_R1101", node_id)
             print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1101"])
+
+    def GenerateSpecifiedMap(self, map_type):  # 0:前车地图，1：后车地图，2：整车地图
+        '''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])
+                self.djks_map_.AddVertex(street_node)
+                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)
+                self.djks_map_.AddVertex(space_node)
+                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
+                for pt1 in points:
+                    for pt2 in points:
+                        if not pt1 == pt2:
+                            self.djks_map_.AddEdge(pt1, pt2)
+        }
+
+
+
+        if type == 0:
+            pass
+        elif type == 1:
+            pass
+        elif type == 2:
+            pass
+        else:
+            print("type of specified map is wrong!\n")'''
+
     def GetMainFrontBackAGV(self):
         controlMain = self.Controller1
         nagtive = False
@@ -204,11 +255,12 @@ class MainWindow(QMainWindow):
         else:
             control2 = self.Controller1
             control1 = self.Controller2
-        return [controlMain,control1,control2]
+        return [controlMain, control1, control2]
+
     def AutoParkTask(self, park_table: message.park_table = None):
         print("AutoParkTask:---------------------\n")
 
-        [controlMain,control1,control2]=self.GetMainFrontBackAGV()
+        [controlMain, control1, control2] = self.GetMainFrontBackAGV()
 
         entrance_id = "S" + str(park_table.terminal_id)  # "S1101"
         entrance_x, entrance_y, entrance_theta = [park_table.entrance_measure_info.measure_info_to_plc_forward.cx,
@@ -307,6 +359,8 @@ class MainWindow(QMainWindow):
 
         threadPool.shutdown(wait=True)
 
+        self.g_rabbitmq.publish()
+
     def updateMap_pick(self, export_id, type):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
 
         self.djks_map_.Reset()  # 重置地图