1.添加俩单车协动按钮界面，添加协动时路径显示

2.修改路径数据结构及精度下发

JointContrallerWidget.py

@@ -0,0 +1,99 @@
+import time
+from concurrent.futures import ThreadPoolExecutor
+
+from PyQt5.QtCore import QTimer
+from PyQt5.QtWidgets import QFrame, QLabel, QPushButton
+
+import Count
+
+
+class JointControlFrame(QFrame):
+    threadPool_ = ThreadPoolExecutor(5)
+
+    def __init__(self, robot1, robot2):
+        QFrame.__init__(self)
+
+        self.targetId_ = None
+        self.begId_ = None
+        self.robot1 = robot1
+        self.robot2 = robot2
+        self.robot1_parameters = dict()
+        self.robot2_parameters = dict()
+
+        self.InitUI()
+        self.setFrameShape(self.StyledPanel)
+        self.timer_ = QTimer()
+        self.timer_.timeout.connect(self.Update)
+        self.timer_.start(100)
+
+    def testadjust(self, beg, target):
+        autoDirect = True
+        self.threadPool_.submit(self.robot1.Navigatting,
+                                beg, target, autoDirect)
+        time.sleep(2)
+        self.threadPool_.submit(self.robot2.Navigatting,
+                                beg, target, autoDirect)
+
+    def InitUI(self):
+        self.btnSend = QPushButton(self)
+        self.btnSend.setGeometry(80, 5, 100, 40)
+        self.btnSend.setText("双单车协动")
+        self.btnSend.clicked.connect(self.btnSendClick)
+
+        self.btnCancel = QPushButton(self)
+        self.btnCancel.setGeometry(200, 5, 100, 40)
+        self.btnCancel.setText("  取消")
+        self.btnCancel.clicked.connect(self.btnCancelClick)
+
+    def Update(self):
+        if self.robot1.IsNavigating() is True or self.robot2.IsNavigating() is True:
+            if self.btnSend.isEnabled():
+                self.btnSend.setEnabled(False)
+        else:
+            self.btnSend.setEnabled(True)
+
+    def btnSendClick(self):
+        autoDirect = True
+
+        self.threadPool_.submit(self.robot1.Navigatting,
+                                self.robot1_parameters["begId_"], self.robot1_parameters["targetId_"], autoDirect, "DoubleAGV")
+        time.sleep(0.5)
+        self.threadPool_.submit(self.robot2.Navigatting,
+                                self.robot2_parameters["begId_"], self.robot2_parameters["targetId_"], autoDirect, "DoubleAGV")
+
+    def btnCancelClick(self):
+        self.threadPool_.submit(self.robot1.CancelNavTask)
+        self.threadPool_.submit(self.robot2.CancelNavTask)
+
+    def SetRobot1_BaTID(self, begId_, targetId_):
+        self.robot1_parameters["begId_"] = begId_
+        self.robot1_parameters["targetId_"] = targetId_
+
+    def SetRobot2_BaTID(self,  begId_, targetId_):
+        self.robot2_parameters["begId_"] = begId_
+        self.robot2_parameters["targetId_"] = targetId_
+
+
+class CountFrame(QFrame):
+    def __init__(self):
+        QFrame.__init__(self)
+
+        self.InitUI()
+        self.timer_ = QTimer()
+        self.timer_.timeout.connect(self.UpdateUI)
+        self.timer_.start(200)
+
+    def InitUI(self):
+        self.static1 = QLabel(self)
+        self.static1.setText("重载导航次数 : /")
+        self.static1.setGeometry(5, 5, 200, 30)
+
+        self.static2 = QLabel(self)
+        self.static2.setText("单车导航次数 : /")
+        self.static2.setGeometry(5, 40, 200, 30)
+
+    def UpdateUI(self):
+        load_str = "重载导航次数 : %d" % (Count.TestCount().loadCount())
+        single_str = "单车导航次数 : %d" % (Count.TestCount().singleCount())
+        self.static1.setText(load_str)
+        self.static2.setText(single_str)

MapGLWidget.py

@@ -253,6 +253,7 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(pt4[0],pt4[1])
         glVertex2d(pt1[0],pt1[1])
         glEnd()
+
     def DrawRobotData(self,robot):
         if robot is not None:
             if robot.currentNavPath_ is not None:
