两节agv独立导航

ControllWidget.py

@@ -1,41 +1,38 @@
 
-from PyQt5.QtWidgets import QWidget, QApplication,QMainWindow,QLabel,QLineEdit,QPushButton,QComboBox
+from PyQt5.QtWidgets import QWidget,QFrame,QLabel,QLineEdit,QPushButton,QComboBox
 from PyQt5.QtGui import QPixmap,QPainter,QResizeEvent,QCloseEvent,QPaintEvent,QFont
-from PyQt5.QtCore import QSize,QTimer,QRect,Qt
+from PyQt5.QtCore import QSize,QTimer
+from RobotData import Robot
 
+class ControlFrame(QFrame):
 
-class Frame(QMainWindow):
     def __init__(self,UI_data):
-        super(Frame, self).__init__()
-        self.setGeometry(0,0,400,500)
-        self.btnCreateTrajFunc=None
-        self.btnSendClickFunc=None
-        self.btnCancelFunc=None
+        QFrame.__init__(self)
+        self.robot_=Robot()
+        self.setGeometry(0,0,500,500)
         self.InitUI(UI_data)
-
-
-    def SetBtnFunc(self,funcs):
-        if not funcs.get("CreateTraj")==None:
-            self.btnCreateTrajFunc=funcs["CreateTraj"]
-        if not funcs.get("SendCmd")==None:
-            self.btnSendClickFunc=funcs["SendCmd"]
-        if not funcs.get("CancelCmd")==None:
-            self.btnCancelFunc=funcs["CancelCmd"]
-
+        self.setFrameShape(self.StyledPanel)
+        self.timer_=QTimer()
+        self.timer_.timeout.connect(self.Update)
+        self.timer_.start(100)
 
     def InitUI(self,UI_data):
+        [name,nodes]=UI_data
+        self.basestatic=QLabel(self)
+        self.basestatic.setText(name)
+        self.basestatic.setGeometry(20, 20, 80, 30)
 
         self.begstatic=QLabel(self)
         self.begstatic.setText("  起点")
-        self.begstatic.setGeometry(0, 0, 50, 30)
+        self.begstatic.setGeometry(100, 5, 50, 30)
         self.begQc=QComboBox(self)
-        self.begQc.setGeometry(60, 0, 80, 30)
+        self.begQc.setGeometry(160, 5, 80, 30)
 
         self.endstatic=QLabel(self)
         self.endstatic.setText("  终点")
-        self.endstatic.setGeometry(0, 50, 50, 30)
+        self.endstatic.setGeometry(100, 50, 50, 30)
         self.endQc=QComboBox(self)
-        self.endQc.setGeometry(60, 50, 80, 30)
+        self.endQc.setGeometry(160, 50, 80, 30)
 
         self.btn=QPushButton(self)
         self.btn.setGeometry(20, 100, 100, 40)
@@ -52,22 +49,42 @@ class Frame(QMainWindow):
         self.btnCancel.setText("  取消导航")
         self.btnCancel.clicked.connect(self.btnCancelClick)
 
-        cb_list=UI_data['nodes']
-        self.begQc.addItems(cb_list)
-        self.endQc.addItems(cb_list)
+
+        self.begQc.addItems(nodes)
+        self.endQc.addItems(nodes)
+
+        self.posestatic=QLabel(self)
+        self.posestatic.setGeometry(260, 5, 100, 90)
+
+        self.LedLabel=QLabel(self)
+        self.LedLabel.setGeometry(280, 120, 50, 50)
+
+    def Update(self):
+        if self.robot_.timedPose_.timeout()==False:
+            x=self.robot_.timedPose_.statu.x
+            y=self.robot_.timedPose_.statu.y
+            yaw=self.robot_.timedPose_.statu.theta
+            self.posestatic.setText("x : %f\ny : %f\nΘ : %f"%(x,y,yaw))
+        statu="min-width: 64px; min-height: 64px;max-width:64px;\
+                max-height: 64px;border-radius: 32px;  border:1px solid black;background:green"
+        if self.robot_.Connected()==False:
+            statu="min-width: 64px; min-height: 64px;max-width:64px;\
+                max-height: 64px;border-radius: 32px;  border:1px solid black;background:gray"
+        elif self.robot_.IsNavigating()==True:
+            statu="min-width: 64px; min-height: 64px;max-width:64px;\
+                max-height: 64px;border-radius: 32px;  border:1px solid black;background:yellow"
+        self.LedLabel.setStyleSheet(statu)
 
 
     def btnCreateClick(self):
