绘制agv相关数据放入robot类，车位点增加三个节点

ControllWidget.py

@@ -1,25 +1,27 @@
+import math
 
 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
 from RobotData import Robot
-
+import dijkstra.Map as mp
 class ControlFrame(QFrame):
 
     def __init__(self,UI_data):
         QFrame.__init__(self)
-        self.robot_=Robot()
+        [self.name_,self.nodes_]=UI_data
+        self.robot_=Robot(self.name_)
         self.setGeometry(0,0,500,500)
-        self.InitUI(UI_data)
+        self.InitUI()
         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
+    def InitUI(self):
+
         self.basestatic=QLabel(self)
-        self.basestatic.setText(name)
+        self.basestatic.setText(self.name_)
         self.basestatic.setGeometry(20, 20, 80, 30)
 
         self.begstatic=QLabel(self)
@@ -34,11 +36,6 @@ class ControlFrame(QFrame):
         self.endQc=QComboBox(self)
         self.endQc.setGeometry(160, 50, 80, 30)
 
-        self.btn=QPushButton(self)
-        self.btn.setGeometry(20, 100, 100, 40)
-        self.btn.setText("  生成轨迹")
-        self.btn.clicked.connect(self.btnCreateClick)
-
         self.btnSend=QPushButton(self)
         self.btnSend.setGeometry(150, 100, 100, 40)
         self.btnSend.setText("  启动导航")
@@ -49,9 +46,9 @@ class ControlFrame(QFrame):
         self.btnCancel.setText("  取消导航")
         self.btnCancel.clicked.connect(self.btnCancelClick)
 
-
-        self.begQc.addItems(nodes)
-        self.endQc.addItems(nodes)
+        self.begQc.addItems(self.nodes_)
+        self.endQc.addItems(self.nodes_)
+        self.endQc.activated.connect(self.endQCChanged)
 
         self.posestatic=QLabel(self)
         self.posestatic.setGeometry(260, 5, 100, 90)
@@ -64,24 +61,32 @@ class ControlFrame(QFrame):
             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"
+            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)
+
+        statu="min-width: 32px; min-height: 32px;max-width:32px;\
+                max-height: 32px;border-radius: 16px;  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"
+            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)
         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"
+            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)
+        else:
+            self.endQc.setEnabled(True)
         self.LedLabel.setStyleSheet(statu)
 
-
-    def btnCreateClick(self):
+    def endQCChanged(self):
         id1=self.begQc.currentText()
         id2=self.endQc.currentText()
         self.robot_.GeneratePath(id1,id2)
 
-
     def btnSendClick(self):
         self.robot_.ExecNavtask()
     def btnCancelClick(self):

MapGLWidget.py

@@ -5,8 +5,8 @@ from PyGLWidget import PyGLWidget
 from OpenGL.GL import *
 import RobotData as RD
 import message_pb2 as message
-
-
+from OpenGL.GLUT import glutStrokeCharacter
+from OpenGL import GLUT
 class MapGLWidget(PyGLWidget):
 
     nodes = {}
@@ -15,6 +15,9 @@ class MapGLWidget(PyGLWidget):
     robot1_ = None
     robot2_ = None
     robotMain_ = None
+    def __init__(self):
+        PyGLWidget.__init__(self)
+        GLUT.glutInit()
     def SetRobot1Instance(self, robot):
         self.robot1_ = robot
     def SetRobot2Instance(self, robot):
@@ -77,12 +80,27 @@ class MapGLWidget(PyGLWidget):
         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):