@@ -291,7 +292,7 @@ class MapGLWidget(PyGLWidget):
                     if not nodeId == nextId:
                         [_, point1] = self.nodes[nodeId]
                         [_, point2] = self.nodes[nextId]
-                        self.drawEdge(point1, point2, 7, [1,1,0])
+                        self.drawEdge(point1, point2, 20, [1,1,0])
         for node in self.nodes.items():
             [name, [type, point]] = node
             if type == "street_node":

RobotData.py

@@ -1,22 +1,25 @@
+import copy
 import enum
 import time
 import message_pb2 as message
 from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
-from dijkstra.Map import DijikstraMap,SpaceNode,StreetNode
+from dijkstra.Map import DijikstraMap, SpaceNode, StreetNode
 import uuid
 import random
 import math
 import Count
+from sigmoid import sigmoid
+
 
 class ActType(enum.Enum):
-    eReady=0
-    eRotation=1
-    eHorizon=2
-    eVertical=3
-    eMPC=4
-    eClampOpen=5
-    eClampClose=6
+    eReady = 0
+    eRotation = 1
+    eHorizon = 2
+    eVertical = 3
+    eMPC = 4
+    eClampOpen = 5
+    eClampClose = 6
 
 
 class TimeStatu:
@@ -29,6 +32,7 @@ class TimeStatu:
         tm = time.time()
         return tm - self.time > self.timeout_ms or self.statu == None
 
+
 class Robot(MqttAsync):
 
     def __init__(self, name=""):
@@ -46,10 +50,10 @@ class Robot(MqttAsync):
         self.robotColor_ = [0.7, 0.2, 0.3]  # agv当前颜色
         self.name_ = name
 
-        self.begId_="------"
-        self.targetId_="------"
+        self.begId_ = "------"
+        self.targetId_ = "------"
 
-        self.autoTest_=False
+        self.autoTest_ = False
 
         self.l_ = 0.8  # 轮长
         self.L_ = 1.3  # 轴距
@@ -132,38 +136,51 @@ class Robot(MqttAsync):
         return self.timedNavStatu_.timeout() == False and self.timedRobotStatu_.timeout() == False
 
     def IsNavigating(self):
-        return not self.ActionType()==ActType.eReady
+        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'''
 
-    def GeneratePath(self, begID, endID):
+    def GeneratePath(self, begID, endID, task_type="None"):
         self.currentNavPathNodes_ = DijikstraMap().GetShortestPath(begID, endID)
+
+        if task_type == "DoubleAGV":
+            print("AdjustPath------------------")
+            if self.IsMainModeStatu() is False:
+                print("原始路径：")
+                for node in self.currentNavPathNodes_:
+                    print(node.id_, node.y_)
+                self.currentNavPathNodes_ = self.AdjustPath(self.currentNavPathNodes_)
+                print("修改路径：")
+                for node in self.currentNavPathNodes_:
+                    print(node.id_, node.y_)
+
         self.currentNavPath_ = DijikstraMap().CreatePath(self.currentNavPathNodes_, 0.5)
         if self.currentNavPathNodes_ is not None and self.currentNavPath_ is not None:
-            self.begId_=begID
-            self.targetId_=endID
+            self.begId_ = begID
+            self.targetId_ = endID
             return True
         return False
 
-
     def PositionId(self):
         if self.timedRobotStatu_.timeout():
             return None
-        x=self.timedRobotStatu_.statu.x
-        y=self.timedRobotStatu_.statu.y
+        x = self.timedRobotStatu_.statu.x
+        y = self.timedRobotStatu_.statu.y
         djks_map = DijikstraMap()
         [label, pt] = djks_map.findNeastNode([x, y])
-        return [label,pt]
+        return [label, pt]
+
     '''
     阻塞直到导航完成
     '''
-    def Navigatting(self, begId, targetId,autoDirect):
+
+    def Navigatting(self, begId, targetId, autoDirect, task_type="None"):
         print("nav")
-        self.GeneratePath(begId, targetId)
-        #self.ExecPathNodes(autoDirect)
+        self.GeneratePath(begId, targetId, task_type=task_type)
+        # self.ExecPathNodes(autoDirect)
         self.ExecNavtask(autoDirect)
         while self.IsNavigating() == True:
             '''if self.Connected() == False:
@@ -180,92 +197,104 @@ class Robot(MqttAsync):
 
         return True
 
-    def AutoTestNavClamp(self,begId,targetId):
-        beg=begId
-        target=targetId
+    def AutoTestNavClamp(self, begId, targetId):
+        beg = begId
+        target = targetId
         while self.autoTest_:
-            if self.TestNavClampOnce(beg,target)==False:
+            if self.TestNavClampOnce(beg, target) == False:
                 print("  quit auto Test")
                 break
             print("1111")
-            posInfo=self.PositionId()
+            posInfo = self.PositionId()
             if posInfo is not None:
-                [label,pt]=posInfo
-                beg=label
-                print("beg",beg)
-                node=DijikstraMap().GetVertex(beg)
+                [label, pt] = posInfo
+                beg = label
+                print("beg", beg)
+                node = DijikstraMap().GetVertex(beg)
 
                 if node is not None:
-                    if isinstance(node,(SpaceNode)):
-                        target="O"
-                    if isinstance(node,(StreetNode)):
-                        end_nodes=["E","B","C","D"]
-                        id=random.randint(0,1000)%4
-                        target=end_nodes[id]
-                    print("3333",target)
-            print("  Next nav clamp cmd,%s to %s"%(beg,target))
+                    if isinstance(node, (SpaceNode)):
+                        target = "O"
+                    if isinstance(node, (StreetNode)):
+                        end_nodes = ["E", "B", "C", "D"]
+                        id = random.randint(0, 1000) % 4
+                        target = end_nodes[id]
+                    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:
+            runningStatu = self.timedNavStatu_.statu
+            if runningStatu.statu == 0:
                 return ActType.eReady
-            if runningStatu.statu==1:
+            if runningStatu.statu == 1:
                 return ActType.eRotation
-            if runningStatu.statu==2:
+            if runningStatu.statu == 2:
                 return ActType.eHorizon
-            if runningStatu.statu==3:
+            if runningStatu.statu == 3:
                 return ActType.eVertical
-            if runningStatu.statu==4:
+            if runningStatu.statu == 4:
                 return ActType.eMPC
-            if runningStatu.statu==5:
+            if runningStatu.statu == 5:
                 return ActType.eClampOpen
-            if runningStatu.statu==6:
+            if runningStatu.statu == 6:
                 return ActType.eClampClose
         else:
             return ActType.eReady
 
     @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
+    def generateProtoNode(node, accuracy):
+        [dx, dy, dyaw] = accuracy
+        protoNode = message.PathNode()
+        protoNode.x = node.x_
+        protoNode.y = node.y_
+        protoNode.l = dx
+        protoNode.w = dy
+        protoNode.theta = dyaw
         return protoNode
-    def ExecNavtask(self,autoDirect):
-        cmd=message.NavCmd()
+
+    def ExecNavtask(self, autoDirect):
+        cmd = message.NavCmd()
         key = str(uuid.uuid4())
         cmd.key = key
-        cmd.action=0
-
-        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.8
-
-        actions=self.SplitPath(self.currentNavPathNodes_)
+        cmd.action = 0
+
+        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.8
+
+        # if task_type == "DoubleAGV":
+        #     print("AdjustPath------------------")
+        #     if self.IsMainModeStatu() is False:
+        #         print("原始路径：")
+        #         for node in self.currentNavPathNodes_:
+        #             print(node.id_, node.y_)
+        #         self.currentNavPathNodes_ = self.AdjustPath(self.currentNavPathNodes_)
+        #         print("修改路径：")
+        #         for node in self.currentNavPathNodes_:
+        #             print(node.id_, node.y_)
+
+        actions = self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
-            [type,nodes]=action
-            if type=="input":
-                [street,space]=nodes
-                protoSpace=self.generateProtoNode(space,[0.05,0.05,0.7*math.pi/180.0])
-                protoStreet=self.generateProtoNode(street,[0.05,0.05,1*math.pi/180.0])
-                act=message.NewAction()
-                act.type=1
+            [type, nodes] = action
+            if type == "input":
+                [street, space] = nodes
+                protoSpace = self.generateProtoNode(space, [0.05, 0.05, 0.7 * math.pi / 180.0])
+                protoStreet = self.generateProtoNode(street, [0.05, 0.05, 1 * math.pi / 180.0])
+                act = message.NewAction()
+                act.type = 1
                 act.spaceNode.CopyFrom(protoSpace)
                 act.streetNode.CopyFrom(protoStreet)
                 act.NodeAngularLimit.CopyFrom(limitRotate)