-        if not self.btnCreateTrajFunc==None:
-            id1=self.begQc.currentText()
-            id2=self.endQc.currentText()
-            self.btnCreateTrajFunc(id1,id2)
+        id1=self.begQc.currentText()
+        id2=self.endQc.currentText()
+        self.robot_.GeneratePath(id1,id2)
+
 
     def btnSendClick(self):
-        if not self.btnSendClickFunc==None:
-            self.btnSendClickFunc()
+        self.robot_.ExecNavtask()
     def btnCancelClick(self):
-        if not self.btnCancelFunc==None:
-            self.btnCancelFunc()
+        self.robot_.CancelNavTask()
 
 

MapGLWidget.py

@@ -1,145 +1,206 @@
-
 import math
 import random
 import time
 from PyGLWidget import PyGLWidget
 from OpenGL.GL import *
-
+import RobotData as RD
 import message_pb2 as message
-class TimeStatu:
-    def __init__(self,statu=None,timeout=3):
-        self.statu=statu
-        self.time=time.time()
-        self.timeout_ms=timeout
-
-    def timeout(self):
-        tm=time.time()
-        return tm-self.time>self.timeout_ms or self.statu==None
+
+
 class MapGLWidget(PyGLWidget):
 
-    colors={"agv1":[1,0,0],
-            "agv2":[0,1,0],
-            "agv":[0,1,1]
-            }
-
-    nodes={}
-    roads={}
-    paths={}
-    timedAgv1=TimeStatu(None,0.5)
-    timedAgv2=TimeStatu(None,0.5)
-    def ResetAgv1(self,statu):
-        self.timedAgv1=TimeStatu(statu,0.5)
-        self.update()
-    def ResetAgv2(self,statu):
-        self.timedAgv2=TimeStatu(statu,0.5)
-        self.update()
-    def AddNode(self,node):
-        [id,type,point]=node
-        self.nodes[id]=[type,point]
-    def AddRoad(self,road):
-        [key,value]=road
-        self.roads[key]=value
-
-    def AddTraj(self,name,path):
-        self.paths[name]=path
-
-
-    def drawPath(self,poses,width,color):
-        glColor3f(color[0],color[1],color[2])
+    nodes = {}
+    roads = {}
+
+    robot1_ = None
+    robot2_ = None
+    robotMain_ = None
+    def SetRobot1Instance(self, robot):
+        self.robot1_ = robot
+    def SetRobot2Instance(self, robot):
+        self.robot2_ = robot
+
+    def SetRobotMainInstance(self, robot):
+        self.robotMain_ = robot
+
+    def AddNode(self, node):
+        [id, type, point] = node
+        self.nodes[id] = [type, point]
+
+    def AddRoad(self, road):
+        [key, value] = road
+        self.roads[key] = value
+
+    def drawTraj(self, poses, width, color):
+        glColor3f(color[0], color[1], color[2])
         glLineWidth(2)
         glBegin(GL_LINES)
         for pose in poses:
-            [x,y,yaw]=pose
-            point1=[x,y]
+            [x, y, yaw] = pose
+            point1 = [x, y]
 
-            [dx,dy]=self.RotatePoint([-width/2,-width/2],yaw)
-            point2=[x+dx,y+dy]
+            [dx, dy] = self.RotatePoint([-width / 2, -width / 2], yaw)
+            point2 = [x + dx, y + dy]
 
-            [dx,dy]=self.RotatePoint([-width/2,width/2],yaw)
-            point3=[x+dx,y+dy]
+            [dx, dy] = self.RotatePoint([-width / 2, width / 2], yaw)
+            point3 = [x + dx, y + dy]
 
-            glVertex2d(point1[0],point1[1])
-            glVertex2d(point3[0],point3[1])
+            glVertex2d(point1[0], point1[1])
+            glVertex2d(point3[0], point3[1])
 
-            glVertex2d(point1[0],point1[1])
-            glVertex2d(point2[0],point2[1])
+            glVertex2d(point1[0], point1[1])
+            glVertex2d(point2[0], point2[1])
         glEnd()
 
