|
|
@@ -33,6 +33,7 @@
|
|
|
#
|
|
|
# ===============================================================================
|
|
|
import math
|
|
|
+import threading
|
|
|
import time
|
|
|
import sys
|
|
|
from PyQt5.QtGui import *
|
|
|
@@ -47,9 +48,27 @@ 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
|
|
|
+
|
|
|
+'''
|
|
|
+"road_1100F": [
|
|
|
+ "S1100",
|
|
|
+ "FInput_R1100"
|
|
|
+ ],
|
|
|
+ "road_1100": [
|
|
|
+ "S1100",
|
|
|
+ "CInput_R1100"
|
|
|
+ ],
|
|
|
+ "road_1100B": [
|
|
|
+ "S1100",
|
|
|
+ "BInput_R1100"
|
|
|
+ ]
|
|
|
+'''
|
|
|
|
|
|
|
|
|
# ===============================================================================
|
|
|
@@ -57,7 +76,7 @@ from custom_define import RobotName
|
|
|
|
|
|
class MainWindow(QMainWindow):
|
|
|
"""docstring for Mainwindow"""
|
|
|
- djks_map_ = mp.DijikstraMap()
|
|
|
+ djks_map_ = mp.MapManager()
|
|
|
ui_nodes = {}
|
|
|
ui_nodes["street_nodes"] = []
|
|
|
ui_nodes["space_nodes"] = []
|
|
|
@@ -67,8 +86,8 @@ class MainWindow(QMainWindow):
|
|
|
self.basic()
|
|
|
self.count_frame_ = ControllWidget.CountFrame()
|
|
|
self.LoadMap()
|
|
|
- self.isLocalTest = True
|
|
|
- self.isAutoDispatch = False
|
|
|
+ self.isLocalTest = False
|
|
|
+ self.isAutoDispatchOnline = True
|
|
|
|
|
|
rpc_1 = "192.168.0.70:9090"
|
|
|
mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
|
|
|
@@ -116,6 +135,335 @@ class MainWindow(QMainWindow):
|
|
|
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")
|
|
|
+ # split_msg = msg.split(' ')
|
|
|
+ # result = [float(split_msg[0]), float(split_msg[1]), float(split_msg[2])]
|
|
|
+ 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")
|
|
|
+ # split_msg = msg.split(' ')
|
|
|
+ # result = [float(split_msg[0]), float(split_msg[1]), float(split_msg[2])]
|
|
|
+ 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 AutoParkTask(self, park_table: message.park_table = None):
|
|
|
+ print("AutoParkTask:---------------------\n")
|
|
|
+ controlMain=self.Controller1
|
|
|
+
|
|
|
+ control1=self.Controller1
|
|
|
+ control2=self.Controller2
|
|
|
+ if control1.robot_.timedRobotStatu_.statu.theta<0 :
|
|
|
+ control1 = self.Controller2
|
|
|
+ control2 = self.Controller1
|
|
|
+
|
|
|
+
|
|
|
+ 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
|
|
|
+ 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)
|
|
|
+
|
|
|
+ # # 开始系列子流程-----------------------------------------------------
|
|
|
+ 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])
|
|
|
+ self.Controller2.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)
|
|
|
+
|
|
|
+ # 双单车出库
|
|
|
+ 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)
|
|
|
+
|
|
|
+ # # 主车进入口
|
|
|
+ #
|
|
|
+ #
|
|
|
+ # node = mp.MapManager().GetVertex(label)# StreetNode
|
|
|
+ # self.Controller1.robot_.Navigatting(label, entrance_id, True, wheel_base)
|
|
|
+ # # 副车进入口
|
|
|
+ # cur_child_pose = self.Controller2.robot_.timedRobotStatu_.statu
|
|
|
+ # [label, pt] = mp.MapManager().findNeastNode([cur_child_pose.x, cur_child_pose.y])
|
|
|
+ # node = mp.MapManager().GetVertex(label)# StreetNode
|
|
|
+ # self.Controller2.robot_.Navigatting(label, entrance_id, True, wheel_base)
|
|
|
+ # # 切换整车模式
|
|
|
+ # self.Controller1.robot_.MainAgvchangecb()
|
|
|
+ # # 整车夹持
|
|
|
+ # self.Controller1.robot_.ClampClose()
|
|
|
+ # # 整车进目标车位
|
|
|
+ # self.Controller1.robot_.Navigatting(entrance_id, target_id, True, wheel_base)
|
|
|
+ # #
|
|
|
+ # # ---------------------------------------------------------------
|
|
|
+ # 恢复
|
|
|
+ # self.djks_map_.Reset()
|
|
|
+
|
|
|
+ def updateMap_pick(self, export_id, type):# 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
|
|
|
+ print("5555555")
|
|
|
+
|
|
|
+ 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 = self.Controller1
|
|
|
+
|
|
|
+ control1 = self.Controller1
|
|
|
+ control2 = self.Controller2
|
|
|
+ if control1.robot_.timedRobotStatu_.statu.theta < 0:
|
|
|
+ control1 = self.Controller2
|
|
|
+ control2 = self.Controller1
|
|
|
+
|
|
|
+
|
|
|
+ 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))
|
|
|
+
|
|
|
+ # self.updateMap_pick(export_id, 2)
|
|
|
+
|
|
|
+
|
|
|
+ # 双单车入库
|
|
|
+
|
|
|
+ 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)
|
|
|
+
|
|
|
+ # 双单车出库
|
|
|
+
|
|
|
+ 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")
|
|
|
@@ -151,14 +499,14 @@ class MainWindow(QMainWindow):
|
|
|
|
|
|
def Update(self):
|
|
|
self.gl.update()
|
|
|
- if self.isAutoDispatch:
|
|
|
- self.Controller1.setVisible(False)
|
|
|
- self.Controller2.setVisible(False)
|
|
|
+ if self.isAutoDispatchOnline:
|
|
|
+ # self.Controller1.setEnabled(False)
|
|
|
+ # self.Controller2.setEnabled(False)
|
|
|
+ pass
|
|
|
else:
|
|
|
self.Controller1.setVisible(True)
|
|
|
self.Controller2.setVisible(True)
|
|
|
|
|
|
-
|
|
|
# 窗口基础属性
|
|
|
def basic(self):
|
|
|
# 设置标题，大小，图标
|
|
|
@@ -172,6 +520,7 @@ class MainWindow(QMainWindow):
|
|
|
|
|
|
def closeEvent(self, QCloseEvent):
|
|
|
self.gl.close()
|
|
|
+ print("close...")
|
|
|
|
|
|
# 分割窗口
|
|
|
def split_(self):
|
