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 threading
import time
import sys
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 JointContrallerWidget
import ManualOperationWidget
import RobotData as RD
import message_pb2 as message
import google.protobuf.json_format as jtf
import google.protobuf.text_format as tf
from concurrent.futures import ThreadPoolExecutor
import uuid
from mytool.txt_helper.txt_operation import TXTOperation
from custom_define import RobotName
from mytool.RabbitMq_helper import async_communication as rabitmq


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


class MainWindow(QMainWindow):
    """docstring for Mainwindow"""
    mapManager = mp.MapManager()
    ui_nodes = {}
    ui_nodes["street_nodes"] = []
    ui_nodes["space_nodes"] = []

    def __init__(self, parent=None):
        super(MainWindow, self).__init__(parent)
        self.basic()
        self.count_frame_ = ControllWidget.CountFrame()
        self.loadJsonMap("./map.json")
        self.isLocalTest = False
        self.isAutoDispatchOnline = True

        rpc_1 = "192.168.0.70:9090"
        mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
        rpc_2 = "192.168.0.71:9090"
        mqtt_2 = ["pyui2", "192.168.0.71", 1883, "admin", "zx123456"]
        if self.isLocalTest:
            rpc_1 = "127.0.0.1:9090"
            mqtt_1 = ["pyui-main", "127.0.0.1", 1883, "admin", "zx123456"]
            rpc_2 = "127.0.0.1:9091"
            mqtt_2 = ["pyui-child", "127.0.0.1", 1883, "admin", "zx123456"]

        config1 = {"label": RobotName.strAGVMain,
                   "rpc": rpc_1,
                   "street_nodes": self.ui_nodes["street_nodes"],
                   "space_nodes": self.ui_nodes["space_nodes"],
                   "mqtt": mqtt_1,
                   "subTopics": {"agv_main/agv_statu": message.RobotStatu.__name__,
                                 "agv_main/nav_statu": message.NavStatu.__name__},
                   "cmdTopic": "agv_main/nav_cmd",
                   "robotColor": [0.7, 0.2, 0.3]}

        config2 = {"label": RobotName.strAGV2,
                   "rpc": rpc_2,
                   # "rpc": "127.0.0.1:9091",
                   "street_nodes": self.ui_nodes["street_nodes"],
                   "space_nodes": self.ui_nodes["space_nodes"],
                   "mqtt": mqtt_2,
                   "subTopics": {"agv_child/agv_statu": message.RobotStatu.__name__,
                                 "agv_child/nav_statu": message.NavStatu.__name__},
                   "cmdTopic": "agv_child/nav_cmd",
                   "robotColor": [0.4, 0.2, 0.8]}
        self.Controller1 = ControllWidget.ControlFrame(config1)
        self.Controller2 = ControllWidget.ControlFrame(config2)
        robot_dict = {self.Controller1.robot_.name_: self.Controller1.robot_,
                      self.Controller2.robot_.name_: self.Controller2.robot_}
        self.ManualOperationWidget = ManualOperationWidget.ManualOperationWidget(robot_dict)

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

        self.gl.SetRobot1Instance(self.Controller1.robot_)
        self.gl.SetRobot2Instance(self.Controller2.robot_)

        self.timer_ = QTimer()
        self.timer_.timeout.connect(self.Update)
        self.timer_.start(100)

        # 启动RabbitMQ
        self.g_rabbitmq = None
        if self.isAutoDispatchOnline:
            self.g_rabbitmq = rabitmq.RabbitAsyncCommunicator("192.168.0.201", 5672, "zx", "123456")
            calbbacks = [["agv_park_command_request_queue", self.recv_park_request],
                         ["agv_pick_command_request_queue", self.recv_pick_request]]
            self.g_rabbitmq.Init(calbbacks, None)
            self.g_rabbitmq.setDaemon(True)  # 守护线程随主线程结束
            self.g_rabbitmq.start()

    def recv_park_request(self, msg):
        print("Recv_park_request------------------\n", msg)

        park_table = message.park_table()
        try:
            tf.Parse(msg, park_table)
        except Exception as e:
            print("Parse  error\n")
        if self.isAutoDispatchOnline:
            self.AutoParkTask(park_table)

    def recv_pick_request(self, msg):
        print("Recv_pick_request------------------\n", msg)
        pick_table = message.pick_table()

        try:
            tf.Parse(msg, pick_table)
        except Exception as e:
            print("Parse  error\n")
        if self.isAutoDispatchOnline:
            self.AutoPickTask(pick_table)

    def ReLoadMap(self, mapName, file):
        self.mapManager.Reset(mapName)
        map = self.readJson(file)
        flag = "C_"
        if mapName == "Front":
            flag = "F_"
        elif mapName == "Back":
            flag = "B_"
        elif mapName == "Main":
            flag = "C_"
        else:
            print(" map name is error:%s" % (mapName))
            return

        for node in map['street_nodes'].items():
            [id, point] = node
            street_node = mp.StreetNode(id, point[0], point[1])
            if street_node.id_.find(flag) >= 0:
                if street_node.id_.find("OutLink")>=0:
                    continue
                self.mapManager.AddVertex(mapName, street_node)

        for node in map['space_nodes'].items():
            [id, point] = node
            [x, y, yaw] = point
            space_node = mp.SpaceNode(id, point[0], point[1], yaw)
            self.mapManager.AddVertex(mapName, space_node)

        for road in map['roads'].items():
            [roadName, points] = road
            for pt1 in points:
                for pt2 in points:
                    if not pt1 == pt2:
                        if roadName.find(flag) >= 0:
                            self.mapManager.AddEdge(mapName, pt1, pt2)

    def loadJsonMap(self, file):
        self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
        map = self.readJson(file)

        for node in map['street_nodes'].items():
            [id, point] = node
            street_node = mp.StreetNode(id, point[0], point[1])
            self.mapManager.AddVertex("Base", street_node)
            if street_node.id_.find("F_") >= 0:
                self.mapManager.AddVertex("Front", street_node)
            if street_node.id_.find("B_") >= 0:
                self.mapManager.AddVertex("Back", street_node)
            if street_node.id_.find("C_") >= 0:
                self.mapManager.AddVertex("Main", street_node)

            self.ui_nodes["street_nodes"].append(id)
        for node in map['space_nodes'].items():
            [id, point] = node
            [x, y, yaw] = point
            space_node = mp.SpaceNode(id, point[0], point[1], yaw)
            self.mapManager.AddVertex("Base", space_node)
            self.mapManager.AddVertex("Front", space_node)
            self.mapManager.AddVertex("Back", space_node)
            self.mapManager.AddVertex("Main", space_node)

            self.ui_nodes["space_nodes"].append(id)

        for road in map['roads'].items():
            [roadName, points] = road
            for pt1 in points:
                for pt2 in points:
                    if not pt1 == pt2:
                        self.mapManager.AddEdge("Base", pt1, pt2)
                        if pt1.find("OutLink")>=0 or pt2.find("OutLink")>=0:
                            continue
                        if roadName.find("F_") >= 0:
                            self.mapManager.AddEdge("Front", pt1, pt2)
                        if roadName.find("B_") >= 0:
                            self.mapManager.AddEdge("Back", pt1, pt2)
                        if roadName.find("C_") >= 0:
                            self.mapManager.AddEdge("Main", pt1, pt2)
        self.gl.SetMaps(self.mapManager.maps)

    def updateMap_park(self, entrance_id, pose, mapName):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
        self.ReLoadMap(mapName, "./map.json")
        trans_x, trans_y, trans_a = pose

        # 加车位点
        self.mapManager.maps[mapName].AddVertex(mp.SpaceNode("S1101", trans_x, trans_y,trans_a))

        entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
        print("entrance_space pose: ", self.mapManager.maps[mapName].graph_.points[entrance_id])

        street_name = "F_InputLink"
        if mapName == "Front":
            street_name = "F_InputLink"
            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[0][0],
                                                                  entrance_street_nodes[0][1]))  # 更新库位点对应马路点
        if mapName == "Main":
            street_name = "C_InputLink"
            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[1][0],
                                                                entrance_street_nodes[1][1]))  # 更新库位点对应马路点
        if mapName == "Back":
            street_name = "B_InputLink"
            self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[2][0],
                                                                  entrance_street_nodes[2][1]))  # 更新库位点对应马路点

        # 加临时边
        for node_id, value in self.mapManager.VertexDict(mapName).items():
            if node_id.find("Input") >= 0:
                self.mapManager.maps[mapName].AddEdge(street_name, node_id)

        self.mapManager.AddVertex(mapName, mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))  # 更新库位点
        self.mapManager.maps[mapName].AddEdge(entrance_id, street_name)

        print(mapName + " Map entrance_street pose ", self.mapManager.maps[mapName].graph_.points[street_name])
        self.gl.SetMap(mapName, self.mapManager.maps[mapName])

    def GenerateSpecifiedMap(self, map_type):  # 0:前车地图，1：后车地图，2：整车地图
        pass
        '''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.mapManager.AddVertex(street_node)
                self.gl.AddNode([id, "street_node", point])
                self.ui_nodes["street_nodes"].append(id)
            for node in map['space_nodes'].items():
                [id, point] = node
                [x, y, yaw] = point
                space_node = mp.SpaceNode(id, point[0], point[1], yaw)
                self.mapManager.AddVertex(space_node)
                glNode = [id, "space_node", [x, y]]
                self.gl.AddNode(glNode)
                self.ui_nodes["space_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.mapManager.AddEdge(pt1, pt2)
        }



        if type == 0:
            pass
        elif type == 1:
            pass
        elif type == 2:
            pass
        else:
            print("type of specified map is wrong!\n")'''

    def GetMainFrontBackAGV(self):
        controlMain = self.Controller1
        nagtive = False
        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
            nagtive = True
        control1 = None
        control2 = None
        if nagtive == False:
            control1 = self.Controller1
            control2 = self.Controller2
        else:
            control2 = self.Controller1
            control1 = self.Controller2
        return [controlMain, control1, control2]

    def AutoParkTask(self, park_table: message.park_table = None):
        print("AutoParkTask:---------------------\n")

        [controlMain, control1, control2] = self.GetMainFrontBackAGV()
        mainMap, frontMap, backMap = ["Main", "Front", "Back"]
        entrance_id = "S" + str(park_table.terminal_id)  # "S1101"
        entrance_x, entrance_y, entrance_theta = [park_table.entrance_measure_info.measure_info_to_plc_forward.cx,
                                                  park_table.entrance_measure_info.measure_info_to_plc_forward.cy,
                                                  (
                                                          90 + park_table.entrance_measure_info.measure_info_to_plc_forward.theta) / 180 * math.pi]
        # 变换到地图坐标系
        # [dx, dy, da] = [-0.223411843181, -0.643030941486, 178.9478 / 180 * math.pi]
        # trans_x = -0.99983137 * entrance_x - 0.01836309 * entrance_y + dx
        # trans_y = 0.01836309 * entrance_x - 0.99983137 * entrance_y + dy
        [dx, dy, da] = [-0.234040126204, -0.647539079189, 178.9302736990612 / 180 * math.pi]
        trans_x = -0.999825716019 * entrance_x - 0.018668506294 * entrance_y + dx
        trans_y = 0.018668506294 * entrance_x - 0.999825716019 * entrance_y + dy
        trans_a = entrance_theta + da - math.pi
        while trans_a < 0:
            trans_a += math.pi
        print("entrance:", entrance_id, trans_x, trans_y, trans_a / math.pi * 180)

        target_id = "S" + str(park_table.allocated_space_info.table_id)  # "S147"
        print("target:", target_id)

        # 轴距
        wheel_base = park_table.entrance_measure_info.measure_info_to_plc_forward.wheelbase
        control1.WheelBaseEdit.setText(str(wheel_base))
        control2.WheelBaseEdit.setText(str(wheel_base))

        trans_x += wheel_base / 2 * math.cos(trans_a)
        trans_y += wheel_base / 2 * math.sin(trans_a)

        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.shutdown(wait=True)

        # # 双单车入库-----------------------------------------------------
        self.ReLoadMap(frontMap, "./map.json")
        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], frontMap)
        cur_pose1 = control1.robot_.timedRobotStatu_.statu
        [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
        control1.robot_.GeneratePath(label, entrance_id, frontMap)
        print("Main: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))

        # 后车 ------------------------------------------------------
        self.ReLoadMap(backMap, "./map.json")
        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], backMap)
        cur_pose2 = control2.robot_.timedRobotStatu_.statu
        [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
        control2.robot_.GeneratePath(label, entrance_id, backMap)

        # print("Child: begin:%s   target:%s    wheelBase:%f" % (label, entrance_id, wheel_base))

        autoDirect = True

        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.Navigatting,
                          label, entrance_id, autoDirect, wheel_base)
        threadPool.submit(control2.robot_.Navigatting,
                          label, entrance_id, autoDirect, wheel_base)

        threadPool.shutdown(wait=True)

        print(" input entrance completed!!!!")
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
        threadPool.shutdown(wait=True)
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.ClampClose)
        threadPool.shutdown(wait=True)

        # 整车入库---------------------------------
        map_name="Front"
        if controlMain.robot_.timedRobotStatu_.statu.theta<0:
            map_name="Back"

        self.ReLoadMap(map_name, "./map.json")
        self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], map_name)
        controlMain.robot_.GeneratePath(entrance_id, target_id, map_name)
        print("begin:%s   target:%s    wheelBase:%f" % (entrance_id, target_id, wheel_base))


        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.Navigatting,
                          entrance_id, target_id, autoDirect, wheel_base)
        threadPool.shutdown(wait=True)

        # 整车松夹池------------------------------------
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.ClampOpen)
        threadPool.shutdown(wait=True)
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.shutdown(wait=True)

        # 双单车出库
        self.ReLoadMap(frontMap, "./map.json")
        controlMain, control1, control2 = self.GetMainFrontBackAGV()
        control1.robot_.GeneratePath(target_id, "F_Input_R147", frontMap)

        self.ReLoadMap(backMap, "./map.json")
        control2.robot_.GeneratePath(target_id, "B_Input_R147", backMap)

        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.Navigatting,
                          target_id, "F_Input_R147", autoDirect, wheel_base)
        threadPool.submit(control2.robot_.Navigatting,
                          target_id, "B_Input_R147", autoDirect, wheel_base)

        threadPool.shutdown(wait=True)

        # 反馈
        self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
                                tf.MessageToString(park_table, as_utf8=True))
        print("Publish_park_response------------------\n", park_table)
        return True

    def updateMap_pick(self, export_id, mapName):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
        self.ReLoadMap(mapName, "./map.json")

        # export_id = "S1101"
        # 添加出口车位点
        export_x = self.mapManager.maps["Base"].graph_.points["S1100"][0]
        export_y = self.mapManager.maps["Base"].graph_.points["S1100"][1]
        self.mapManager.AddVertex(mapName, mp.SpaceNode(export_id, export_x, export_y, 90))
        # 更新库位点对应马路点
        flag = "F_"
        if mapName == "Front":
            export_y = self.mapManager.maps["Base"].graph_.points["F_OutLink"][1]
            node = mp.StreetNode("F_exportLink", export_x, export_y)
        elif mapName == "Back":
            flag = "B_"
            export_y = self.mapManager.maps["Base"].graph_.points["B_OutLink"][1]
            node = mp.StreetNode("B_exportLink", export_x, export_y)
        elif mapName == "Main":
            flag = "C_"
            export_y = self.mapManager.maps["Base"].graph_.points["C_OutLink"][1]
            node = mp.StreetNode("C_exportLink", export_x, export_y)

        self.mapManager.AddVertex(mapName, node)  # 更新库位点对应马路点
        self.mapManager.AddEdge(mapName, export_id, node.id_)

        for streetID, value in self.mapManager.VertexDict(mapName).items():
            if streetID.find(flag) >= 0:
                self.mapManager.AddEdge(mapName, node.id_, streetID)
            print("Add edge :%s-%s  ", node.id_, streetID)

    def AutoPickTask(self, pick_table: message.pick_table = None):
        print("AutoPickTask:---------------------\n")

        controlMain, control1, control2 = self.GetMainFrontBackAGV()
        mainMap, frontMap, backMap = ["Main", "Front", "Back"]

        space_id = "S" + str(pick_table.actually_space_info.table_id)  # "S147"

        export_id = "S" + str(pick_table.terminal_id)  # "S1101"
        print(space_id, export_id)
        # 轴距
        wheel_base = pick_table.actually_measure_info.measure_info_to_plc_forward.wheelbase
        control1.WheelBaseEdit.setText(str(wheel_base))
        control2.WheelBaseEdit.setText(str(wheel_base))


        # 双单车入库
        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.shutdown(wait=True)


        self.ReLoadMap(frontMap, "./map.json")
        self.updateMap_pick(export_id, frontMap)
        cur_pose1 = control1.robot_.timedRobotStatu_.statu
        [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
        control1.robot_.GeneratePath(label, space_id, frontMap)
        print("Main: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))

        self.ReLoadMap(backMap, "./map.json")
        self.updateMap_pick(export_id, backMap)
        cur_pose2 = control2.robot_.timedRobotStatu_.statu
        [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
        control2.robot_.GeneratePath(label, space_id, backMap)
        print("Child: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))


        autoDirect = True
        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.Navigatting,
                          label, space_id, autoDirect, wheel_base)
        threadPool.submit(control2.robot_.Navigatting,
                          label, space_id, autoDirect, wheel_base)
        threadPool.shutdown(wait=True)

        # 整车夹持------------------------------------
        print(" input space completed!!!!")
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
        threadPool.shutdown(wait=True)
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.ClampClose)
        threadPool.shutdown(wait=True)

        # 整车到出口---------------------------------
        map_name = "Front"
        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
            map_name = "Back"
        self.ReLoadMap(map_name, "./map.json")
        self.updateMap_pick(export_id, map_name)

        controlMain.robot_.GeneratePath(space_id, export_id, map_name)
        print("Total: begin:%s   target:%s    wheelBase:%f" % (space_id, export_id, wheel_base))



        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.Navigatting,
                          space_id, export_id, True, wheel_base)
        threadPool.shutdown(wait=True)


        # 整车松夹池------------------------------------

        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.ClampOpen)
        threadPool.shutdown(wait=True)
        threadPool = ThreadPoolExecutor(1)
        threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
        threadPool.shutdown(wait=True)

        # 双单车出库
        controlMain, control1, control2 = self.GetMainFrontBackAGV()
        self.ReLoadMap(frontMap, "./map.json")
        self.updateMap_pick(export_id, frontMap)
        control1.robot_.GeneratePath(export_id, "F_exportLink", frontMap)

        self.ReLoadMap(backMap, "./map.json")
        self.updateMap_pick(export_id, backMap)
        control2.robot_.GeneratePath(export_id, "B_exportLink", backMap)


        threadPool = ThreadPoolExecutor(2)
        threadPool.submit(control1.robot_.Navigatting,
                          export_id, "F_exportLink", True, wheel_base)
        threadPool.submit(control2.robot_.Navigatting,
                          export_id, "B_exportLink", True, wheel_base)
        threadPool.shutdown(wait=True)

        # 反馈
        self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
                                tf.MessageToString(pick_table, as_utf8=True))
        print("Publish_park_response------------------\n", pick_table)
        return True


    def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
        """

        """
        n_x, n_y = 0, 0
        if s_id == "S1101":
            n_y = self.mapManager.maps["Base"].graph_.points["C_OutLink"][1]
            k = math.tan(s_theta)
            n_x = (n_y - s_y) / k + s_x  # 弧度

            n_yF = self.mapManager.maps["Base"].graph_.points["F_OutLink"][1]
            n_xF = (n_yF - s_y) / k + s_x  # 弧度

            n_yB = self.mapManager.maps["Base"].graph_.points["B_OutLink"][1]
            n_xB = (n_yB - s_y) / k + s_x  # 弧度

        # print(n_x, n_y)
        return [[n_xF, n_yF], [n_x, n_y], [n_xB, n_yB]]

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

    def Update(self):
        self.gl.update()
        if self.isAutoDispatchOnline:
            # self.Controller1.setEnabled(False)
            # self.Controller2.setEnabled(False)
            pass
        else:
            self.Controller1.setVisible(True)
            self.Controller2.setVisible(True)

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

    # 分割窗口
    def split_(self):
        splitter_main = QSplitter(Qt.Horizontal)
        splitter = QSplitter(Qt.Vertical)

        splitter.addWidget(self.count_frame_)
        splitter.addWidget(self.Controller1)
        splitter.addWidget(self.Controller2)
        splitter.addWidget(self.ManualOperationWidget)

        splitter.setStretchFactor(0, 1)
        splitter.setStretchFactor(1, 3)
        splitter.setStretchFactor(2, 3)
        splitter.setStretchFactor(3, 1)

        splitter_main.addWidget(splitter)
        splitter_main.addWidget(self.gl)

        splitter_main.setStretchFactor(0, 1)
        splitter_main.setStretchFactor(2, 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:
#
# ===============================================================================