@@ -273,12 +302,12 @@ class Robot(MqttAsync):
                 act.adjustVelocitylimit.CopyFrom(limAdjustV)
                 act.adjustHorizonLimit.CopyFrom(limitAdjustH)
                 cmd.newActions.add().CopyFrom(act)
-            if type=="output":
-                [space,street]=nodes
-                protoSpace=self.generateProtoNode(space,[0.05,0.05,0.7*math.pi/180.0])
-                protoStreet=self.generateProtoNode(street,[0.05,0.05,1*math.pi/180.0])
-                act=message.NewAction()
-                act.type=2
+            if type == "output":
+                [space, street] = nodes
+                protoSpace = self.generateProtoNode(space, [0.05, 0.05, 0.7 * 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)
@@ -286,8 +315,8 @@ class Robot(MqttAsync):
                 act.adjustVelocitylimit.CopyFrom(limAdjustV)
                 act.adjustHorizonLimit.CopyFrom(limitAdjustH)
                 cmd.newActions.add().CopyFrom(act)
-            if type=="nav":
-                action =self.CreateNavPathNodesAction(nodes,autoDirect)
+            if type == "nav":
+                action = self.CreateNavPathNodesAction(nodes, autoDirect)
                 if action is None:
                     print("Nav cmd is None")
                     return False
@@ -298,48 +327,50 @@ class Robot(MqttAsync):
             print("Nav cmd is None")
             return False
 
-        count=3
-        while self.ActionType() == ActType.eReady and count>0:
+        count = 3
+        while self.ActionType() == ActType.eReady and count > 0:
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-            count-=1
+            count -= 1
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
-    def CreateNavPathNodesAction(self,path,autoDirect):
+
+    def CreateNavPathNodesAction(self, path, autoDirect):
         if path is not None and self.timedRobotStatu_.timeout() == False:
             for node in path:
-                if isinstance(node,(SpaceNode)):
+                if isinstance(node, (SpaceNode)):
                     print(" nodes must all street node")
                     return None
             statu = self.timedRobotStatu_.statu
             if statu is not None:
-                newAction=message.NewAction()
-                limitMPC_v=message.SpeedLimit()
-                limitRotate=message.SpeedLimit()
-                limAdjustV=message.SpeedLimit()
-                limitAdjustH=message.SpeedLimit()
-                limitMPC_v.min=0.03
-                limitMPC_v.max=1.5
-                limitRotate.min=3*math.pi/180.0
-                limitRotate.max=25*math.pi/180.0
-                limAdjustV.min=0.03
-                limAdjustV.max=0.3
-                limitAdjustH.min=0.03
-                limitAdjustH.max=0.3
-
-                count=0
-                size=len(path)
+                newAction = message.NewAction()
+                limitMPC_v = message.SpeedLimit()
+                limitRotate = message.SpeedLimit()
+                limAdjustV = message.SpeedLimit()
+                limitAdjustH = message.SpeedLimit()
+                limitMPC_v.min = 0.03
+                limitMPC_v.max = 1.5
+                limitRotate.min = 3 * math.pi / 180.0
+                limitRotate.max = 25 * math.pi / 180.0
+                limAdjustV.min = 0.03
+                limAdjustV.max = 0.3
+                limitAdjustH.min = 0.03
+                limitAdjustH.max = 0.3
+
+                count = 0
+                size = len(path)
                 for i in range(size):
-                    node=path[i]
-                    pose=message.Pose2d()
-                    accuracy=message.Pose2d()
-                    pose.x=node.x_
-                    pose.y=node.y_
-                    if i+1<size:
-                        next=path[i+1]
+                    node = path[i]
+                    pathNode = message.PathNode()
+                    pathNode.x = node.x_
+                    pathNode.y = node.y_
+                    distance = 0
+                    if i + 1 < size:
+                        next = path[i + 1]
                         vector = [next.x_ - node.x_, next.y_ - node.y_]
                         dx = vector[0]
                         dy = vector[1]
+                        distance = math.sqrt(dx * dx + dy * dy)
                         yaw = math.asin(dy / math.sqrt(dx * dx + dy * dy))
                         if yaw >= 0:
                             if dx < 0:
@@ -347,31 +378,23 @@ class Robot(MqttAsync):
                         if yaw < 0:
                             if dx < 0:
                                 yaw = -math.pi - yaw
-                        pose.theta=yaw
+                        pathNode.theta = -yaw
                     else:
-                        pose.theta=0
+                        pathNode.theta = 0
 
-                    if self.IsMainModeStatu() and count==0:
-                        accuracy.x=0.05
-                        accuracy.y=0.1
-                        accuracy.theta=0.5*math.pi/(180.0)
 
-                    if count==size-1:
-                        accuracy.x=0.02
-                        accuracy.y=0.02
-                        accuracy.theta=1*math.pi/(180.0)
+                    if self.IsMainModeStatu():
+                        pathNode.l = 0.15
+                        pathNode.w = 0.05
                     else:
-                        accuracy.x=0.05
-                        accuracy.y=0.1
-                        accuracy.theta=2*math.pi/180.0
-                    count+=1
-
-                    pathNode=message.PathNode()
-                    pathNode.pose.CopyFrom(pose)
-                    pathNode.accuracy.CopyFrom(accuracy)
-                    newAction.type=4
+                        pathNode.l = 0.3
+                        pathNode.w = 0.1
+
+                    count += 1
+
+                    newAction.type = 4
                     if autoDirect:
-                        newAction.type=3
+                        newAction.type = 3
                     newAction.pathNodes.add().CopyFrom(pathNode)
                     newAction.NodeVelocityLimit.CopyFrom(limitMPC_v)
                     newAction.NodeAngularLimit.CopyFrom(limitRotate)
@@ -383,80 +406,108 @@ class Robot(MqttAsync):
         else:
             print("current path is none")
         return None
-    def ExecPathNodes(self,autoDirect):
+
+    def ExecPathNodes(self, autoDirect):
         if self.navCmdTopic_ == None:
             print("Robot has not set nav cmd topic")
             return False
-        if not self.ActionType()==ActType.eReady:
+        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
+        cmd.action = 0
 
-        action =self.CreateNavPathNodesAction(self.currentNavPathNodes_,autoDirect)
+        action = self.CreateNavPathNodesAction(self.currentNavPathNodes_, autoDirect)
         if action is None:
             print("Nav cmd is None")
             return False
         cmd.newActions.add().CopyFrom(action)
 
         print(cmd)
-        published=False
+        published = False
         while self.IsNavigating() == False:
             if not self.ActionType() == ActType.eReady:
-                published=True
-            if published==False:
+                published = True
+            if published == False:
                 self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
             time.sleep(1)
         print("send nav cmd completed!!!")
         return True
 
+    '''
+    根据AGV单车的起始位置调整路径
+    '''
+
+    def AdjustPath(self, path):
+        adjusted_path = copy.deepcopy(path)
+        y_offset = 1.2
+        dy = 0
+        # if self.IsMainAgv() or self.name_ == "AgvMain":
+        if self.name_ == "AgvMain":
+            dy = 1
+        # if not self.IsMainAgv() or self == "AGV2":
+        if self.name_ == "AGV2":
+            dy = -1
+        print("dy:", dy)
+        for node in adjusted_path:
+            if isinstance(node, StreetNode):
+                if dy > 0:
+                    node.y_ += y_offset
+                elif dy < 0:
+                    node.y_ -= y_offset
+                else:
+                    pass
+
+        return adjusted_path
+
     '''
     拆分路径，生成动作集,input:入库动作，nav:导航动作，output:出库动作
     '''
-    def SplitPath(self,path):
 
-        space_count=0
+    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:
+            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
+        actions = []
+        lastNode = None
 
-        navIndex=-1
+        navIndex = -1
         for node in path:
-            if lastNode==None:
-                lastNode=node
+            if lastNode == None:
+                lastNode = node
                 continue
 
-            if isinstance(node,(SpaceNode)):    #当前点是车位点
-                if isinstance(lastNode,(SpaceNode)):  #上一点为车位点
+            if isinstance(node, (SpaceNode)):  # 当前点是车位点
+                if isinstance(lastNode, (SpaceNode)):  # 上一点为车位点
                     print("Error: last node and current node is space node ")
-                    lastNode=None
+                    lastNode = None
                     continue
-                elif isinstance(lastNode,(StreetNode)):    #上一点为马路点,则动作为入库
-                    actions.append(["input",[lastNode,node]])
-                    lastNode=None
+                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]])
+            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
+                elif isinstance(lastNode, (StreetNode)):
+                    if navIndex < 0:
+                        actions.append(["nav", [lastNode]])
+                        navIndex = len(actions) - 1
                     actions[navIndex][1].append(node)
