修改指令发送，兼容车位节点

RobotData.py

@@ -163,8 +163,8 @@ class Robot(MqttAsync):
     def Navigatting(self, begId, targetId,autoDirect):
         print("nav")
         self.GeneratePath(begId, targetId)
-        self.ExecPathNodes(autoDirect)
-        #self.ExecNavtask()
+        #self.ExecPathNodes(autoDirect)
+        self.ExecNavtask(autoDirect)
         while self.IsNavigating() == True:
             '''if self.Connected() == False:
                 print("robot disconnected cancel task")
@@ -229,22 +229,82 @@ class Robot(MqttAsync):
         else:
             return ActType.eReady
 
-    def ExecNavtask(self):
-        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
+    @staticmethod
+    def generateProtoNode(node,accuracy):
+        [dx,dy,dyaw]=accuracy
+        protoNode=message.PathNode()
+        protoNode.pose.x=node.x_
+        protoNode.pose.y=node.y_
+        protoNode.accuracy.x=dx
+        protoNode.accuracy.y=dx
+        protoNode.accuracy.theta=dyaw
+        return protoNode
+    def ExecNavtask(self,autoDirect):
+        cmd=message.NavCmd()
+        key = str(uuid.uuid4())
+        cmd.key = key
+        cmd.action=0
 
-        cmd = None
-        if self.currentNavPathNodes_ is not None and self.timedRobotStatu_.timeout() == False:
-            statu = self.timedRobotStatu_.statu
-            if statu is not None:
-                cmd = DijikstraMap().CreateNavCmd([statu.x, statu.y, statu.theta], self.currentNavPathNodes_)
-        else:
-            print("current path is none")
-            return False
+        limitMPC_v=message.SpeedLimit()
+        limitRotate=message.SpeedLimit()
+        limAdjustV=message.SpeedLimit()
+        limitAdjustH=message.SpeedLimit()
+        limitInOutV=message.SpeedLimit()
+        limitMPC_v.min=0.03
+        limitMPC_v.max=1.2
+        limitRotate.min=3*math.pi/180.0
+        limitRotate.max=25*math.pi/180.0
+        limAdjustV.min=0.03
+        limAdjustV.max=0.5
+        limitAdjustH.min=0.03
+        limitAdjustH.max=0.3
+        limitInOutV.min=0.03
+        limitInOutV.max=0.5
+
+
+
+        '''
+        newAction.NodeVelocityLimit.CopyFrom(limitMPC_v)
+                newAction.NodeAngularLimit.CopyFrom(limitRotate)
+                newAction.adjustVelocitylimit.CopyFrom(limAdjustV)
+                newAction.adjustHorizonLimit.CopyFrom(limitAdjustH)
+        '''
+        actions=self.SplitPath(self.currentNavPathNodes_)
+        for action in actions:
+            [type,nodes]=action
+            if type=="input":
+                [street,space]=nodes
+                protoSpace=self.generateProtoNode(space,[0.02,0.02,0.5*math.pi/180.0])
+                protoStreet=self.generateProtoNode(street,[0.02,0.02,1*math.pi/180.0])
+                act=message.NewAction()
+                act.type=1
+                act.spaceNode.CopyFrom(protoSpace)
+                act.streetNode.CopyFrom(protoStreet)
+                act.NodeAngularLimit.CopyFrom(limitRotate)
+                act.InOutVLimit.CopyFrom(limitInOutV)
+                act.adjustVelocitylimit.CopyFrom(limAdjustV)
+                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
+                cmd.newActions.add().CopyFrom(act)
+            if type=="output":
+                [space,street]=nodes
+                protoSpace=self.generateProtoNode(space,[0.02,0.2,0.5*math.pi/180.0])
+                protoStreet=self.generateProtoNode(street,[0.05,0.05,1*math.pi/180.0])
+                act=message.NewAction()
+                act.type=2
+                act.spaceNode.CopyFrom(protoSpace)
+                act.streetNode.CopyFrom(protoStreet)
+                act.InOutVLimit.CopyFrom(limitInOutV)
+                act.NodeAngularLimit.CopyFrom(limitRotate)
+                act.adjustVelocitylimit.CopyFrom(limAdjustV)
+                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
+                cmd.newActions.add().CopyFrom(act)
+            if type=="nav":
+                action =self.CreateNavPathNodesAction(nodes,autoDirect)
+                if action is None:
+                    print("Nav cmd is None")
+                    return False
+                else:
+                    cmd.newActions.add().CopyFrom(action)
 
         if cmd is None:
             print("Nav cmd is None")
@@ -258,9 +318,9 @@ class Robot(MqttAsync):
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
-    def CreateNavPathNodesAction(self,autoDirect):
-        if self.currentNavPathNodes_ is not None and self.timedRobotStatu_.timeout() == False:
-            for node in self.currentNavPathNodes_:
+    def CreateNavPathNodesAction(self,path,autoDirect):
+        if path is not None and self.timedRobotStatu_.timeout() == False:
+            for node in path:
                 if isinstance(node,(SpaceNode)):
                     print(" nodes must all street node")
                     return None
@@ -281,15 +341,15 @@ class Robot(MqttAsync):
                 limitAdjustH.max=0.3
 
                 count=0
-                size=len(self.currentNavPathNodes_)
+                size=len(path)
                 for i in range(size):
-                    node=self.currentNavPathNodes_[i]
+                    node=path[i]
                     pose=message.Pose2d()
                     accuracy=message.Pose2d()
                     pose.x=node.x_
                     pose.y=node.y_
                     if i+1<size:
-                        next=self.currentNavPathNodes_[i+1]
+                        next=path[i+1]
                         vector = [next.x_ - node.x_, next.y_ - node.y_]
                         dx = vector[0]
                         dy = vector[1]
@@ -344,7 +404,7 @@ class Robot(MqttAsync):
         cmd.key = key
         cmd.action=0
 
-        action =self.CreateNavPathNodesAction(autoDirect)
+        action =self.CreateNavPathNodesAction(self.currentNavPathNodes_,autoDirect)
         if action is None:
             print("Nav cmd is None")
             return False
@@ -361,6 +421,59 @@ class Robot(MqttAsync):
         print("send nav cmd completed!!!")
         return True
 
+    '''
+    拆分路径，生成动作集,input:入库动作，nav:导航动作，output:出库动作
+    '''
+    def SplitPath(self,path):
+
+        space_count=0
+        for i in range(len(path)):
+            if isinstance(path[i],(SpaceNode)):
+                space_count+=1
+                if i==0 or i==len(path)-1:
+                    pass
+                else:
+                    print("Error: space node is not first/last of path")
+                    return None
+
+        actions=[]
+        lastNode=None
+
+        navIndex=-1
+        for node in path:
+            if lastNode==None:
+                lastNode=node
+                continue
+
+            if isinstance(node,(SpaceNode)):    #当前点是车位点
+                if isinstance(lastNode,(SpaceNode)):  #上一点为车位点
+                    print("Error: last node and current node is space node ")
+                    lastNode=None
+                    continue
+                elif isinstance(lastNode,(StreetNode)):    #上一点为马路点,则动作为入库
+                    actions.append(["input",[lastNode,node]])
+                    lastNode=None
+                    continue
+
+            if isinstance(node,(StreetNode)):
+                if isinstance(lastNode,(SpaceNode)):  #上一点为车位点, 出库
+                    actions.append(["output",[lastNode,node]])
+
+                elif isinstance(lastNode,(StreetNode)):
+                    if navIndex<0:
+                        actions.append(["nav",[lastNode]])
+                        navIndex=len(actions)-1
+                    actions[navIndex][1].append(node)
+            lastNode=node
+
+        return actions
+
+
+
+
+
+
+
 
     def SwitchMoveMode(self, mode, wheelbase):
         if self.IsMainAgv() == False: