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 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, pose] = node
        self.nodes[id] = [type, pose]

    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=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/10,1,[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)
        #绘制地图
        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, 20, [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_)