+    @staticmethod
+    def Transform(point,yaw,dt):
         [x,y]=point
         [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 DrawText(self,pt,text,width,size,rgb):
+        glDisable(GL_TEXTURE_2D)
+        glLineWidth(width)
+        glPointSize(width)
+        r, g, b = rgb
+        glPushMatrix()
+        glColor3f(r, g, b)
+        glTranslatef(pt[0],pt[1],0)
+        s = size*0.005
+        glScalef(s, s, s)
+        for char in text:
+            GLUT.glutStrokeCharacter(GLUT.GLUT_STROKE_ROMAN, ord(char))
+        glPopMatrix()
+        glEnable(GL_TEXTURE_2D)
 
     def drawAxis(self, pose, len, width):
         [x, y, yaw] = pose
@@ -103,7 +121,7 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(x, y)
         glVertex2d(y_source[0], y_source[1])
         glEnd()
-    def DrawAGV(self,pose,rgb):
+    def DrawAGV(self,pose,rgb,label=""):
         [x,y,yaw]=pose
         l=0.8        #轮长
         L=1.3       #轴距
@@ -146,16 +164,20 @@ class MapGLWidget(PyGLWidget):
         glVertex2d(pt7[0],pt7[1])
         glEnd()
         #绘制方向
-        self.drawAxis(pose,1,5)
+        self.drawAxis(pose,1.2,5)
+        color=[1,1,1]
+        self.DrawText([x-W/2,y-l/2],label,3,1.5,color)
 
-    def drawNode(self, node, size, color):
-        [x, y] = node
+    def drawNode(self, pt, size, color,label=""):
+        [x, y] = pt
         glPointSize(size)
         glDepthMask(GL_FALSE)
         glBegin(GL_POINTS)
         glColor4f(color[0], color[1], color[2], 0.5)
         glVertex2d(x, y)
         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'''
@@ -172,24 +194,19 @@ class MapGLWidget(PyGLWidget):
                 x = agv_statu.x
                 y = agv_statu.y
                 theta = agv_statu.theta
-                self.DrawAGV([x, y, theta], robot.robotColor_)
+                self.DrawAGV([x, y, theta], robot.robotColor_,robot.name_)
 
             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], 3, 2)
 
         #绘制地图
-        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])
         for road in self.roads.items():
             [_, value] = road
             for nodeId in value:
@@ -197,7 +214,13 @@ class MapGLWidget(PyGLWidget):
                     if not nodeId == nextId:
                         [_, point1] = self.nodes[nodeId]
                         [_, point2] = self.nodes[nextId]
-                        self.drawEdge(point1, point2, 3., [0, 0.5, 0.5])
+                        self.drawEdge(point1, point2, 7, [1,1,0])
+        for node in self.nodes.items():
+            [name, [type, point]] = node
+            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)
 
         #绘制agv相关数据，路径、轨迹、位姿
         self.DrawRobotData(self.robot1_)

PyGLWidget.py

@@ -83,7 +83,7 @@ class PyGLWidget(QtOpenGL.QGLWidget):
 
     def initializeGL(self):
         # OpenGL state
-        glClearColor(0.8, 0.8, 0.8, 0.5)
+        glClearColor(0.6, 0.6, 0.6, 0.5)
         glEnable(GL_DEPTH_TEST)
         self.reset_view()
         self.translate([-10,-10,-25.])

RobotData.py

@@ -1,7 +1,5 @@
 
 
-import math
-import random
 import time
 import message_pb2 as message
 from mqtt_async import MqttAsync
@@ -18,7 +16,7 @@ class TimeStatu:
         return tm-self.time>self.timeout_ms or self.statu==None
 
 class Robot(MqttAsync):
-    def __init__(self):
+    def __init__(self,name=""):
         self.timedPose_=TimeStatu(message.AGVStatu,0)
         self.timedNavStatu_=TimeStatu(message.NavStatu,0)
         self.dataTopic_={}
@@ -29,7 +27,11 @@ class Robot(MqttAsync):
         self.currentNavPath_=None
 
         self.pathColor_=[1,1,0]
-        self.robotColor_=[0.7,0.2,0.3]
+        self.robotColor_=[0.7,0.2,0.3]      #agv当前颜色
+        self.name_=name
+
+
+
     def SetSubDataTopic(self,match:dict,messageSignal=None):
         self.dataTopic_=match
         self.messageArrivedSignal=messageSignal
@@ -113,9 +115,9 @@ class Robot(MqttAsync):
             return
 
         self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
-        while self.IsNavigating()==False:
+        '''while self.IsNavigating()==False:
             time.sleep(1)
-            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))
+            self.publish(self.navCmdTopic_,jtf.MessageToJson(cmd))'''
 
     def CancelNavTask(self):
         cmd=message.NavCmd()

dijkstra/Map.py

@@ -1,5 +1,7 @@
 import math
 from dijkstra.dijkstra_algorithm import Graph
+import numpy as np
+import scipy.spatial as spt
 import message_pb2 as message
 import uuid
 
@@ -42,7 +44,6 @@ def singleton(cls):
 map 采用单例
 '''
 
