import math import random import time from PyGLWidget import PyGLWidget from OpenGL.GL import * import RobotData as RD import message_pb2 as message from dijkstra import Map from OpenGL.GLUT import glutStrokeCharacter from OpenGL import GLUT def singleton(cls): _instance = {} def inner(): if cls not in _instance: _instance[cls] = cls() return _instance[cls] return inner @singleton class MapGLWidget(PyGLWidget): maps_={} pointColors={"Front":[0,0.5,0.5], "Back":[0.5,0,0], "Main":[0.5,0,0.5], "Base":[0,0.5,0]} edgeColors = {"Front": [0, 0.5, 0.5], "Back": [0.5, 0, 0], "Main": [0.5, 0, 0.5], "Base": [0, 0.5, 0]} pointSizes={"Front":20, "Back":20, "Main":20, "Base":20 } edgeLineWidth = {"Front": 5, "Back": 5, "Main": 5, "Base": 2 } enableMaps=["Front","Back","Main"] robot1_ = None robot2_ = None def __init__(self): PyGLWidget.__init__(self) GLUT.glutInit() def SetMaps(self,maps): self.maps_=maps def SetMap(self,mapName,map:Map.DijikstraMap): self.maps_[mapName]=map def SetEnableMaps(self,mapNames): self.enableMaps=mapNames def SetRobot1Instance(self, robot): self.robot1_ = robot def SetRobot2Instance(self, robot): self.robot2_ = robot 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] [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] glVertex2d(point1[0], point1[1]) glVertex2d(point3[0], point3[1]) glVertex2d(point1[0], point1[1]) glVertex2d(point2[0], point2[1]) glEnd() 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() 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]) glEnd() @staticmethod 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] @staticmethod def Transform(point,yaw,dt=None): [x,y]=point nx = math.cos(yaw) * x - y * math.sin(yaw) ny = x * math.sin(yaw) + y * math.cos(yaw) if dt is not None: [dx,dy]=dt nx+=dx ny+=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 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) glLineWidth(width) glBegin(GL_LINES) glVertex2d(x, y) glVertex2d(x_source[0], x_source[1]) glEnd() glColor3f(0, 1, 0) glBegin(GL_LINES) glVertex2d(x, y) glVertex2d(y_source[0], y_source[1]) glEnd() def DrawMainAGV(self,pose,rgb,label,running=False): l=0.8 #轮长 L=1.3 #轴距 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]) 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(2) 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,L,5) 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 #轮长 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(3) 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]) 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=[1,1,1] self.DrawText([x-W/2,y-l/2],label,3,1.5,color) 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/200,0.5,[1,1,1]) def drawSpaceNode(self,pose,size,color,label=''): [x, y,yaw] = pose self.drawNode([x,y],size,color,label) width=2.4 length=6 pt1=self.Transform([x-length/2,y+width/2],yaw) pt2=self.Transform([x+length/2,y+width/2],yaw) pt3=self.Transform([x-length/2,y-width/2],yaw) pt4=self.Transform([x+length/2,y-width/2],yaw) glDepthMask(GL_FALSE) glColor3f(1,1,1) glLineWidth(10) glBegin(GL_LINES) glVertex2d(pt1[0],pt1[1]) glVertex2d(pt2[0],pt2[1]) glVertex2d(pt2[0],pt2[1]) glVertex2d(pt3[0],pt3[1]) glVertex2d(pt3[0],pt3[1]) glVertex2d(pt4[0],pt4[1]) 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: for traj in robot.currentNavPath_: self.drawTraj(traj, 0.5, robot.pathColor_) # 绘制agv if robot is not None: if robot.timedRobotStatu_.timeout() == False: agv_statu = robot.timedRobotStatu_.statu x = agv_statu.x y = agv_statu.y theta = agv_statu.theta if robot.IsMainAgv(): 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_) 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], 3, 2) #绘制地图 pointSize = 10 pointColor = [0.2, 0.3, 0.6] edgeWidth = 10 edgeColor = [0.1, 0.6, 0.1] for item in self.maps_.items(): key,map=item if self.enableMaps.count(key)<=0: continue for edge in map.Edges(): [node1,node2,distance]=edge vertex1=map.GetVertex(node1) vertex2 = map.GetVertex(node2) self.drawEdge([vertex1.x_,vertex1.y_],[vertex2.x_,vertex2.y_],self.edgeLineWidth[key],self.edgeColors[key]) for vertex in map.VertexDict().items(): nodeID,node=vertex self.drawNode([node.x_,node.y_], self.pointSizes[key], self.pointColors[key], nodeID) #绘制agv相关数据，路径、轨迹、位姿 self.DrawRobotData(self.robot1_) self.DrawRobotData(self.robot2_)