|
|
@@ -55,21 +55,6 @@ 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"
|
|
|
- ]
|
|
|
-'''
|
|
|
-
|
|
|
|
|
|
# ===============================================================================
|
|
|
|
|
|
@@ -147,25 +132,37 @@ class MainWindow(QMainWindow):
|
|
|
|
|
|
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")
|
|
|
- # 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()
|
|
|
+ # 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")
|
|
|
- # split_msg = msg.split(' ')
|
|
|
- # result = [float(split_msg[0]), float(split_msg[1]), float(split_msg[2])]
|
|
|
if self.isAutoDispatchOnline:
|
|
|
self.AutoPickTask(pick_table)
|
|
|
|
|
|
@@ -206,16 +203,64 @@ class MainWindow(QMainWindow):
|
|
|
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
|
|
|
+ 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)
|
|
|
+ }
|
|
|
|
|
|
- control1=self.Controller1
|
|
|
- control2=self.Controller2
|
|
|
- if control1.robot_.timedRobotStatu_.statu.theta<0 :
|
|
|
- control1 = self.Controller2
|
|
|
+
|
|
|
+
|
|
|
+ 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,
|
|
|
@@ -223,9 +268,12 @@ class MainWindow(QMainWindow):
|
|
|
(
|
|
|
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.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
|
|
|
@@ -242,7 +290,12 @@ class MainWindow(QMainWindow):
|
|
|
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
|
|
|
@@ -255,7 +308,7 @@ class MainWindow(QMainWindow):
|
|
|
|
|
|
cur_pose2 = control2.robot_.timedRobotStatu_.statu
|
|
|
[label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
|
|
|
- self.Controller2.robot_.GeneratePath(label, entrance_id)
|
|
|
+ control2.robot_.GeneratePath(label, entrance_id)
|
|
|
print("Child: begin:%s target:%s wheelBase:%f" % (label, entrance_id, wheel_base))
|
|
|
|
|
|
autoDirect = True
|
|
|
@@ -293,6 +346,7 @@ class MainWindow(QMainWindow):
|
|
|
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")
|
|
|
@@ -305,38 +359,18 @@ class MainWindow(QMainWindow):
|
|
|
|
|
|
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.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_.map_t.graph_.points["S1100"][1], 90)) # 更新库位点对应马路点
|
|
|
+ if type == 0:
|
|
|
self.djks_map_.AddEdge_t(export_id, "FInput_R1100")
|
|
|
- elif type==1:
|
|
|
+ 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]
|
|
|
@@ -351,20 +385,10 @@ class MainWindow(QMainWindow):
|
|
|
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
|
|
|
-
|
|
|
+ controlMain, control1, control2 = self.GetMainFrontBackAGV()
|
|
|
|
|
|
self.djks_map_.Reset() # 重置地图
|
|
|
space_id = "S" + str(pick_table.actually_space_info.table_id) # "S147"
|
|
|
@@ -376,13 +400,14 @@ class MainWindow(QMainWindow):
|
|
|
control1.WheelBaseEdit.setText(str(wheel_base))
|
|
|
control2.WheelBaseEdit.setText(str(wheel_base))
|
|
|
|
|
|
- # self.updateMap_pick(export_id, 2)
|
|
|
-
|
|
|
-
|
|
|
# 双单车入库
|
|
|
+ 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))
|
|
|
@@ -399,8 +424,6 @@ class MainWindow(QMainWindow):
|
|
|
threadPool.submit(control2.robot_.Navigatting,
|
|
|
label, space_id, autoDirect, wheel_base)
|
|
|
threadPool.shutdown(wait=True)
|
|
|
-
|
|
|
-
|
|
|
|
|
|
# 整车夹持------------------------------------
|
|
|
print(" input space completed!!!!")
|
|
|
@@ -410,17 +433,15 @@ class MainWindow(QMainWindow):
|
|
|
threadPool = ThreadPoolExecutor(1)
|
|
|
threadPool.submit(controlMain.robot_.ClampClose)
|
|
|
threadPool.shutdown(wait=True)
|
|
|
-
|
|
|
|
|
|
# 整车到出口---------------------------------
|
|
|
- self.updateMap_pick(export_id,2)
|
|
|
+ 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)
|
|
|
-
|
|
|
|
|
|
# 整车松夹池------------------------------------
|
|
|
|
|
|
@@ -432,7 +453,7 @@ class MainWindow(QMainWindow):
|
|
|
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)
|
