添加主从cpu

ControllWidget.py

@@ -1,8 +1,8 @@
 import math
 
-from PyQt5.QtWidgets import QWidget,QFrame,QLabel,QLineEdit,QPushButton,QComboBox
+from PyQt5.QtWidgets import QWidget,QFrame,QLabel,QLineEdit,QPushButton,QComboBox,QCheckBox
 from PyQt5.QtGui import QPixmap,QPainter,QResizeEvent,QCloseEvent,QPaintEvent,QFont
-from PyQt5.QtCore import QSize,QTimer
+from PyQt5.QtCore import QSize,QTimer,Qt
 from RobotData import Robot
 import dijkstra.Map as mp
 class ControlFrame(QFrame):
@@ -16,7 +16,8 @@ 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):
 
@@ -54,7 +55,19 @@ class ControlFrame(QFrame):
         self.posestatic.setGeometry(260, 5, 100, 90)
 
         self.LedLabel=QLabel(self)
-        self.LedLabel.setGeometry(280, 120, 50, 50)
+        self.LedLabel.setGeometry(80, 120, 50, 50)
+
+        #发送轴距
+        self.wheelbasestatic=QLabel(self)
+        self.wheelbasestatic.setText("轴距:")
+        self.wheelbasestatic.setGeometry(260, 100, 40, 30)
+        self.WheelBaseEdit=QLineEdit(self)
+        self.WheelBaseEdit.setText("3.0")
+        self.WheelBaseEdit.setGeometry(300, 100, 50, 30)
+
+        self.btnModelCheck=QCheckBox("双车模式",self)
+        self.btnModelCheck.setGeometry(360, 150, 100, 40)
+        self.btnModelCheck.stateChanged.connect(self.MainAgvchangecb)
 
     def Update(self):
         if self.robot_.timedPose_.timeout()==False:
@@ -73,21 +86,28 @@ class ControlFrame(QFrame):
         if self.robot_.Connected()==False:
             statu="min-width: 32px; min-height: 32px;max-width:32px;\
                 max-height: 32px;border-radius: 16px; border:1px solid black;background:gray"
-            self.endQc.setEnabled(False)
+            #self.endQc.setEnabled(False)
         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)
         else:
             self.endQc.setEnabled(True)
         self.LedLabel.setStyleSheet(statu)
 
+    def MainAgvchangecb(self):
+        if self.btnModelCheck.checkState() == Qt.Checked:
+            leng=float(self.WheelBaseEdit.text())
+            self.robot_.SwitchMoveMode(2,[-leng/2.0,0,0])
+        if self.btnModelCheck.checkState() == Qt.Unchecked:
+            self.robot_.SwitchMoveMode(1,[0,0,0])
     def endQCChanged(self):
         id1=self.begQc.currentText()
         id2=self.endQc.currentText()
         self.robot_.GeneratePath(id1,id2)
 
     def btnSendClick(self):
+        self.robot_.wheelbase_=3
         self.robot_.ExecNavtask()
     def btnCancelClick(self):
         self.robot_.CancelNavTask()

MapGLWidget.py

@@ -121,7 +121,8 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(x, y)
         glVertex2d(y_source[0], y_source[1])
         glEnd()
-    def DrawAGV(self,pose,rgb,label=""):
+
+    def DrawMainAGV(self,pose,rgb,label):
         [x,y,yaw]=pose
         l=0.8        #轮长
         L=1.3       #轴距
@@ -151,8 +152,54 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(pt8[0],pt8[1])
         glEnd()
 