-    def drawEdge(self,pt1,pt2,width,color):
+    def drawTrajWithPoint(self, poses, ptsize, color):
+        glPointSize(ptsize)
+        glDepthMask(GL_FALSE)
+        glBegin(GL_POINTS)
+        glColor3f(color[0], color[1], color[2])
+        for pt in poses:
+            [x, y, _] = pt
+            glVertex2d(x, y)
+        glEnd()
 
-        glColor4f(color[0],color[1],color[2],0)
+    def drawEdge(self, pt1, pt2, width, color):
+
+        glColor4f(color[0], color[1], color[2], 0)
         glLineWidth(width)
         glBegin(GL_LINES)
-        glVertex2d(pt1[0],pt1[1])
-        glVertex2d(pt2[0],pt2[1])
+        glVertex2d(pt1[0], pt1[1])
+        glVertex2d(pt2[0], pt2[1])
         glEnd()
 
     @staticmethod
-    def RotatePoint(point,yaw):
+    def RotatePoint(point, yaw):
+        [x, y] = point
+        nx = math.cos(yaw) * x - y * math.sin(yaw)
+        ny = x * math.sin(yaw) + y * math.cos(yaw)
+        return [nx, ny]
+    def Transform(self,point,yaw,dt):
         [x,y]=point
-        nx=math.cos(yaw)*x-y*math.sin(yaw)
-        ny=x*math.sin(yaw)+y*math.cos(yaw)
-        return [nx,ny]
-    def drawAxis(self,pose,len,width):
-        [x,y,yaw]=pose
-        x_source=[x+len*math.cos(yaw),y+math.sin(yaw)*len]
-        y_source=[x-math.sin(yaw)*len,y+len*math.cos(yaw)]
+        [dx,dy]=dt
+        nx = math.cos(yaw) * x - y * math.sin(yaw)+dx
+        ny = x * math.sin(yaw) + y * math.cos(yaw)+dy
+        return [nx, ny]
+
+    def drawAxis(self, pose, len, width):
+        [x, y, yaw] = pose
+        x_source = [x + len * math.cos(yaw), y + math.sin(yaw) * len]
+        y_source = [x - math.sin(yaw) * len, y + len * math.cos(yaw)]
 
         glDepthMask(GL_FALSE)
-        glColor3f(1,0,0)
+        glColor3f(1, 0, 0)
 
         glLineWidth(width)
         glBegin(GL_LINES)
-        glVertex2d(x,y)
-        glVertex2d(x_source[0],x_source[1])
+        glVertex2d(x, y)
+        glVertex2d(x_source[0], x_source[1])
         glEnd()
 
-        glColor3f(0,1,0)
+        glColor3f(0, 1, 0)
         glBegin(GL_LINES)
-        glVertex2d(x,y)
-        glVertex2d(y_source[0],y_source[1])
+        glVertex2d(x, y)
+        glVertex2d(y_source[0], y_source[1])
         glEnd()