-            lastNode=node
+            lastNode = node
 
         return actions
 
@@ -467,16 +518,16 @@ class Robot(MqttAsync):
         cmd = message.NavCmd()
         key = str(uuid.uuid4())
         cmd.key = key
-        cmd.action=0
+        cmd.action = 0
 
-        action=message.NewAction()
-        action.type=8
+        action = message.NewAction()
+        action.type = 8
         action.changedMode = mode
         if mode == 2:
             action.wheelbase = wheelbase
         cmd.newActions.add().CopyFrom(action)
-        loop=3
-        while loop>0:
+        loop = 3
+        while loop > 0:
             if mode == 2:
                 if self.IsMainModeStatu():
                     return True
@@ -484,7 +535,7 @@ class Robot(MqttAsync):
                 if self.IsMainModeStatu() == False:
                     return True
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
-            loop-=1
+            loop -= 1
             time.sleep(0.5)
         return False
 
@@ -498,6 +549,7 @@ class Robot(MqttAsync):
                 return self.timedRobotStatu_.statu.agvStatu.clamp == 1
 
         return False
+
     def IsClampRunning(self):
         if self.timedRobotStatu_.timeout() == False:
             if self.IsMainModeStatu():
@@ -525,17 +577,17 @@ class Robot(MqttAsync):
             print("clamp closed")
             return True
         cmd = message.NavCmd()
