增加O点车位点

ControllWidget.py

@@ -13,7 +13,7 @@ import dijkstra.Map as mp
 
 
 class ControlFrame(QFrame):
-    threadPool_ = ThreadPoolExecutor(2)
+    threadPool_ = ThreadPoolExecutor(5)
     last_beg = "**"
     begId_ = "------"
     targetId_ = "------"

RobotData.py

@@ -1,3 +1,4 @@
+import enum
 import time
 import message_pb2 as message
 from mqtt_async import MqttAsync
@@ -7,6 +8,15 @@ import uuid
 from auto_test import TestCount
 import random
 
+class ActType(enum.Enum):
+    eReady=0
+    eRotation=1
+    eHorizon=2
+    eVertical=3
+    eMPC=4
+    eClampOpen=5
+    eClampClose=6
+
 
 class TimeStatu:
     def __init__(self, statu=None, timeout=3):
@@ -18,8 +28,8 @@ class TimeStatu:
         tm = time.time()
         return tm - self.time > self.timeout_ms or self.statu == None
 
-
 class Robot(MqttAsync):
+
     def __init__(self, name=""):
         MqttAsync.__init__(self)
         self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
@@ -121,11 +131,12 @@ class Robot(MqttAsync):
         return self.timedNavStatu_.timeout() == False and self.timedRobotStatu_.timeout() == False
 
     def IsNavigating(self):
-        if self.timedNavStatu_.timeout() == False:
+        return not self.ActionType()==ActType.eReady
+        '''if self.timedNavStatu_.timeout() == False:
             key = self.timedNavStatu_.statu.key
             if key == "" or key == None:
                 return False
-        return True
+        return True'''
 
     def GeneratePath(self, begID, endID):
         self.currentNavPathNodes_ = DijikstraMap().GetShortestPath(begID, endID)
@@ -196,13 +207,31 @@ class Robot(MqttAsync):
                     print("3333",target)
             print("  Next nav clamp cmd,%s to %s"%(beg,target))
 
-
+    def ActionType(self):
+        if self.timedNavStatu_.timeout() == False:
+            runningStatu=self.timedNavStatu_.statu
+            if runningStatu.statu==0:
+                return ActType.eReady
+            if runningStatu.statu==1:
+                return ActType.eRotation
+            if runningStatu.statu==2:
+                return ActType.eHorizon
+            if runningStatu.statu==3:
+                return ActType.eVertical
+            if runningStatu.statu==4:
+                return ActType.eMPC
+            if runningStatu.statu==5:
+                return ActType.eClampOpen
+            if runningStatu.statu==6:
+                return ActType.eClampClose
+        else:
+            return ActType.eReady
 
     def ExecNavtask(self):
         if self.navCmdTopic_ == None:
             print("Robot has not set nav cmd topic")
             return False
-        if self.IsNavigating():
+        if not self.ActionType()==ActType.eReady:
             print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
             return False
 
@@ -218,11 +247,12 @@ class Robot(MqttAsync):
         if cmd is None:
             print("Nav cmd is None")
             return False
-
-        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-        time.sleep(1)
+        published=False
         while self.IsNavigating() == False:
-            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+            if not self.ActionType() == ActType.eReady:
+                published=True
+            if published==False:
+                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
@@ -237,17 +267,17 @@ class Robot(MqttAsync):
             cmd.action = 4
         if mode == 1:
             cmd.action = 5
-
-        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-        while True:
+        loop=3
+        while loop>0:
             if mode == 2:
                 if self.IsMainModeStatu():
                     return True
             if mode == 1:
                 if self.IsMainModeStatu() == False:
                     return True
-            time.sleep(0.1)
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
+            loop-=1
+            time.sleep(0.5)
         return False
 
     def IsClampClosed(self):
@@ -293,15 +323,14 @@ class Robot(MqttAsync):
         act = message.Action()
         act.type = 3
         cmd.actions.add().CopyFrom(act)
-        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-        time.sleep(1)
-        while True:
-            if self.IsClampClosed() == True:
-                return True
-            if self.IsClampRunning()==False:
+        published=False
+        while self.IsClampClosed()==False:
+            if self.ActionType()==ActType.eClampClose:
+                published=True
+            if published==False:
                 self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-            time.sleep(1)
-        return False
+            time.sleep(0.5)
+        return True
 
     def ClampOpen(self):
         if self.IsClampOpened() == True:
@@ -313,20 +342,18 @@ class Robot(MqttAsync):
         act = message.Action()
         act.type = 4
         cmd.actions.add().CopyFrom(act)
-        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-        time.sleep(1)
-        while True:
-            if self.IsClampOpened() == True:
-                return True
-            if self.IsClampRunning() == False:
+        published=False
+        while self.IsClampOpened()==False:
+            if self.ActionType()==ActType.eClampOpen:
+                published=True
+            if published==False:
                 self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-            time.sleep(1)
-        return False
+            time.sleep(0.5)
+        return True
 
     def CancelNavTask(self):
         cmd = message.NavCmd()
         cmd.action = 3
-        self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
         while self.IsNavigating() == True:
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)

dijkstra/Map.py

@@ -170,14 +170,14 @@ class DijikstraMap(object):
         node_mpcdiff.theta=(10 * math.pi / 180.0)
 
         #最后一个巡线目标点精度
