AGVUI_python/MapGLWidget.py

import math
import random
import time
from PyGLWidget import PyGLWidget
from OpenGL.GL import *

import message_pb2 as message
class TimeStatu:
    def __init__(self,statu=None,timeout=3):
        self.statu=statu
        self.time=time.time()
        self.timeout_ms=timeout

    def timeout(self):
        tm=time.time()
        return tm-self.time>self.timeout_ms or self.statu==None
class MapGLWidget(PyGLWidget):

    colors={"agv1":[1,0,0],
            "agv2":[0,1,0],
            "agv":[0,1,1]
            }

    nodes={}
    roads={}
    paths={}
    timedAgv1=TimeStatu(None,0.5)
    timedAgv2=TimeStatu(None,0.5)
    def ResetAgv1(self,statu):
        self.timedAgv1=TimeStatu(statu,0.5)
        self.update()
    def ResetAgv2(self,statu):
        self.timedAgv2=TimeStatu(statu,0.5)
        self.update()
    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 AddTraj(self,name,path):
        self.paths[name]=path


    def drawPath(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 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]
    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 drawNode(self,node,size,color):
        [x,y]=node
        glPointSize(size)
        glDepthMask(GL_FALSE)
        glBegin(GL_POINTS)
        glColor4f(color[0],color[1],color[2],0.5)
        glVertex2d(x,y)
        glEnd()
        '''glColor3f(1,1,1);
        #glBegin(GL_TEXTURE)
        glText'''

    def paintGL(self):
        PyGLWidget.paintGL(self)

        self.drawAxis([0,0,0],5,5)
        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:
                for nextId in value:
                    if not nodeId==nextId:
                        [_,point1]=self.nodes[nodeId]
                        [_,point2]=self.nodes[nextId]
                        self.drawEdge(point1,point2,3.,[0,0.5,0.5])

        for [id,path] in self.paths.items():

            rgb=self.colors.get(id)
            if rgb is not None:
                for traj in path:
                    self.drawPath(traj,0.5,rgb)

        #绘制agv
        if self.timedAgv1.timeout()==False:
            x=self.timedAgv1.statu.x
            y=self.timedAgv1.statu.y
            theta=self.timedAgv1.statu.theta
            self.drawAxis([x,y,theta],3,5)