-    def drawNode(self,node,size,color):
-        [x,y]=node
+    def DrawAGV(self,pose,rgb):
+        [x,y,yaw]=pose
+        l=0.8        #轮长
+        L=1.3       #轴距
+        W=2.5       #宽
+        pt1=self.Transform([-(L+l)/2,W/2],yaw,[x,y])
+        pt2=self.Transform([-(L-l)/2,W/2],yaw,[x,y])
+        pt3=self.Transform([(L-l)/2,W/2],yaw,[x,y])
+        pt4=self.Transform([(L+l)/2,W/2],yaw,[x,y])
+
+        pt5=self.Transform([-(L+l)/2,-W/2],yaw,[x,y])
+        pt6=self.Transform([-(L-l)/2,-W/2],yaw,[x,y])
+        pt7=self.Transform([(L-l)/2,-W/2],yaw,[x,y])
+        pt8=self.Transform([(L+l)/2,-W/2],yaw,[x,y])
+
+        glDepthMask(GL_FALSE)
+        glColor3f(rgb[0],rgb[1],rgb[2])
+
+        glLineWidth(20)
+        glBegin(GL_LINES)
+        glVertex2d(pt1[0],pt1[1])
+        glVertex2d(pt2[0],pt2[1])
+        glVertex2d(pt3[0],pt3[1])
+        glVertex2d(pt4[0],pt4[1])
+        glVertex2d(pt5[0],pt5[1])
+        glVertex2d(pt6[0],pt6[1])
+        glVertex2d(pt7[0],pt7[1])
+        glVertex2d(pt8[0],pt8[1])
+        glEnd()
+
+        glLineWidth(5)
+        glBegin(GL_LINES)
+        glVertex2d(pt2[0],pt2[1])
+        glVertex2d(pt6[0],pt6[1])
+        glVertex2d(pt3[0],pt3[1])
+        glVertex2d(pt7[0],pt7[1])
+
+        glVertex2d(pt2[0],pt2[1])
+        glVertex2d(pt3[0],pt3[1])
+        glVertex2d(pt6[0],pt6[1])
+        glVertex2d(pt7[0],pt7[1])
+        glEnd()
+        #绘制方向
+        self.drawAxis(pose,1,5)
+
+    def drawNode(self, node, size, color):
+        [x, y] = node
         glPointSize(size)
         glDepthMask(GL_FALSE)
         glBegin(GL_POINTS)
-        glColor4f(color[0],color[1],color[2],0.5)
-        glVertex2d(x,y)
+        glColor4f(color[0], color[1], color[2], 0.5)
+        glVertex2d(x, y)
         glEnd()
         '''glColor3f(1,1,1);
         #glBegin(GL_TEXTURE)
         glText'''
-
+    def DrawRobotData(self,robot):
+        if robot is not None:
+            if robot.currentNavPath_ is not None:
+                for traj in robot.currentNavPath_:
+                    self.drawTraj(traj, 0.5, robot.pathColor_)
+
+        # 绘制agv
+        if robot is not None:
+            if robot.timedPose_.timeout() == False:
+                agv_statu = robot.timedPose_.statu
+                x = agv_statu.x
+                y = agv_statu.y
+                theta = agv_statu.theta
+                self.DrawAGV([x, y, theta], robot.robotColor_)
+
+            if robot.timedNavStatu_.timeout() == False:
+                select_traj = robot.MpcSelectTraj()
+                predict_traj = robot.MpcPredictTraj()
+                self.drawTrajWithPoint(select_traj, 8, [1, 0, 1])
+                self.drawTrajWithPoint(predict_traj, 10, [0, 1, 1])
     def paintGL(self):
         PyGLWidget.paintGL(self)
+        self.drawAxis([0, 0, 0], 5, 5)
 
-        self.drawAxis([0,0,0],5,5)
+        #绘制地图
         for node in self.nodes.items():
-            [_,[type,point]]=node
-            if type=="street_node":
-                self.drawNode(point,8,[0,0,1])
-            if type=="space_node":
-                self.drawNode(point,8,[1,1,0])
+            [_, [type, point]] = node
+            if type == "street_node":
+                self.drawNode(point, 8, [0, 0, 1])
+            if type == "space_node":
+                self.drawNode(point, 8, [1, 1, 0])
         for road in self.roads.items():
-            [_,value]=road
+            [_, value] = road
             for nodeId in value:
                 for nextId in value:
-                    if not nodeId==nextId:
-                        [_,point1]=self.nodes[nodeId]
-                        [_,point2]=self.nodes[nextId]
-                        self.drawEdge(point1,point2,3.,[0,0.5,0.5])
-
-        for [id,path] in self.paths.items():
-
-            rgb=self.colors.get(id)
-            if rgb is not None:
-                for traj in path:
-                    self.drawPath(traj,0.5,rgb)
-
-        #绘制agv
-        if self.timedAgv1.timeout()==False:
-            x=self.timedAgv1.statu.x
-            y=self.timedAgv1.statu.y
-            theta=self.timedAgv1.statu.theta
-            self.drawAxis([x,y,theta],3,5)
+                    if not nodeId == nextId:
+                        [_, point1] = self.nodes[nodeId]
+                        [_, point2] = self.nodes[nextId]
+                        self.drawEdge(point1, point2, 3., [0, 0.5, 0.5])
+
+        #绘制agv相关数据，路径、轨迹、位姿
+        self.DrawRobotData(self.robot1_)
+        self.DrawRobotData(self.robot2_)
+        self.DrawRobotData(self.robotMain_)
+

