AGVUI_python/MapGLWidget.py

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_)