-
 @singleton
 class DijikstraMap(object):
     def __init__(self):
@@ -72,6 +73,24 @@ class DijikstraMap(object):
         if direct == False:
             self.graph_.AddEdge(id2, id1)
 
+    def VertexDict(self):
+        return self.nodes_
+    def Edges(self):
+        return self.graph_.graph_edges
+    def findNeastNode(self,pt):
+        labels=[]
+        pts=[]
+        for item in self.nodes_.items():
+            [label,node]=item
+            labels.append(label)
+            pts.append([node.x_,node.y_])
+        points=np.array(pts)
+        ckt=spt.KDTree(data=points,leafsize=10)
+        find_pt=np.array(pt)
+        d,i=ckt.query(find_pt)
+        if i>=0 and i<len(pts):
+            return [labels[i],pts[i]]
+
     def GetShortestPath(self, beg, end):
         [pathId, distance] = self.graph_.shortest_path(beg, end)
         print("distance:", distance)
@@ -130,6 +149,7 @@ class DijikstraMap(object):
         adjustdiff = message.Pose2d()
         node_mpcdiff = message.Pose2d()
         enddiff = message.Pose2d()
+        lastAdjustDiff=message.Pose2d()
 
         # 目标点精度设置
         adjustdiff.x=(0.1)
@@ -138,12 +158,16 @@ class DijikstraMap(object):
 
         node_mpcdiff.x=(0.1)
         node_mpcdiff.y=(0.1)
-        node_mpcdiff.theta=(2 * math.pi / 180.0)
+        node_mpcdiff.theta=(10 * math.pi / 180.0)
 
         enddiff.x=(0.1)
         enddiff.y=(0.1)
         enddiff.theta=(5 * math.pi / 180.0)
 
+        lastAdjustDiff.x=(0.03)
+        lastAdjustDiff.y=(0.03)
+        lastAdjustDiff.theta=(0.3 * math.pi / 180.0)
+
         # 速度限制
         v_limit = message.SpeedLimit()
         angular_limit = message.SpeedLimit()
@@ -193,8 +217,11 @@ class DijikstraMap(object):
             act_adjust.target.x=(last_node.x_)
             act_adjust.target.y=(last_node.y_)
             act_adjust.target.theta=(yaw)
-
-            act_adjust.target_diff.CopyFrom(adjustdiff)
+            #最后一个调整点
+            if count==len(path)-2:
+                act_adjust.target_diff.CopyFrom(lastAdjustDiff)
+            else:
+                act_adjust.target_diff.CopyFrom(adjustdiff)
             act_adjust.velocity_limit.CopyFrom(v_limit)
             act_adjust.horize_limit.CopyFrom(horize_limit)
             act_adjust.angular_limit.CopyFrom(angular_limit)

map.json

@@ -1,25 +1,36 @@
 
 {
   "street_nodes":{
-    "node1":[3.3,3.5],
-    "node2":[10.3,3.5],
-    "node3": [15.4,3.5],
-    "node4":[17.9,3.5],
-    "node5":[10.3,17.7],
-    "node6":[14.95,17.7],
-    "node7":[17.9,17.7],
-    "node8":[10.3,-0.18],
-    "node11":[18.8,3.5]
+    "O1":[3.3,3.5],
+    "N1":[6.3,3.5],
+    "NT":[6.3,0],
+    "O2":[3.3,0],
+    "N2":[10.3,3.5],
+    "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],
+    "N8":[10.3,-0.18],
+    "N11":[18.8,3.5]
     
   },
   
   "space_nodes":{
-  	"A1":[15.4,7.5,"node3"],
-  	"B1":[14.85,13.7,"node6"]
+  	"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"]
   },
   "roads":{
-    "road1":["node1","node2","node3","node4","node11"],
-    "road2":["node8","node5","node2"],
-    "road3":["node5","node6","node7"]
+    "road1":["O1","N1","N2","N3","N4","N11"],
+    "conn1":["NT","N1"],
+    "conn2":["NT","O2"],
+    "road2":["N8","N5","N2"],
+    "road3":["N5","N6","N7"],
+    "road_A":["A1","A","A2","N3"],
+    "road_B":["B1","B","B2","N6"]
   }
 }