RobotData.py

@@ -0,0 +1,128 @@
+
+
+import math
+import random
+import time
+import message_pb2 as message
+from mqtt_async import MqttAsync
+import google.protobuf.json_format as jtf
+from dijkstra.Map import DijikstraMap
+class TimeStatu:
+    def __init__(self,statu=None,timeout=3):
+        self.statu=statu
+        self.time=time.time()
+        self.timeout_ms=timeout
+
+    def timeout(self):
+        tm=time.time()
+        return tm-self.time>self.timeout_ms or self.statu==None
+
+class Robot(MqttAsync):
+    def __init__(self):
+        self.timedPose_=TimeStatu(message.AGVStatu,0)
+        self.timedNavStatu_=TimeStatu(message.NavStatu,0)
+        self.dataTopic_={}
+        self.messageArrivedSignal=None
+        self.currentNavData_=None
+        self.navCmdTopic_=None
+        self.currentNavPathNodes_=None
+        self.currentNavPath_=None
+
+        self.pathColor_=[1,1,0]
+        self.robotColor_=[0.7,0.2,0.3]
+    def SetSubDataTopic(self,match:dict,messageSignal=None):
+        self.dataTopic_=match
+        self.messageArrivedSignal=messageSignal
+        for item in match.items():
+            [topic,_]=item
+            self.subscribe(topic,self.on_message)
+    def SetCmdTopic(self,topic):
+        self.navCmdTopic_=topic
+    def SetColor(self,pathColor,robotColor):
+        self.pathColor_=pathColor
+        self.robotColor_=robotColor
+    def ResetPose(self,agv : message.AGVStatu):
+        self.timedPose_=TimeStatu(agv,0.5)
+    def ResetNavStatu(self,statu:message.NavStatu):
+        self.timedNavStatu_=TimeStatu(statu,0.5)
+    def on_message(self,client, userdata, msg):
+        topic=msg.topic
+        if self.dataTopic_.get(topic) is not None:
+            dtype=self.dataTopic_[topic]
+            if dtype==message.AGVStatu.__name__:
+                proto=message.AGVStatu()
+                jtf.Parse(msg.payload.decode(),proto)
+                self.ResetPose(proto)
+            if dtype==message.NavStatu.__name__:
+                self.last_running=self.IsNavigating()
+                proto=message.NavStatu()
+                jtf.Parse(msg.payload.decode(),proto)
+                self.ResetNavStatu(proto)
+                if self.last_running==True and self.IsNavigating()==False:
+                    self.currentNavPathNodes_=None
+                    self.currentNavPath_=None
+            if self.messageArrivedSignal is not None:
+                self.messageArrivedSignal()
+
+    def MpcSelectTraj(self):
+        traj=[]
+        if self.timedNavStatu_.timeout()==False:
+            nav=self.timedNavStatu_.statu
+            for pose2d in nav.selected_traj.poses:
+                traj.append([pose2d.x,pose2d.y,pose2d.theta])
+        return traj
+
+    def MpcPredictTraj(self):
+        traj=[]
+        if self.timedNavStatu_.timeout()==False:
+            nav=self.timedNavStatu_.statu
+            for pose2d in nav.predict_traj.poses:
+                traj.append([pose2d.x,pose2d.y,pose2d.theta])
+        return traj
+
+    def Connected(self):
+        return self.timedNavStatu_.timeout()==False and self.timedPose_.timeout()==False
+    def IsNavigating(self):
+        if self.timedNavStatu_.timeout()==False:
+            key=self.timedNavStatu_.statu.key
+            if key=="" or key==None:
+                return False
+        return True
+    def GeneratePath(self,begID,endID):
+        self.currentNavPathNodes_=DijikstraMap().GetShortestPath(begID,endID)
+        self.currentNavPath_=DijikstraMap().CreatePath(self.currentNavPathNodes_,0.5)
+    def ExecNavtask(self):
+        if self.navCmdTopic_ == None:
+            print("Robot has not set nav cmd topic")
+            return
+        if self.IsNavigating():
+            print(" robot is navigating ,key:%s"%(self.timedNavStatu_.statu.key))
+            return
+
+        cmd=None
+        if self.currentNavPathNodes_ is not None and self.timedPose_.timeout()==False:
+            statu=self.timedPose_.statu
+            if statu is not None:
+                cmd=DijikstraMap().CreateNavCmd([statu.x,statu.y,statu.theta],self.currentNavPathNodes_)
+        else:
+            print("current path is none")
+            return
+
+        if cmd is None:
+            print("Nav cmd is None")
+            return
+
+        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+        while self.IsNavigating()==False:
+            time.sleep(1)
+            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+
+    def CancelNavTask(self):
+        cmd=message.NavCmd()
+        cmd.action=3
+        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+
+
+
+
+

