AGVUI_python/test.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#===============================================================================
#
# test.py
#
# Test program for the simple GL Viewer.
#
# Copyright (c) 2011, Arne Schmitz <arne.schmitz@gmx.net>
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the <organization> nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
#===============================================================================
import math
import time
import sys
from PyQt5.QtGui import *
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from MapGLWidget import MapGLWidget,TimeStatu
import json
import dijkstra.Map as mp
import ControllWidget
import mqtt_async as mqtt
import message_pb2 as message
import google.protobuf.json_format as jtf
#===============================================================================


class MainWindow(QMainWindow):
    """docstring for Mainwindow"""

    def __init__(self, parent = None):
        super(MainWindow,self).__init__(parent)
        self.basic()


        splitter_main = self.split_()
        self.setCentralWidget(splitter_main)

        self.path=None

        self.agv1_statu=TimeStatu(None,0.1)
        self.agv2_statu=TimeStatu(None,0.1)
        self.mqtt_=mqtt.MqttAsync("UI")
        self.mqtt_.connect("127.0.0.1",1883,"admin","zx123456")
        self.mqtt_.subscribe("agv1_child/agv_statu",self.agv_statu_arrived)
        self.mqtt_.loop_start()



    def agv_statu_arrived(self,client,user_data,msg):
        statu=message.AGVStatu()
        jtf.Parse(msg.payload.decode(),statu)
        self.gl.ResetAgv1(statu)
        self.agv1_statu=TimeStatu(statu,0.3)


    def load_map(self,file):
        with open(file,'r',encoding='utf-8') as fp:
            map=json.load(fp)
            return map


    #窗口基础属性
    def basic(self):
        #设置标题，大小，图标
        self.setWindowTitle("GT")
        self.resize(1100,650)
        self.setWindowIcon(QIcon("./image/Gt.png"))
        #居中显示
        screen = QDesktopWidget().geometry()
        self_size = self.geometry()
        self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))

    def closeEvent(self, QCloseEvent):
        self.gl.close()


    @staticmethod
    def Create_traj(path,delta):
        last_node=None
        trajectry=[]
        for node in path:
            if last_node==None:
                last_node=node
                continue
            dis=last_node.distance(node)
            if dis<0.5:
                last_node=node
                continue    #同一点
            else:
                vector=[node.x_-last_node.x_,node.y_-last_node.y_]

                dx=vector[0]
                dy=vector[1]
                yaw=math.asin(dy/math.sqrt(dx*dx+dy*dy))
                if yaw>=0:
                    if dx<0:
                        yaw=math.pi-yaw
                if yaw<0:
                    if dx<0:
                        yaw=-math.pi-yaw
                len=int(math.sqrt(dx*dx+dy*dy)/delta)
                ax=math.cos(yaw)*delta
                ay=math.sin(yaw)*delta
                poses=[]
                if isinstance(last_node,(mp.SpaceNode)):
                    yaw=yaw+math.pi
                for i in range(len+1):
                    pose=[last_node.x_+i*ax,last_node.y_+i*ay,yaw]
                    poses.append(pose)
                trajectry.append(poses)
                last_node=node
        return trajectry

    def generater_path(self,beg,end):
        self.path=self.djks_map_.GetShortestPath(beg,end)
        self.traj=self.Create_traj(self.path,0.5)
        self.gl.AddTraj("agv1",self.traj)
        self.gl.update()

    def send_cmd(self):
        if self.path is not None and self.agv1_statu.timeout()==False:
            statu=self.agv1_statu.statu
            if statu is not None:
                cmd=self.djks_map_.CreateNavCmd([statu.x,statu.y,statu.theta],self.path)
                if cmd is not None:
                    self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))
                else:
                    print(" NavCmd is None")

    def Cancel_cmd(self):
        cmd=message.NavCmd()
        cmd.action=3
        self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))


    #分割窗口
    def split_(self):
        splitter = QSplitter(Qt.Vertical)
        self.gl = MapGLWidget()   #将opengl例子嵌入GUI
        map=self.load_map("./map.json")
        UI_data={}
        UI_data["nodes"]=[]
        self.djks_map_=mp.Map()

        for node in map['street_nodes'].items():
            [id,point]=node
            street_node=mp.StreetNode(id,point[0],point[1])
            self.djks_map_.AddVertex(street_node)
            self.gl.AddNode([id,"street_node",point])
            UI_data["nodes"].append(id)
        for node in map['space_nodes'].items():
            [id,point]=node
            [x,y,connectId]=point
            space_node=mp.SpaceNode(id,point[0],point[1],connectId)
            self.djks_map_.AddVertex(space_node)
            self.djks_map_.AddEdge(id,connectId)
            glNode=[id,"space_node",[x,y]]
            self.gl.AddNode(glNode)
            self.gl.AddRoad(["conn%s-%s"%(id,connectId),[id,connectId]])
            UI_data["nodes"].append(id)

        for road in map['roads'].items():
            self.gl.AddRoad(road)

            [_,points]=road
            for pt1 in points:
                for pt2 in points:
                    if not pt1==pt2:
                        self.djks_map_.AddEdge(pt1,pt2)

        splitter.addWidget(self.gl)

        testedit = QTextEdit()
        splitter.addWidget(testedit)
        splitter.setStretchFactor(0,3)
        splitter.setStretchFactor(1,2)
        splitter_main = QSplitter(Qt.Horizontal)
        Controller = ControllWidget.Frame(UI_data)

        callbacks={}
        callbacks["CreateTraj"]=self.generater_path
        callbacks["SendCmd"]=self.send_cmd
        callbacks["CancelCmd"]=self.Cancel_cmd


        Controller.SetBtnFunc(callbacks)
        splitter_main.addWidget(Controller)


        splitter_main.addWidget(splitter)
        splitter_main.setStretchFactor(0,1)
        splitter_main.setStretchFactor(1,4)
        return splitter_main

if __name__ == "__main__":
    app = QApplication(sys.argv)
    win = MainWindow()
    win.show()
    sys.exit(app.exec_())


#===============================================================================
# Main
#===============================================================================

'''app = QApplication(sys.argv)
mainWindow = MapGLWidget()
mainWindow.show()
mainWindow.raise_() # Need this at least on OS X, otherwise the window ends up in background
sys.exit(app.exec_())'''

#===============================================================================
#
# Local Variables:
# mode: Python
# indent-tabs-mode: nil
# End:
#
#===============================================================================