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 import QtWidgets, QtGui
from PyQt5.QtGui import *
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from MapGLWidget import MapGLWidget
import json
import dijkstra.Map as mp
import ControllWidget
import RobotData as RD
import message_pb2 as message
import google.protobuf.json_format as jtf
import uuid


# ===============================================================================


class MainWindow(QMainWindow):
    """docstring for Mainwindow"""
    djks_map_ = mp.DijikstraMap()
    ui_nodes = []

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

        self.LoadMap()
        self.Controller1 = ControllWidget.ControlFrame(["AGV1", self.ui_nodes])
        self.Controller2 = ControllWidget.ControlFrame(["AGV2", self.ui_nodes])

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

        self.Controller1.robot_.connect("pyUI_r1", "127.0.0.1", 1883, "admin", "zx123456")
        dataTopics = {"agv1_child/agv_statu": message.AGVStatu.__name__,
                      "agv1_child/nav_statu": message.NavStatu.__name__}
        self.Controller1.robot_.SetSubDataTopic(dataTopics, self.messageArrived)
        self.Controller1.robot_.SetCmdTopic("agv1_child/nav_cmd")
        self.Controller1.robot_.loop_start()
        self.Controller1.robot_.SetColor([0.7, 0.2, 0.3], [0.7, 0.2, 0.3])
        self.gl.SetRobot1Instance(self.Controller1.robot_)

        self.Controller2.robot_.connect("pyUI_r2", "127.0.0.1", 1883, "admin", "zx123456")
        dataTopics = {"agv1/agv_statu": message.AGVStatu.__name__,
                      "agv1/nav_statu": message.NavStatu.__name__}
        self.Controller2.robot_.SetSubDataTopic(dataTopics, self.messageArrived)
        self.Controller2.robot_.SetCmdTopic("agv1/nav_cmd")
        self.Controller2.robot_.loop_start()
        self.Controller2.robot_.SetColor([0.4, 0.2, 0.8], [0.4, 0.2, 0.8])
        self.gl.SetRobot2Instance(self.Controller2.robot_)

    def LoadMap(self):
        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
        map = self.load_map("./map.json")

        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])
            self.ui_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]])
            self.ui_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)

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

    def messageArrived(self):
        self.gl.update()

    # 窗口基础属性
    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()

    # 基于布局的显示窗口
    def split_(self):
        total_layout = QtWidgets.QGridLayout()

        # 地图显示布局
        display_layout = QtWidgets.QGridLayout()
        display_layout.setObjectName("display_layout")
        display_layout.addWidget(self.gl)
        total_layout.addLayout(display_layout, 0, 1, 1, 1)

        # 操作控制布局
        control_layout = QtWidgets.QGridLayout()
        control_layout.setObjectName("control_layout")
        control_layout.addWidget(self.Controller1)
        control_layout.addWidget(self.Controller2)
        total_layout.addLayout(control_layout, 0, 0, 1, 1)

        # 总布局设置
        total_layout.setColumnStretch(0, 2)
        total_layout.setColumnStretch(1, 5)
        total_layout.setRowStretch(0, 1)

        # 将总布局加载到主窗口
        centerWidget = QtWidgets.QWidget()
        # font = QtGui.QFont()
        # font.setFamily("楷体")
        # font.setPointSize(15)
        # centerWidget.setFont(font)
        centerWidget.setLayout(total_layout)


        return centerWidget


if __name__ == "__main__":
    app = QApplication(sys.argv)
    win = MainWindow()
    win.showMaximized()
    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
# Park_End:
#
# ===============================================================================