dijkstra/Map.py

@@ -1,7 +1,7 @@
 import math
 from dijkstra.dijkstra_algorithm import Graph
 import message_pb2 as message
-
+import uuid
 
 class Node(object):
     def __init__(self, id, x, y):
@@ -44,7 +44,7 @@ map 采用单例
 
 
 @singleton
-class Map(object):
+class DijikstraMap(object):
     def __init__(self):
         self.nodes_ = {}
         self.graph_ = Graph()
@@ -82,13 +82,50 @@ class Map(object):
             path.append(node)
         return path
 
-    def CreateNavCmd(self, pose, path):
+    @staticmethod
+    def CreatePath(pathNodes,delta):
+        last_node=None
+        trajectry=[]
+        for node in pathNodes:
+            if last_node==None:
+                last_node=node
+                continue
+            dis=last_node.distance(node)
+            if dis<0.5:
+                last_node=node
+                continue    #同一点
+            else:
+                vector=[node.x_-last_node.x_,node.y_-last_node.y_]
+
+                dx=vector[0]
+                dy=vector[1]
+                yaw=math.asin(dy/math.sqrt(dx*dx+dy*dy))
+                if yaw>=0:
+                    if dx<0:
+                        yaw=math.pi-yaw
+                if yaw<0:
+                    if dx<0:
+                        yaw=-math.pi-yaw
+                len=int(math.sqrt(dx*dx+dy*dy)/delta)
+                ax=math.cos(yaw)*delta
+                ay=math.sin(yaw)*delta
+                poses=[]
+                if isinstance(last_node,(SpaceNode)):
+                    yaw=yaw+math.pi
+                for i in range(len+1):
+                    pose=[last_node.x_+i*ax,last_node.y_+i*ay,yaw]
+                    poses.append(pose)
+                trajectry.append(poses)
+                last_node=node
+        return trajectry
+    @staticmethod
+    def CreateNavCmd(pose, path):
         if len(path) <= 1:
             return None
 
         cmd = message.NavCmd()
         cmd.action=0  # 新导航
-        key = "12354-ahsbd-kkkk"
+        key = str(uuid.uuid4())
         cmd.key=(key)
         adjustdiff = message.Pose2d()
         node_mpcdiff = message.Pose2d()

mqtt_async.py

@@ -1,11 +1,10 @@
 from paho.mqtt import client as mqtt_client
 
-
 class MqttAsync(object):
-    def __init__(self,client_id):
-        self.client_id=client_id
+    def __init__(self):
         self.connected=False
-    def connect(self,ip,port,user,password):
+    def connect(self,client_id,ip,port,user,password):
+        self.client_id=client_id
         def on_connect(client, userdata, flags, rc):
             if rc == 0:
                 print("Connected to MQTT Broker!")
@@ -30,8 +29,6 @@ class MqttAsync(object):
 
 
     def subscribe(self,topic,callback):
-        def on_message(client, userdata, msg):
-            pass
         self.client.subscribe(topic)
         self.client.on_message = callback
 

test.py

@@ -38,140 +38,63 @@ import sys
 from PyQt5.QtGui import *
 from PyQt5.QtWidgets import *
 from PyQt5.QtCore import *
-from MapGLWidget import MapGLWidget,TimeStatu
+from MapGLWidget import MapGLWidget
 import json
 import dijkstra.Map as mp
 import ControllWidget
-import mqtt_async as mqtt
+import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
+import uuid
 #===============================================================================
 
 
 class MainWindow(QMainWindow):
     """docstring for Mainwindow"""
