按照新协议修改指令

ControllWidget.py

@@ -110,7 +110,7 @@ class ControlFrame(QFrame):
         self.WheelBaseEdit.setGeometry(300, 90, 50, 30)
 
         self.btnModelCheck = QCheckBox("双车模式", self)
-        self.btnModelCheck.setGeometry(260, 130, 100, 40)
+        self.btnModelCheck.setGeometry(360, 130, 100, 40)
         self.btnModelCheck.clicked.connect(self.MainAgvchangecb)
 
         self.btnClampCheck = QCheckBox("夹持", self)
@@ -122,6 +122,11 @@ class ControlFrame(QFrame):
         self.btnAuto.setGeometry(260, 195, 100, 40)
         self.btnAuto.stateChanged.connect(self.AutoCheck)
 
+        self.btnAutoDirect = QCheckBox(self)
+        self.btnAutoDirect.setText("自由方向")
+        self.btnAutoDirect.setGeometry(260, 130, 100, 40)
+        self.btnAutoDirect.setCheckState(Qt.Checked)
+
     def Update(self):
 
         if self.robot_.timedRobotStatu_.timeout() == False:
@@ -222,8 +227,12 @@ class ControlFrame(QFrame):
             else:
                 print("agv not in main statu")
         else :
+            autoDirect=False
+            if self.btnAutoDirect.checkState()==Qt.Checked:
+                print("自由方向")
+                autoDirect=True
             self.threadPool_.submit(self.robot_.Navigatting,
-                                    self.begId_, self.targetId_)
+                                    self.begId_, self.targetId_,autoDirect)
 
     def btnCancelClick(self):
         self.threadPool_.submit(self.robot_.CancelNavTask)

RobotData.py

@@ -160,10 +160,10 @@ class Robot(MqttAsync):
     '''
     阻塞直到导航完成
     '''
-    def Navigatting(self, begId, targetId):
+    def Navigatting(self, begId, targetId,autoDirect):
         print("nav")
         self.GeneratePath(begId, targetId)
-        self.ExecPathNodes()
+        self.ExecPathNodes(autoDirect)
         #self.ExecNavtask()
         while self.IsNavigating() == True:
             '''if self.Connected() == False:
@@ -258,17 +258,12 @@ class Robot(MqttAsync):
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
-    def ExecPathNodes(self,autoDirect=False):
-        if self.navCmdTopic_ == None:
-            print("Robot has not set nav cmd topic")
-            return False
-        if not self.ActionType()==ActType.eReady:
-            print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
-            return False
-
-        cmd = message.NavCmd()
-        cmd.action=6
+    def CreateNavPathNodesAction(self,autoDirect):
         if self.currentNavPathNodes_ is not None and self.timedRobotStatu_.timeout() == False:
+            for node in self.currentNavPathNodes_:
+                if isinstance(node,(SpaceNode)):
+                    print(" nodes must all street node")
+                    return None
             statu = self.timedRobotStatu_.statu
             if statu is not None:
                 newAction=message.NewAction()
@@ -276,7 +271,7 @@ class Robot(MqttAsync):
                 limitRotate=message.SpeedLimit()
                 limAdjustV=message.SpeedLimit()
                 limitAdjustH=message.SpeedLimit()
-                limitMPC_v.min=0.05
+                limitMPC_v.min=0.03
                 limitMPC_v.max=1.2
                 limitRotate.min=3*math.pi/180.0
                 limitRotate.max=25*math.pi/180.0
@@ -317,6 +312,7 @@ class Robot(MqttAsync):
                         accuracy.x=0.05
                         accuracy.y=0.1
                         accuracy.theta=3*math.pi/180.0
+                    count+=1
 
                     pathNode=message.PathNode()
                     pathNode.pose.CopyFrom(pose)
@@ -329,19 +325,31 @@ class Robot(MqttAsync):
                     newAction.NodeAngularLimit.CopyFrom(limitRotate)
                     newAction.adjustVelocitylimit.CopyFrom(limAdjustV)
                     newAction.adjustHorizonLimit.CopyFrom(limitAdjustH)
-                cmd.newActions.add().CopyFrom(newAction)
-
-                count+=1
-
+                return newAction
             else:
                 print("statu is None")
-                return False
         else:
             print("current path is none")
+        return None
+    def ExecPathNodes(self,autoDirect):
+        if self.navCmdTopic_ == None:
+            print("Robot has not set nav cmd topic")
             return False
-        if cmd is None:
+        if not self.ActionType()==ActType.eReady:
+            print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
+            return False
+
+        cmd = message.NavCmd()
+        key = str(uuid.uuid4())
+        cmd.key = key
+        cmd.action=0
+
+        action =self.CreateNavPathNodesAction(autoDirect)
+        if action is None:
             print("Nav cmd is None")
             return False
+        cmd.newActions.add().CopyFrom(action)
+
         print(cmd)
         published=False
         while self.IsNavigating() == False:
@@ -359,11 +367,16 @@ class Robot(MqttAsync):
             print(" this agv is single can not switch mode")
             return False
         cmd = message.NavCmd()
+        key = str(uuid.uuid4())
+        cmd.key = key
+        cmd.action=0
+
+        action=message.NewAction()
+        action.type=8
+        action.changedMode = mode
         if mode == 2:
-            cmd.wheelbase = wheelbase
-            cmd.action = 4
-        if mode == 1:
-            cmd.action = 5
+            action.wheelbase = wheelbase
+        cmd.newActions.add().CopyFrom(action)
         loop=3
         while loop>0:
             if mode == 2:
@@ -415,11 +428,12 @@ class Robot(MqttAsync):
             print("clamp closed")
             return True
         cmd = message.NavCmd()
+        cmd.action=0
         key = str(uuid.uuid4())
         cmd.key = (key)
-        act = message.Action()
-        act.type = 3
-        cmd.actions.add().CopyFrom(act)
+        act = message.NewAction()
+        act.type = 6
+        cmd.newActions.add().CopyFrom(act)
         published=False
         while self.IsClampClosed()==False:
             if self.ActionType()==ActType.eClampClose:
@@ -434,11 +448,12 @@ class Robot(MqttAsync):
             print("clamp opended")
             return True
         cmd = message.NavCmd()
+        cmd.action=0
         key = str(uuid.uuid4())
         cmd.key = (key)
-        act = message.Action()
-        act.type = 4
-        cmd.actions.add().CopyFrom(act)
+        act = message.NewAction()
+        act.type = 7
+        cmd.newActions.add().CopyFrom(act)
         published=False
         while self.IsClampOpened()==False:
             if self.ActionType()==ActType.eClampOpen:
@@ -450,7 +465,8 @@ class Robot(MqttAsync):
 
     def CancelNavTask(self):
         cmd = message.NavCmd()
-        cmd.action = 3
+        cmd.action=3
+
         while self.IsNavigating() == True:
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)

message.proto

@@ -50,25 +50,13 @@ message Trajectory
   repeated Pose2d poses=1;
 }
 
-message Action
-{
-  int32 type = 1; // 1:原地调整，2：巡线,3,夹持，4松夹持
-  Pose2d begin = 2;
-  Pose2d target = 3;
-  Pose2d target_diff = 4;
-  SpeedLimit velocity_limit=5;
-  SpeedLimit angular_limit=6;
-  SpeedLimit horize_limit=7;
-}
-
-
 //----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
   int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，5，夹持，6，松夹持，7，切换模式
-  PathNode begNode = 2; //进出库起点
+  PathNode spaceNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
-  PathNode targetNode = 4; //进出库终点
+  PathNode streetNode = 4; //进出库终点
   repeated PathNode pathNodes=5;//导航路径点
   SpeedLimit InOutVLimit=6; //进出库速度
   SpeedLimit NodeVelocityLimit=7; //马路点MPC速度限制
@@ -82,10 +70,9 @@ message NewAction //进库，出库，轨迹导航，夹持，松夹持
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,6,新导航模式
+  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
   string key=2;
   float wheelbase=3;		//轴距
-  repeated Action actions=4;
   repeated NewAction newActions=5;
 }
 
@@ -95,7 +82,6 @@ message NavStatu
   bool main_agv=2; //是否是两节控制plc
   int32 move_mode=3; //运动模式，1：single，2：双车
   string key = 4; // 任务唯一码
-  repeated Action unfinished_actions = 5;  //未完成的动作，第一个即为当前动作
   Trajectory selected_traj = 6;
   Trajectory predict_traj = 7;