+        glLineWidth(3)
+        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.2,5)
+        color=[0,0,0]
+        self.DrawText([x-W/2,y-l/2],label,5,1.5,color)
+    def DrawAGV(self,pose,rgb,label=""):
+        [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(5)
         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(1)
+        glBegin(GL_LINES)
         glVertex2d(pt2[0],pt2[1])
         glVertex2d(pt6[0],pt6[1])
         glVertex2d(pt3[0],pt3[1])
@@ -194,7 +241,10 @@ class MapGLWidget(PyGLWidget):
                 x = agv_statu.x
                 y = agv_statu.y
                 theta = agv_statu.theta
-                self.DrawAGV([x, y, theta], robot.robotColor_,robot.name_)
+                if robot.IsMainAgv():
+                    self.DrawMainAGV([x, y, theta], robot.Color(),robot.name_)
+                else:
+                    self.DrawAGV([x, y, theta], robot.Color(),robot.name_)
 
             if robot.timedNavStatu_.timeout() == False:
                 select_traj = robot.MpcSelectTraj()
@@ -207,7 +257,7 @@ class MapGLWidget(PyGLWidget):
         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:
@@ -220,10 +270,10 @@ 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_)
         self.DrawRobotData(self.robot2_)
-        self.DrawRobotData(self.robotMain_)
+
 

RobotData.py

@@ -30,8 +30,21 @@ class Robot(MqttAsync):
         self.robotColor_=[0.7,0.2,0.3]      #agv当前颜色
         self.name_=name
 
-
-
+    def Color(self):
+        if self.IsMainModeStatu():
+            return [0,0,0]
+        return self.robotColor_
+    def IsMainAgv(self):
+        if self.timedNavStatu_.timeout()==False:
+            if self.timedNavStatu_.statu.main_agv==True:
+                return True
+        return False
+    def IsMainModeStatu(self):
+        if self.IsMainAgv():
+            if self.timedNavStatu_.timeout() == False:
+                if self.timedNavStatu_.statu.move_mode==2:
+                    return True
+        return False
     def SetSubDataTopic(self,match:dict,messageSignal=None):
         self.dataTopic_=match
         self.messageArrivedSignal=messageSignal
@@ -47,6 +60,7 @@ class Robot(MqttAsync):
         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:
@@ -118,6 +132,18 @@ class Robot(MqttAsync):
         '''while self.IsNavigating()==False:
             time.sleep(1)
             self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))'''
+    def SwitchMoveMode(self,mode,transform):
+
+        cmd =message.NavCmd()
+        if mode==2:
+            trans=message.Pose2d()
+            trans.x,trans.y,trans.theta=transform
+            cmd.transform.CopyFrom(trans)
+            cmd.action=4
+        if mode==1:
+            cmd.action=5
+
+        self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
 
     def CancelNavTask(self):
         cmd=message.NavCmd()

message.proto

@@ -8,12 +8,18 @@ message AGVStatu {
   float vth=5;
 }
 
+message AGVSpeed{
+  float v=1;
+  float w=2;
+}
+
 message Speed {
   int32 H=1;  //心跳
-  int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
-  float V=3;  //角速度
-  float W=4;  //线速度
-  int32 end=5;
+  int32 M=2;	//模式：1整车模式，其他：单车
+  int32 T=3; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
+  float V=4;  //角速度
+  float W=5;  //线速度
+  int32 end=6;
 }
 
 message SpeedLimit
@@ -48,18 +54,22 @@ message Action
 
 message NavCmd
 {
-  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel,4:切换到双车模式，5:切换到单车模式
   string key=2;
-  repeated Action actions=3;
+  Pose2d transform=3;		//整车中心与单节的变换
+  repeated Action actions=4;
 }
 
 message NavStatu
 {
   bool statu = 1; //0:ready  1:running
-  string key = 2; // 任务唯一码
-  repeated Action unfinished_actions = 3;  //未完成的动作，第一个即为当前动作
-  Trajectory selected_traj = 4;
-  Trajectory predict_traj = 5;
+  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;
 
 }
 
+

test.py

@@ -66,8 +66,6 @@ class MainWindow(QMainWindow):
         splitter_main = self.split_()
         self.setCentralWidget(splitter_main)
 
-        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__,
@@ -76,6 +74,7 @@ class MainWindow(QMainWindow):
         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.gl.SetRobot1Instance(self.Controller1.robot_)
 
         self.Controller2.robot_.connect("pyUI_r2","127.0.0.1",1883,"admin","zx123456")
         dataTopics={"agv1/agv_statu":message.AGVStatu.__name__,
@@ -84,6 +83,16 @@ class MainWindow(QMainWindow):
         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])
+        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")