import math import random import time 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 = {} roads = {} robot1_ = None robot2_ = None def __init__(self): PyGLWidget.__init__(self) GLUT.glutInit() def SetRobot1Instance(self, robot): self.robot1_ = robot def SetRobot2Instance(self, robot): self.robot2_ = 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] [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): [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 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,1.2,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/10,1,[1,1,1]) 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 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) #绘制地图 for road in self.roads.items(): [_, 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, 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_) self.DrawRobotData(self.robot2_)