#!/usr/bin/env python # -*- coding: utf-8 -*- # =============================================================================== # # test.py # # Test program for the simple GL Viewer. # # Copyright (c) 2011, Arne Schmitz # 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 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 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""" djks_map_ = 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.LoadMap() 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) # time.sleep(30) # # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port", # # tf.MessageToString(table, as_utf8=True)) # self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port", # msg) # print("Publish_park_response------------------\n", msg) # return True 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() # time.sleep(30) # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port", # tf.MessageToString(table, as_utf8=True)) # self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port", # msg) # print("Publish_pick_response------------------\n", msg) # return True try: tf.Parse(msg, pick_table) except Exception as e: print("Parse error\n") if self.isAutoDispatchOnline: self.AutoPickTask(pick_table) def updateMap_park(self, entrance_id, pose, type): # 0:前车轨迹,1:后车轨迹,2:添加整车轨迹 self.djks_map_.Reset() # 重置地图 trans_x, trans_y, trans_a = pose self.djks_map_.AddVertex_t(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a)) # 更新库位点 entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a) print("entrance_space pose: ", self.djks_map_.map_t.graph_.points[entrance_id]) if type == 0: self.djks_map_.AddVertex_t(mp.StreetNode("FInput_R1101", entrance_street_nodes[0][0], entrance_street_nodes[0][1])) # 更新库位点对应马路点 self.djks_map_.AddEdge_t(entrance_id, "FInput_R1101") # 加临时边 for node_id, value in self.djks_map_.VertexDict().items(): if node_id.find("FInput") >= 0: self.djks_map_.AddEdge_t("FInput_R1101", node_id) print("F entrance_street pose ", self.djks_map_.map_t.graph_.points["FInput_R1101"]) elif type == 1: self.djks_map_.AddVertex_t(mp.StreetNode("BInput_R1101", entrance_street_nodes[2][0], entrance_street_nodes[2][1])) # 更新库位点对应马路点 self.djks_map_.AddEdge_t(entrance_id, "BInput_R1101") # 加临时边 for node_id, value in self.djks_map_.VertexDict().items(): if node_id.find("BInput") >= 0: self.djks_map_.AddEdge_t("BInput_R1101", node_id) else: self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1101", entrance_street_nodes[1][0], entrance_street_nodes[1][1])) # 更新库位点对应马路点 self.djks_map_.AddEdge_t(entrance_id, "CInput_R1101") for node_id, value in self.djks_map_.VertexDict().items(): if node_id.find("CInput") >= 0: self.djks_map_.AddEdge_t("CInput_R1101", node_id) print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1101"]) def GenerateSpecifiedMap(self, map_type): # 0:前车地图,1:后车地图,2:整车地图 '''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["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.djks_map_.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.djks_map_.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() 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.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 0) cur_pose1 = control1.robot_.timedRobotStatu_.statu [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y]) control1.robot_.GeneratePath(label, entrance_id) print("Main: begin:%s target:%s wheelBase:%f" % (label, entrance_id, wheel_base)) # 后车 ------------------------------------------------------ self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 1) cur_pose2 = control2.robot_.timedRobotStatu_.statu [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y]) control2.robot_.GeneratePath(label, entrance_id) 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) # 整车入库--------------------------------- self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 2) controlMain.robot_.GeneratePath(entrance_id, target_id) 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) # 双单车出库 controlMain, control1, control2 = self.GetMainFrontBackAGV() self.djks_map_.Reset() # 重置地图 control1.robot_.GeneratePath(target_id, "FInput_R147") control2.robot_.GeneratePath(target_id, "BInput_R147") threadPool = ThreadPoolExecutor(2) threadPool.submit(control1.robot_.Navigatting, target_id, "FInput_R147", autoDirect, wheel_base) threadPool.submit(control2.robot_.Navigatting, target_id, "BInput_R147", autoDirect, wheel_base) threadPool.shutdown(wait=True) self.g_rabbitmq.publish() def updateMap_pick(self, export_id, type): # 0:前车轨迹,1:后车轨迹,2:添加整车轨迹 self.djks_map_.Reset() # 重置地图 # export_id = "S1103" # 添加出口车位点 self.djks_map_.AddVertex_t(mp.SpaceNode(export_id, self.djks_map_.map_t.graph_.points["S1100"][0], self.djks_map_.map_t.graph_.points["S1100"][1], 90)) # 更新库位点对应马路点 if type == 0: self.djks_map_.AddEdge_t(export_id, "FInput_R1100") elif type == 1: self.djks_map_.AddEdge_t(export_id, "BInput_R1100") elif type == 2: export_street_node_x = self.djks_map_.map_t.graph_.points["CInput_R1100"][0] export_street_node_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][1] self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1103", export_street_node_x, export_street_node_y)) # 更新库位点对应马路点 self.djks_map_.AddEdge_t(export_id, "CInput_R1103") for node_id, value in self.djks_map_.VertexDict().items(): if node_id.find("CInput") >= 0: self.djks_map_.AddEdge_t("CInput_R1103", node_id) print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1103"]) def AutoPickTask(self, pick_table: message.pick_table = None): print("AutoPickTask:---------------------\n") controlMain, control1, control2 = self.GetMainFrontBackAGV() self.djks_map_.Reset() # 重置地图 space_id = "S" + str(pick_table.actually_space_info.table_id) # "S147" export_id = "S" + str(pick_table.terminal_id) # "S1103" 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) cur_pose1 = control1.robot_.timedRobotStatu_.statu [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y]) control1.robot_.GeneratePath(label, space_id) print("Main: begin:%s target:%s wheelBase:%f" % (label, space_id, wheel_base)) cur_pose2 = control2.robot_.timedRobotStatu_.statu [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y]) control2.robot_.GeneratePath(label, space_id) 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) # 整车到出口--------------------------------- self.updateMap_pick(export_id, 2) controlMain.robot_.GeneratePath(space_id, export_id) 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.updateMap_pick(export_id, 0) control1.robot_.GeneratePath(export_id, "FInput_R1100") self.updateMap_pick(export_id, 1) control2.robot_.GeneratePath(export_id, "BInput_R1100") threadPool = ThreadPoolExecutor(2) threadPool.submit(control1.robot_.Navigatting, export_id, "FInput_R1100", True, wheel_base) threadPool.submit(control2.robot_.Navigatting, export_id, "BInput_R1100", True, wheel_base) threadPool.shutdown(wait=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.djks_map_.map_t.graph_.points["CInput_R1100"][1] k = math.tan(s_theta) n_x = (n_y - s_y) / k + s_x # 弧度 n_yF = self.djks_map_.map_t.graph_.points["FInput_R1100"][1] n_xF = (n_yF - s_y) / k + s_x # 弧度 n_yB = self.djks_map_.map_t.graph_.points["BInput_R1100"][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 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["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.djks_map_.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.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 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: # # ===============================================================================