-        cmd.action=0
+        cmd.action = 0
         key = str(uuid.uuid4())
         cmd.key = (key)
         act = message.NewAction()
         act.type = 6
         cmd.newActions.add().CopyFrom(act)
-        published=False
-        while self.IsClampClosed()==False:
-            if self.ActionType()==ActType.eClampClose:
-                published=True
-            if published==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(0.5)
         return True
@@ -545,24 +597,24 @@ class Robot(MqttAsync):
             print("clamp opended")
             return True
         cmd = message.NavCmd()
-        cmd.action=0
+        cmd.action = 0
         key = str(uuid.uuid4())
         cmd.key = (key)
         act = message.NewAction()
         act.type = 7
         cmd.newActions.add().CopyFrom(act)
-        published=False
-        while self.IsClampOpened()==False:
-            if self.ActionType()==ActType.eClampOpen:
-                published=True
-            if published==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(0.5)
         return True
 
     def CancelNavTask(self):
         cmd = message.NavCmd()
-        cmd.action=3
+        cmd.action = 3
 
         while self.IsNavigating() == True:
             self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))

count.json

@@ -1 +1 @@
-{"load_count": 531, "single_count": 673}
+{"load_count": 531, "single_count": 1146}

map.json

@@ -8,6 +8,7 @@
     "V4": [5.37,-0.42],
     "V5": [20.43,-0.42],
     "V6": [22.78,-0.42],
+    "V7": [5.37,16.27],
     "S4":[5.37,3.58],
     "N1":[26.65,-0.42],
     "H1": [26.65,11.5],
@@ -37,6 +38,7 @@
   "roads":{
     "road1":["I1","I","I2","H2"],
     "road2":["H1","N1","H2"],
+    "road3":["V7","H2"],
     "conn":["V1","V2","V3","V4","V5","V6","N1","N2"],
     "rp":["V4","PW","PWA"],
     "vp1":["V1","P1","P","P2"],

message.proto

@@ -39,10 +39,14 @@ message Pose2d
   float theta=3;
 }
 
+
 message PathNode  //导航路径点及精度
 {
-  Pose2d pose=1;    //路径点
-  Pose2d accuracy=2; //要求精度
+  float x=1; //路径点
+  float y=2;
+  float l=3; //要求精度范围
+  float w=4;
+  float theta=5;
 }
 
 message Trajectory