-        enddiff.x=(0.1)
-        enddiff.y=(0.1)
-        enddiff.theta=(3 * math.pi / 180.0)
+        enddiff.x=(0.02)
+        enddiff.y=(0.02)
+        enddiff.theta=(0.5 * math.pi / 180.0)
 
         #最后一个原地调整精度
         lastAdjustDiff.x=(0.03)
-        lastAdjustDiff.y=(0.03)
-        lastAdjustDiff.theta=(0.3 * math.pi / 180.0)
+        lastAdjustDiff.y=(0.01)
+        lastAdjustDiff.theta=(0.7 * math.pi / 180.0)
 
         # 速度限制
         v_limit = message.SpeedLimit()
@@ -186,14 +186,17 @@ class DijikstraMap(object):
         v_limit.min=(0.1)
         v_limit.max=(0.2)
         horize_limit.min=(0.05)
-        horize_limit.max=(0.5)
-        angular_limit.min=(1.5)
+        horize_limit.max=(0.2)
+        angular_limit.min=(2)
         angular_limit.max=(40.0)
         # mpc速度限制
         mpc_x_limit = message.SpeedLimit()
+        last_MPC_v=message.SpeedLimit()
         mpc_angular_limit = message.SpeedLimit()
         mpc_x_limit.min=(0.05)
         mpc_x_limit.max=(1.2)
+        last_MPC_v.min=0.03
+        last_MPC_v.max=0.4
 
         mpc_angular_limit.min=(0 * math.pi / 180.0)
         mpc_angular_limit.max=(3 * math.pi / 180.0)
@@ -250,9 +253,10 @@ class DijikstraMap(object):
             act_along.target.theta=(yaw)
             if count==len(path)-1:
                 act_along.target_diff.CopyFrom(enddiff)
+                act_along.velocity_limit.CopyFrom(last_MPC_v)
             else:
                 act_along.target_diff.CopyFrom(node_mpcdiff)
-            act_along.velocity_limit.CopyFrom(mpc_x_limit)
+                act_along.velocity_limit.CopyFrom(mpc_x_limit)
             act_along.angular_limit.CopyFrom(mpc_angular_limit)
             cmd.actions.add().CopyFrom(act_along)
 

map.json

@@ -1,9 +1,8 @@
 
 {
   "street_nodes":{
-    "O":[3.3,1.7],
-    "O1":[3.3,3.5],
-    "O2":[3.3,0],
+    "OF":[3.3,3.5],
+    "OB":[3.3,0],
     "N1":[6.3,3.5],
     "NT":[10.3,1.7],
     "NT1":[6.3,0],
@@ -11,31 +10,48 @@
     "N3": [15.4,3.5],
     "N4":[17.9,3.5],
     "N5":[10.3,17.7],
-    "N6":[15.1,17.7],
+    "N6":[15.18,17.7],
     "N7":[17.4,17.7],
     "N8":[10.3,-0.18],
-    "N11":[18.8,3.5]
+    "N11":[18.8,3.5],
+    "CC":[10.8,22.2],
+    "CC1":[10.8,23.85],
+    "CC2":[10.8,20.55]
     
   },
   
   "space_nodes":{
+    "O1":[1.65,1.7,3.1415],
+    "O":[3.3,1.7,3.1415],
+    "O2":[4.95,1.7,3.1415],
   	"A":[15.4,8,1.57],
-  	"B":[15.1,13,-1.57],
-    "C":[15.1,22,1.57],
-    "D":[17.4,22,1.57],
-    "E":[17.4,13,-1.57]
+    "B1":[15.18,11.35,-1.57],
+  	"B":[15.18,13,-1.57],
+    "B2":[15.18,14.65,-1.57],
+    "C1":[15.18,23.85,1.57],
+    "C":[15.18,22.2,1.57],
+    "C2":[15.18,20.55,1.57],
+    "D1":[17.4,23.85,1.57],
+    "D":[17.4,22.2,1.57],
+    "D2":[17.4,20.55,1.57],
+    "E1":[17.4,11.35,-1.57],
+    "E":[17.4,13,-1.57],
+    "E2":[17.4,14.65,-1.57]
   },
   "roads":{
-    "road1":["O1","N1","N2","N3","N4","N11"],
-    "conn":["O","NT"],
+    "road1":["OF","N1","N2","N3","N4","N11"],
+    "conn":["O","NT","O1","O2"],
     "conn1":["NT1","N1"],
-    "conn2":["NT1","O2"],
+    "conn2":["NT1","OB"],
     "road2":["N8","N5","N2","NT"],
     "road3":["N5","N6","N7"],
     "road_A":["A","N3"],
-    "road_B":["B","N6"],
-    "road_C":["C","N6"],
-    "road_D":["D","N7"],
-    "road_E":["E","N7"]
+    "road_B":["B","N6","B1","B2"],
+    "road_C":["N6","C","C1","C2"],
+    "road_D":["D","N7","D1","D2"],
+    "road_E":["N7","E","E1","E2"],
+    "road_cc":["C","CC"],
+    "road_cc1":["C1","CC1"],
+    "road_cc2":["C2","CC2"]
   }
 }

message.proto

@@ -66,7 +66,7 @@ message NavCmd
 
 message NavStatu
 {
-  bool statu = 1; //0:ready  1:running
+  int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，1：single，2：双车
   string key = 4; // 任务唯一码