+    djks_map_=mp.DijikstraMap()
+    ui_nodes=[]
 
     def __init__(self, parent = None):
         super(MainWindow,self).__init__(parent)
         self.basic()
 
+        self.LoadMap()
+        self.Controller1 = ControllWidget.ControlFrame(["AGV1",self.ui_nodes])
+        self.Controller2 = ControllWidget.ControlFrame(["AGV2",self.ui_nodes])
+        self.ControllerMain = ControllWidget.ControlFrame(["AGVMain",self.ui_nodes])
 
         splitter_main = self.split_()
         self.setCentralWidget(splitter_main)
 
-        self.path=None
-
-        self.agv1_statu=TimeStatu(None,0.1)
-        self.agv2_statu=TimeStatu(None,0.1)
-        self.mqtt_=mqtt.MqttAsync("UI")
-        self.mqtt_.connect("127.0.0.1",1883,"admin","zx123456")
-        self.mqtt_.subscribe("agv1_child/agv_statu",self.agv_statu_arrived)
-        self.mqtt_.loop_start()
-
-
-
-    def agv_statu_arrived(self,client,user_data,msg):
-        statu=message.AGVStatu()
-        jtf.Parse(msg.payload.decode(),statu)
-        self.gl.ResetAgv1(statu)
-        self.agv1_statu=TimeStatu(statu,0.3)
-
-
-    def load_map(self,file):
-        with open(file,'r',encoding='utf-8') as fp:
-            map=json.load(fp)
-            return map
-
-
-    #窗口基础属性
-    def basic(self):
-        #设置标题，大小，图标
-        self.setWindowTitle("GT")
-        self.resize(1100,650)
-        self.setWindowIcon(QIcon("./image/Gt.png"))
-        #居中显示
-        screen = QDesktopWidget().geometry()
-        self_size = self.geometry()
-        self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))
-
-    def closeEvent(self, QCloseEvent):
-        self.gl.close()
-
-
-    @staticmethod
-    def Create_traj(path,delta):
-        last_node=None
-        trajectry=[]
-        for node in path:
-            if last_node==None:
-                last_node=node
-                continue
-            dis=last_node.distance(node)
-            if dis<0.5:
-                last_node=node
-                continue    #同一点
-            else:
-                vector=[node.x_-last_node.x_,node.y_-last_node.y_]
-
-                dx=vector[0]
-                dy=vector[1]
-                yaw=math.asin(dy/math.sqrt(dx*dx+dy*dy))
-                if yaw>=0:
-                    if dx<0:
-                        yaw=math.pi-yaw
-                if yaw<0:
-                    if dx<0:
-                        yaw=-math.pi-yaw
-                len=int(math.sqrt(dx*dx+dy*dy)/delta)
-                ax=math.cos(yaw)*delta
-                ay=math.sin(yaw)*delta
-                poses=[]
-                if isinstance(last_node,(mp.SpaceNode)):
-                    yaw=yaw+math.pi
-                for i in range(len+1):
-                    pose=[last_node.x_+i*ax,last_node.y_+i*ay,yaw]
-                    poses.append(pose)
-                trajectry.append(poses)
-                last_node=node
-        return trajectry
-
-    def generater_path(self,beg,end):
-        self.path=self.djks_map_.GetShortestPath(beg,end)
-        self.traj=self.Create_traj(self.path,0.5)
-        self.gl.AddTraj("agv1",self.traj)
-        self.gl.update()
-
-    def send_cmd(self):
-        if self.path is not None and self.agv1_statu.timeout()==False:
-            statu=self.agv1_statu.statu
-            if statu is not None:
-                cmd=self.djks_map_.CreateNavCmd([statu.x,statu.y,statu.theta],self.path)
-                if cmd is not None:
-                    self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))
-                else:
-                    print(" NavCmd is None")
-
-    def Cancel_cmd(self):
-        cmd=message.NavCmd()
-        cmd.action=3
-        self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))
-
-
-    #分割窗口
-    def split_(self):
-        splitter = QSplitter(Qt.Vertical)
+        self.gl.SetRobot1Instance(self.Controller1.robot_)
+        self.gl.SetRobot2Instance(self.Controller2.robot_)
+
+        self.Controller1.robot_.connect("pyUI_r1","127.0.0.1",1883,"admin","zx123456")
+        dataTopics={"agv1_child/agv_statu":message.AGVStatu.__name__,
+                    "agv1_child/nav_statu":message.NavStatu.__name__}
+        self.Controller1.robot_.SetSubDataTopic(dataTopics,self.messageArrived)
+        self.Controller1.robot_.SetCmdTopic("agv1_child/nav_cmd")
+        self.Controller1.robot_.loop_start()
+        self.Controller1.robot_.SetColor([0.7,0.2,0.3],[0.7,0.2,0.3])
+
+        self.Controller2.robot_.connect("pyUI_r2","127.0.0.1",1883,"admin","zx123456")
+        dataTopics={"agv1/agv_statu":message.AGVStatu.__name__,
+                    "agv1/nav_statu":message.NavStatu.__name__}
+        self.Controller2.robot_.SetSubDataTopic(dataTopics,self.messageArrived)
+        self.Controller2.robot_.SetCmdTopic("agv1/nav_cmd")
+        self.Controller2.robot_.loop_start()
+        self.Controller2.robot_.SetColor([0.4,0.2,0.8],[0.4,0.2,0.8])
+    def LoadMap(self):
         self.gl = MapGLWidget()   #将opengl例子嵌入GUI
         map=self.load_map("./map.json")