@@ -53,7 +57,7 @@ message Trajectory
 //----------------------------
 message NewAction //进库，出库，轨迹导航，夹持，松夹持
 {
-  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，5，夹持，6，松夹持，7，切换模式
+  int32 type =1; //1,进库，2，出库，3，自动选择动作导航，4，保证agv朝前导航，5，汽车模型导航，6，夹持，7，松夹持，8，切换模式
   PathNode spaceNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
   PathNode streetNode = 4; //进出库终点
@@ -70,9 +74,8 @@ message NewAction //进库，出库，轨迹导航，夹持，松夹持
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式,
+  int32 action=1;  //  0 开始任务，1 pause， 2 continue ，3 cancel
   string key=2;
-  float wheelbase=3;		//轴距
   repeated NewAction newActions=5;
 }
 

sigmoid.py

@@ -0,0 +1,44 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# f(x) = 1/(1+e(-z))
+
+'''
+min_value: y最小值
+max_value: y最大值
+right_offset: 曲线右移
+span: y从接近最小到接近最大时,自变量x的跨度
+'''
+
+
+def sigmoid(x, min_value=0.3, max_value=5, right_offset=5, span=10):
+    # 右移
+    x -= right_offset
+    # 缩放
+    x *= 10 / span
+    # 振幅
+    return (max_value - min_value) / (1 + np.exp(-x)) + min_value
+
+
+# def test():
+#     plt.rcParams["font.family"] = "SimHei"
+#     plt.rcParams["axes.unicode_minus"] = False
+#     plt.rcParams["font.size"] = 12
+#
+#     z = np.linspace(-10, 10, 200)
+#     plt.plot(z, sigmoid(z))
+#
+#     plt.axvline(x=0, ls="--", c="k")
+#     plt.axhline(ls=":", c="k")
+#     plt.axhline(y=0.5, ls=":", c="k")
+#     plt.axhline(y=1, ls=":", c="k")
+#     plt.xlabel("z值")
+#     plt.ylabel("sigmoid(z)值")
+
+
+if __name__ == "__main__":
+    for i in range(20):
+        x = i + 1
+        print(x, sigmoid(x, span=10))
+
+

test.py

@@ -42,6 +42,7 @@ from MapGLWidget import MapGLWidget
 import json
 import dijkstra.Map as mp
 import ControllWidget
+import JointContrallerWidget
 import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
@@ -65,6 +66,8 @@ class MainWindow(QMainWindow):
         config1={"label":"AgvMain",
                  "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.225.224",1883,"admin","zx123456"],
                  "mqtt":["pyui1","192.168.0.70",1883,"admin","zx123456"],
                  "subTopics":{"agv_main/agv_statu":message.RobotStatu.__name__,
                               "agv_main/nav_statu":message.NavStatu.__name__},
@@ -74,13 +77,17 @@ class MainWindow(QMainWindow):
         config2={"label":"AGV2",
                  "street_nodes":self.ui_nodes["street_nodes"],
                  "space_nodes":self.ui_nodes["space_nodes"],
-                 "mqtt":["pyui2","192.168.0.71",1883,"admin","zx123456"],
+                 # "mqtt":["pyui2","192.168.0.224",1883,"admin","zx123456"],
+                 # "mqtt":["pyui2","192.168.225.224",1883,"admin","zx123456"],
+                 "mqtt":["pyui1","192.168.0.71",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_)
 
         splitter_main = self.split_()
         self.setCentralWidget(splitter_main)
@@ -92,6 +99,7 @@ class MainWindow(QMainWindow):
         self.timer_.timeout.connect(self.Update)
         self.timer_.start(100)
 
+
     def LoadMap(self):
         self.gl = MapGLWidget()   #将opengl例子嵌入GUI
         map=self.load_map("./map.json")
@@ -126,6 +134,8 @@ 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()
 
     #窗口基础属性
@@ -150,10 +160,12 @@ class MainWindow(QMainWindow):
         splitter.addWidget(self.count_frame_)
         splitter.addWidget(self.Controller1)
         splitter.addWidget(self.Controller2)
+        splitter.addWidget(self.JointContrallerWidget)
 
         splitter.setStretchFactor(0,1)
         splitter.setStretchFactor(1,3)
         splitter.setStretchFactor(2,3)
+        splitter.setStretchFactor(3,1)
 
         splitter_main.addWidget(splitter)
         splitter_main.addWidget(self.gl)