cpu包含单节cpu和双节cpu

ControllWidget.py

@@ -16,7 +16,7 @@ class ControlFrame(QFrame):
         self.setFrameShape(self.StyledPanel)
         self.timer_=QTimer()
         self.timer_.timeout.connect(self.Update)
-        #self.timer_.start(100)
+        self.timer_.start(100)
 
 
     def InitUI(self):
@@ -66,20 +66,21 @@ class ControlFrame(QFrame):
         self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
 
         self.btnModelCheck=QCheckBox("双车模式",self)
-        self.btnModelCheck.setGeometry(360, 150, 100, 40)
+        self.btnModelCheck.setGeometry(260, 150, 100, 40)
         self.btnModelCheck.stateChanged.connect(self.MainAgvchangecb)
 
+
     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 : %.3f\ny : %.3f\nΘ : %.2f"%(x,y,yaw*180/math.pi))
-            if self.robot_.IsNavigating():
-                djks_map=mp.DijikstraMap()
-                [label,pt]=djks_map.findNeastNode([x,y])
-                if self.nodes_.index(label)>=0:
-                    self.begQc.setCurrentText(label)
+
+            '''djks_map=mp.DijikstraMap()
+            [label,pt]=djks_map.findNeastNode([x,y])
+            if self.nodes_.index(label)>=0:
+                self.begQc.setCurrentText(label)'''
 
         statu="min-width: 32px; min-height: 32px;max-width:32px;\
                 max-height: 32px;border-radius: 16px;  border:1px solid black;background:green"
@@ -90,17 +91,24 @@ class ControlFrame(QFrame):
         elif self.robot_.IsNavigating()==True:
             statu="min-width: 32px; min-height: 32px;max-width:32px;\
                 max-height: 32px;border-radius: 16px; border:1px solid black;background:yellow"
-            #self.endQc.setEnabled(False)
+            self.endQc.setEnabled(False)
+            self.btnModelCheck.setEnabled(False)
         else:
             self.endQc.setEnabled(True)
+            self.btnModelCheck.setEnabled(True)
         self.LedLabel.setStyleSheet(statu)
 
+
+        self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
+        self.WheelBaseEdit.setVisible(self.robot_.IsMainAgv())
+        self.btnModelCheck.setVisible(self.robot_.IsMainAgv())
+
     def MainAgvchangecb(self):
         if self.btnModelCheck.checkState() == Qt.Checked:
             leng=float(self.WheelBaseEdit.text())
-            self.robot_.SwitchMoveMode(2,[-leng/2.0,0,0])
+            self.robot_.SwitchMoveMode(2,leng)
         if self.btnModelCheck.checkState() == Qt.Unchecked:
-            self.robot_.SwitchMoveMode(1,[0,0,0])
+            self.robot_.SwitchMoveMode(1,0)
     def endQCChanged(self):
         id1=self.begQc.currentText()
         id2=self.endQc.currentText()

MapGLWidget.py

@@ -14,7 +14,6 @@ class MapGLWidget(PyGLWidget):
 
     robot1_ = None
     robot2_ = None
-    robotMain_ = None
     def __init__(self):
         PyGLWidget.__init__(self)
         GLUT.glutInit()
@@ -23,9 +22,6 @@ class MapGLWidget(PyGLWidget):
     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]
@@ -122,11 +118,16 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(y_source[0], y_source[1])
         glEnd()
 
-    def DrawMainAGV(self,pose,rgb,label):
-        [x,y,yaw]=pose
-        l=0.8        #轮长
+    def DrawMainAGV(self,pose,rgb,label,running=False):
+        l=0.8       #轮长
         L=1.3       #轴距
-        W=2.5       #宽
+        W=2.5
+        if running:
+            l=1.6       #轮长
+            L=3       #轴距
+            W=2.5
+        [x,y,yaw]=pose
+            #宽
         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])
@@ -152,7 +153,7 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(pt8[0],pt8[1])
         glEnd()
 