-        UI_data={}
-        UI_data["nodes"]=[]
-        self.djks_map_=mp.Map()
+
 
         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])
-            UI_data["nodes"].append(id)
+            self.ui_nodes.append(id)
         for node in map['space_nodes'].items():
             [id,point]=node
             [x,y,connectId]=point
@@ -181,7 +104,7 @@ class MainWindow(QMainWindow):
             glNode=[id,"space_node",[x,y]]
             self.gl.AddNode(glNode)
             self.gl.AddRoad(["conn%s-%s"%(id,connectId),[id,connectId]])
-            UI_data["nodes"].append(id)
+            self.ui_nodes.append(id)
 
         for road in map['roads'].items():
             self.gl.AddRoad(road)
@@ -192,28 +115,48 @@ class MainWindow(QMainWindow):
                     if not pt1==pt2:
                         self.djks_map_.AddEdge(pt1,pt2)
 
-        splitter.addWidget(self.gl)
+    def load_map(self,file):
+        with open(file,'r',encoding='utf-8') as fp:
+            map=json.load(fp)
+            return map
 
-        testedit = QTextEdit()
-        splitter.addWidget(testedit)
-        splitter.setStretchFactor(0,3)
-        splitter.setStretchFactor(1,2)
-        splitter_main = QSplitter(Qt.Horizontal)
-        Controller = ControllWidget.Frame(UI_data)
+    def messageArrived(self):
+        self.gl.update()
 
-        callbacks={}
-        callbacks["CreateTraj"]=self.generater_path
-        callbacks["SendCmd"]=self.send_cmd
-        callbacks["CancelCmd"]=self.Cancel_cmd
+    #窗口基础属性
+    def basic(self):
+        #设置标题，大小，图标
+        self.setWindowTitle("GT")
+        self.resize(1100,650)
+        self.setWindowIcon(QIcon("./image/Gt.png"))
+        #居中显示
+        screen = QDesktopWidget().geometry()
+        self_size = self.geometry()
+        self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))
 
+    def closeEvent(self, QCloseEvent):
+        self.gl.close()
 
-        Controller.SetBtnFunc(callbacks)
-        splitter_main.addWidget(Controller)
+    #分割窗口
+    def split_(self):
+        splitter_main = QSplitter(Qt.Horizontal)
+        splitter = QSplitter(Qt.Vertical)
 
+        splitter.addWidget(self.Controller1)
+        splitter.addWidget(self.Controller2)
+        splitter.addWidget(self.ControllerMain)
+        splitter.setStretchFactor(0,1)
+        splitter.setStretchFactor(1,1)
+        splitter.setStretchFactor(2,1)
 
         splitter_main.addWidget(splitter)
+        splitter_main.addWidget(self.gl)
+
         splitter_main.setStretchFactor(0,1)
-        splitter_main.setStretchFactor(1,4)
+        splitter_main.setStretchFactor(2,4)
+
+
+
         return splitter_main
 
 if __name__ == "__main__":