-        glLineWidth(3)
+        glLineWidth(2)
         glBegin(GL_LINES)
         glVertex2d(pt2[0],pt2[1])
         glVertex2d(pt6[0],pt6[1])
@@ -166,8 +167,7 @@ class MapGLWidget(PyGLWidget):
         glEnd()
         #绘制方向
         self.drawAxis(pose,1.2,5)
-        color=[0,0,0]
-        self.DrawText([x-W/2,y-l/2],label,5,1.5,color)
+        self.DrawText([x-W/2,y-l/2],label,5,1.5,rgb)
     def DrawAGV(self,pose,rgb,label=""):
         [x,y,yaw]=pose
         l=0.8        #轮长
@@ -186,7 +186,7 @@ class MapGLWidget(PyGLWidget):
         glDepthMask(GL_FALSE)
         glColor3f(rgb[0],rgb[1],rgb[2])
 
-        glLineWidth(5)
+        glLineWidth(3)
         glBegin(GL_LINES)
         glVertex2d(pt1[0],pt1[1])
         glVertex2d(pt2[0],pt2[1])
@@ -225,9 +225,7 @@ class MapGLWidget(PyGLWidget):
         glEnd()
         l=len(label)
         self.DrawText([x-l/8,y],label,size/10,1,[1,1,1])
-        '''glColor3f(1,1,1);
-        #glBegin(GL_TEXTURE)
-        glText'''
+
     def DrawRobotData(self,robot):
         if robot is not None:
             if robot.currentNavPath_ is not None:
@@ -242,7 +240,10 @@ class MapGLWidget(PyGLWidget):
                 y = agv_statu.y
                 theta = agv_statu.theta
                 if robot.IsMainAgv():
-                    self.DrawMainAGV([x, y, theta], robot.Color(),robot.name_)
+                    if robot.IsMainModeStatu():
+                        self.DrawMainAGV([x, y, theta], robot.Color(),robot.name_,True)
+                    else:
+                        self.DrawMainAGV([x, y, theta], robot.Color(),robot.name_)
                 else:
                     self.DrawAGV([x, y, theta], robot.Color(),robot.name_)
 
@@ -255,9 +256,8 @@ class MapGLWidget(PyGLWidget):
     def paintGL(self):
         PyGLWidget.paintGL(self)
         self.drawAxis([0, 0, 0], 3, 2)
-
         #绘制地图
-        '''for road in self.roads.items():
+        for road in self.roads.items():
             [_, value] = road
             for nodeId in value:
                 for nextId in value:
@@ -270,7 +270,7 @@ class MapGLWidget(PyGLWidget):
             if type == "street_node":
                 self.drawNode(point, 30, [0, 0.5, 0.5],name)
             if type == "space_node":
-                self.drawNode(point, 20, [0.3, 0.8, 0.7],name)'''
+                self.drawNode(point, 20, [0.3, 0.8, 0.7],name)
 
         #绘制agv相关数据，路径、轨迹、位姿
         self.DrawRobotData(self.robot1_)

RobotData.py

@@ -30,6 +30,10 @@ class Robot(MqttAsync):
         self.robotColor_=[0.7,0.2,0.3]      #agv当前颜色
         self.name_=name
 
+        self.l_=0.8       #轮长
+        self.L_=1.3       #轴距
+        self.W_=2.5       #宽
+
     def Color(self):
         if self.IsMainModeStatu():
             return [0,0,0]
@@ -132,13 +136,11 @@ class Robot(MqttAsync):
         '''while self.IsNavigating()==False:
             time.sleep(1)
             self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))'''
-    def SwitchMoveMode(self,mode,transform):
+    def SwitchMoveMode(self,mode,wheelbase):
 
         cmd =message.NavCmd()
         if mode==2:
-            trans=message.Pose2d()
-            trans.x,trans.y,trans.theta=transform
-            cmd.transform.CopyFrom(trans)
+            cmd.wheelbase=wheelbase
             cmd.action=4
         if mode==1:
             cmd.action=5

map.json

@@ -9,8 +9,8 @@
     "N3": [15.4,3.5],
     "N4":[17.9,3.5],
     "N5":[10.3,17.7],
-    "N6":[14.85,17.7],
-    "N7":[17.9,17.7],
+    "N6":[15.3,17.7],
+    "N7":[18.3,17.7],
     "N8":[10.3,-0.18],
     "N11":[18.8,3.5]
     
@@ -20,9 +20,9 @@
   	"A1":[15.4,9,"N3"],
   	"A":[15.4,7.5,"N3"],
   	"A2":[15.4,6,"N3"],
-  	"B1":[14.85,12.2,"N6"],
-  	"B":[14.85,13.7,"N6"],
-  	"B2":[14.85,15.2,"N6"]
+  	"B1":[15.3,12.2,"N6"],
+  	"B":[15.3,13.7,"N6"],
+  	"B2":[15.3,15.2,"N6"]
   },
   "roads":{
     "road1":["O1","N1","N2","N3","N4","N11"],

message.proto

@@ -19,7 +19,8 @@ message Speed {
   int32 T=3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
   float V=4;  //角速度
   float W=5;  //线速度
-  int32 end=6;
+  float L=6;  //轴距
+  int32 end=7;
 }
 
 message SpeedLimit
@@ -51,15 +52,13 @@ message Action
   SpeedLimit horize_limit=7;
 }
 
-
 message NavCmd
 {
   int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式
   string key=2;
-  Pose2d transform=3;		//整车中心与单节的变换
+  float wheelbase=3;		//轴距
   repeated Action actions=4;
 }
-
 message NavStatu
 {
   bool statu = 1; //0:ready  1:running

test.py

@@ -61,13 +61,12 @@ class MainWindow(QMainWindow):
         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.Controller1.robot_.connect("pyUI_r1","127.0.0.1",1883,"admin","zx123456")
+        self.Controller1.robot_.connect("pyUI_r1","192.168.0.70",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)
@@ -76,7 +75,7 @@ class MainWindow(QMainWindow):
         self.Controller1.robot_.SetColor([0.7,0.2,0.3],[0.7,0.2,0.3])
         self.gl.SetRobot1Instance(self.Controller1.robot_)
 
-        self.Controller2.robot_.connect("pyUI_r2","127.0.0.1",1883,"admin","zx123456")
+        self.Controller2.robot_.connect("pyUI_r2","192.168.0.70",1883,"admin","zx123456")
         dataTopics={"agv1/agv_statu":message.AGVStatu.__name__,
                     "agv1/nav_statu":message.NavStatu.__name__}
         self.Controller2.robot_.SetSubDataTopic(dataTopics,self.messageArrived)
@@ -85,14 +84,6 @@ class MainWindow(QMainWindow):
         self.Controller2.robot_.SetColor([0.4,0.2,0.8],[0.4,0.2,0.8])
         self.gl.SetRobot2Instance(self.Controller2.robot_)
 
-        '''self.ControllerMain.robot_.connect("pyUI_main","127.0.0.1",1883,"admin","zx123456")
-        dataTopics={"agv1_main/agv_statu":message.AGVStatu.__name__,
-                    "agv1_main/nav_statu":message.NavStatu.__name__}
-        self.ControllerMain.robot_.SetSubDataTopic(dataTopics,self.messageArrived)
-        self.ControllerMain.robot_.SetCmdTopic("agv1_child/nav_cmd")
-        self.ControllerMain.robot_.loop_start()
-        self.ControllerMain.robot_.SetColor([0,0,0],[0,0,0])
-        self.gl.SetRobotMainInstance(self.ControllerMain.robot_)'''
     def LoadMap(self):
         self.gl = MapGLWidget()   #将opengl例子嵌入GUI
         map=self.load_map("./map.json")
@@ -153,10 +144,9 @@ class MainWindow(QMainWindow):
 
         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)