test.py 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593
  1. #!/usr/bin/env python
  2. # -*- coding: utf-8 -*-
  3. # ===============================================================================
  4. #
  5. # test.py
  6. #
  7. # Test program for the simple GL Viewer.
  8. #
  9. # Copyright (c) 2011, Arne Schmitz <arne.schmitz@gmx.net>
  10. # All rights reserved.
  11. #
  12. # Redistribution and use in source and binary forms, with or without
  13. # modification, are permitted provided that the following conditions are met:
  14. # * Redistributions of source code must retain the above copyright
  15. # notice, this list of conditions and the following disclaimer.
  16. # * Redistributions in binary form must reproduce the above copyright
  17. # notice, this list of conditions and the following disclaimer in the
  18. # documentation and/or other materials provided with the distribution.
  19. # * Neither the name of the <organization> nor the
  20. # names of its contributors may be used to endorse or promote products
  21. # derived from this software without specific prior written permission.
  22. #
  23. # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  24. # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  25. # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  26. # DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
  27. # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  28. # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29. # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  30. # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  31. # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  32. # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33. #
  34. # ===============================================================================
  35. import math
  36. import threading
  37. import time
  38. import sys
  39. from PyQt5.QtGui import *
  40. from PyQt5.QtWidgets import *
  41. from PyQt5.QtCore import *
  42. from MapGLWidget import MapGLWidget
  43. import json
  44. import dijkstra.Map as mp
  45. import ControllWidget
  46. import JointContrallerWidget
  47. import ManualOperationWidget
  48. import RobotData as RD
  49. import message_pb2 as message
  50. import google.protobuf.json_format as jtf
  51. import google.protobuf.text_format as tf
  52. from concurrent.futures import ThreadPoolExecutor
  53. import uuid
  54. from mytool.txt_helper.txt_operation import TXTOperation
  55. from custom_define import RobotName
  56. from mytool.RabbitMq_helper import async_communication as rabitmq
  57. # ===============================================================================
  58. class MainWindow(QMainWindow):
  59. """docstring for Mainwindow"""
  60. djks_map_ = mp.MapManager()
  61. ui_nodes = {}
  62. ui_nodes["street_nodes"] = []
  63. ui_nodes["space_nodes"] = []
  64. def __init__(self, parent=None):
  65. super(MainWindow, self).__init__(parent)
  66. self.basic()
  67. self.count_frame_ = ControllWidget.CountFrame()
  68. self.LoadMap()
  69. self.isLocalTest = False
  70. self.isAutoDispatchOnline = True
  71. rpc_1 = "192.168.0.70:9090"
  72. mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
  73. rpc_2 = "192.168.0.71:9090"
  74. mqtt_2 = ["pyui2", "192.168.0.71", 1883, "admin", "zx123456"]
  75. if self.isLocalTest:
  76. rpc_1 = "127.0.0.1:9090"
  77. mqtt_1 = ["pyui-main", "127.0.0.1", 1883, "admin", "zx123456"]
  78. rpc_2 = "127.0.0.1:9091"
  79. mqtt_2 = ["pyui-child", "127.0.0.1", 1883, "admin", "zx123456"]
  80. config1 = {"label": RobotName.strAGVMain,
  81. "rpc": rpc_1,
  82. "street_nodes": self.ui_nodes["street_nodes"],
  83. "space_nodes": self.ui_nodes["space_nodes"],
  84. "mqtt": mqtt_1,
  85. "subTopics": {"agv_main/agv_statu": message.RobotStatu.__name__,
  86. "agv_main/nav_statu": message.NavStatu.__name__},
  87. "cmdTopic": "agv_main/nav_cmd",
  88. "robotColor": [0.7, 0.2, 0.3]}
  89. config2 = {"label": RobotName.strAGV2,
  90. "rpc": rpc_2,
  91. # "rpc": "127.0.0.1:9091",
  92. "street_nodes": self.ui_nodes["street_nodes"],
  93. "space_nodes": self.ui_nodes["space_nodes"],
  94. "mqtt": mqtt_2,
  95. "subTopics": {"agv_child/agv_statu": message.RobotStatu.__name__,
  96. "agv_child/nav_statu": message.NavStatu.__name__},
  97. "cmdTopic": "agv_child/nav_cmd",
  98. "robotColor": [0.4, 0.2, 0.8]}
  99. self.Controller1 = ControllWidget.ControlFrame(config1)
  100. self.Controller2 = ControllWidget.ControlFrame(config2)
  101. robot_dict = {self.Controller1.robot_.name_: self.Controller1.robot_,
  102. self.Controller2.robot_.name_: self.Controller2.robot_}
  103. self.ManualOperationWidget = ManualOperationWidget.ManualOperationWidget(robot_dict)
  104. splitter_main = self.split_()
  105. self.setCentralWidget(splitter_main)
  106. self.gl.SetRobot1Instance(self.Controller1.robot_)
  107. self.gl.SetRobot2Instance(self.Controller2.robot_)
  108. self.timer_ = QTimer()
  109. self.timer_.timeout.connect(self.Update)
  110. self.timer_.start(100)
  111. # 启动RabbitMQ
  112. self.g_rabbitmq = None
  113. if self.isAutoDispatchOnline:
  114. self.g_rabbitmq = rabitmq.RabbitAsyncCommunicator("192.168.0.201", 5672, "zx", "123456")
  115. calbbacks = [["agv_park_command_request_queue", self.recv_park_request],
  116. ["agv_pick_command_request_queue", self.recv_pick_request]]
  117. self.g_rabbitmq.Init(calbbacks, None)
  118. self.g_rabbitmq.setDaemon(True) # 守护线程随主线程结束
  119. self.g_rabbitmq.start()
  120. def recv_park_request(self, msg):
  121. print("Recv_park_request------------------\n", msg)
  122. # time.sleep(30)
  123. # # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port",
  124. # # tf.MessageToString(table, as_utf8=True))
  125. # self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
  126. # msg)
  127. # print("Publish_park_response------------------\n", msg)
  128. # return True
  129. park_table = message.park_table()
  130. try:
  131. tf.Parse(msg, park_table)
  132. except Exception as e:
  133. print("Parse error\n")
  134. if self.isAutoDispatchOnline:
  135. self.AutoParkTask(park_table)
  136. def recv_pick_request(self, msg):
  137. print("Recv_pick_request------------------\n", msg)
  138. pick_table = message.pick_table()
  139. # time.sleep(30)
  140. # self.g_rabbitmq.publish("command_ex", "user_pick_command_request_port",
  141. # tf.MessageToString(table, as_utf8=True))
  142. # self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
  143. # msg)
  144. # print("Publish_pick_response------------------\n", msg)
  145. # return True
  146. try:
  147. tf.Parse(msg, pick_table)
  148. except Exception as e:
  149. print("Parse error\n")
  150. if self.isAutoDispatchOnline:
  151. self.AutoPickTask(pick_table)
  152. def updateMap_park(self, entrance_id, pose, type): # 0:前车轨迹,1:后车轨迹,2:添加整车轨迹
  153. self.djks_map_.Reset() # 重置地图
  154. trans_x, trans_y, trans_a = pose
  155. self.djks_map_.AddVertex_t(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a)) # 更新库位点
  156. entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
  157. print("entrance_space pose: ", self.djks_map_.map_t.graph_.points[entrance_id])
  158. if type == 0:
  159. self.djks_map_.AddVertex_t(mp.StreetNode("FInput_R1101", entrance_street_nodes[0][0],
  160. entrance_street_nodes[0][1])) # 更新库位点对应马路点
  161. self.djks_map_.AddEdge_t(entrance_id, "FInput_R1101")
  162. # 加临时边
  163. for node_id, value in self.djks_map_.VertexDict().items():
  164. if node_id.find("FInput") >= 0:
  165. self.djks_map_.AddEdge_t("FInput_R1101", node_id)
  166. print("F entrance_street pose ", self.djks_map_.map_t.graph_.points["FInput_R1101"])
  167. elif type == 1:
  168. self.djks_map_.AddVertex_t(mp.StreetNode("BInput_R1101", entrance_street_nodes[2][0],
  169. entrance_street_nodes[2][1])) # 更新库位点对应马路点
  170. self.djks_map_.AddEdge_t(entrance_id, "BInput_R1101")
  171. # 加临时边
  172. for node_id, value in self.djks_map_.VertexDict().items():
  173. if node_id.find("BInput") >= 0:
  174. self.djks_map_.AddEdge_t("BInput_R1101", node_id)
  175. else:
  176. self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1101", entrance_street_nodes[1][0],
  177. entrance_street_nodes[1][1])) # 更新库位点对应马路点
  178. self.djks_map_.AddEdge_t(entrance_id, "CInput_R1101")
  179. for node_id, value in self.djks_map_.VertexDict().items():
  180. if node_id.find("CInput") >= 0:
  181. self.djks_map_.AddEdge_t("CInput_R1101", node_id)
  182. print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1101"])
  183. def GenerateSpecifiedMap(self, map_type): # 0:前车地图,1:后车地图,2:整车地图
  184. '''map = self.load_map("./map.json")
  185. {
  186. for node in map['street_nodes'].items():
  187. [id, point] = node
  188. street_node = mp.StreetNode(id, point[0], point[1])
  189. self.djks_map_.AddVertex(street_node)
  190. self.gl.AddNode([id, "street_node", point])
  191. self.ui_nodes["street_nodes"].append(id)
  192. for node in map['space_nodes'].items():
  193. [id, point] = node
  194. [x, y, yaw] = point
  195. space_node = mp.SpaceNode(id, point[0], point[1], yaw)
  196. self.djks_map_.AddVertex(space_node)
  197. glNode = [id, "space_node", [x, y]]
  198. self.gl.AddNode(glNode)
  199. self.ui_nodes["space_nodes"].append(id)
  200. for road in map['roads'].items():
  201. self.gl.AddRoad(road)
  202. [_, points] = road
  203. for pt1 in points:
  204. for pt2 in points:
  205. if not pt1 == pt2:
  206. self.djks_map_.AddEdge(pt1, pt2)
  207. }
  208. if type == 0:
  209. pass
  210. elif type == 1:
  211. pass
  212. elif type == 2:
  213. pass
  214. else:
  215. print("type of specified map is wrong!\n")'''
  216. def GetMainFrontBackAGV(self):
  217. controlMain = self.Controller1
  218. nagtive = False
  219. if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
  220. nagtive = True
  221. control1 = None
  222. control2 = None
  223. if nagtive == False:
  224. control1 = self.Controller1
  225. control2 = self.Controller2
  226. else:
  227. control2 = self.Controller1
  228. control1 = self.Controller2
  229. return [controlMain, control1, control2]
  230. def AutoParkTask(self, park_table: message.park_table = None):
  231. print("AutoParkTask:---------------------\n")
  232. [controlMain, control1, control2] = self.GetMainFrontBackAGV()
  233. entrance_id = "S" + str(park_table.terminal_id) # "S1101"
  234. entrance_x, entrance_y, entrance_theta = [park_table.entrance_measure_info.measure_info_to_plc_forward.cx,
  235. park_table.entrance_measure_info.measure_info_to_plc_forward.cy,
  236. (
  237. 90 + park_table.entrance_measure_info.measure_info_to_plc_forward.theta) / 180 * math.pi]
  238. # 变换到地图坐标系
  239. # [dx, dy, da] = [-0.223411843181, -0.643030941486, 178.9478 / 180 * math.pi]
  240. # trans_x = -0.99983137 * entrance_x - 0.01836309 * entrance_y + dx
  241. # trans_y = 0.01836309 * entrance_x - 0.99983137 * entrance_y + dy
  242. [dx, dy, da] = [-0.234040126204, -0.647539079189, 178.9302736990612 / 180 * math.pi]
  243. trans_x = -0.999825716019 * entrance_x - 0.018668506294 * entrance_y + dx
  244. trans_y = 0.018668506294 * entrance_x - 0.999825716019 * entrance_y + dy
  245. trans_a = entrance_theta + da - math.pi
  246. while trans_a < 0:
  247. trans_a += math.pi
  248. print("entrance:", entrance_id, trans_x, trans_y, trans_a / math.pi * 180)
  249. target_id = "S" + str(park_table.allocated_space_info.table_id) # "S147"
  250. print("target:", target_id)
  251. # 轴距
  252. wheel_base = park_table.entrance_measure_info.measure_info_to_plc_forward.wheelbase
  253. control1.WheelBaseEdit.setText(str(wheel_base))
  254. control2.WheelBaseEdit.setText(str(wheel_base))
  255. trans_x += wheel_base / 2 * math.cos(trans_a)
  256. trans_y += wheel_base / 2 * math.sin(trans_a)
  257. # # 双单车入库-----------------------------------------------------
  258. threadPool = ThreadPoolExecutor(2)
  259. threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
  260. threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
  261. threadPool.shutdown(wait=True)
  262. self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 0)
  263. cur_pose1 = control1.robot_.timedRobotStatu_.statu
  264. [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y])
  265. control1.robot_.GeneratePath(label, entrance_id)
  266. print("Main: begin:%s target:%s wheelBase:%f" % (label, entrance_id, wheel_base))
  267. # 后车 ------------------------------------------------------
  268. self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 1)
  269. cur_pose2 = control2.robot_.timedRobotStatu_.statu
  270. [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
  271. control2.robot_.GeneratePath(label, entrance_id)
  272. print("Child: begin:%s target:%s wheelBase:%f" % (label, entrance_id, wheel_base))
  273. autoDirect = True
  274. threadPool = ThreadPoolExecutor(2)
  275. threadPool.submit(control1.robot_.Navigatting,
  276. label, entrance_id, autoDirect, wheel_base)
  277. threadPool.submit(control2.robot_.Navigatting,
  278. label, entrance_id, autoDirect, wheel_base)
  279. threadPool.shutdown(wait=True)
  280. print(" input entrance completed!!!!")
  281. threadPool = ThreadPoolExecutor(1)
  282. threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
  283. threadPool.shutdown(wait=True)
  284. threadPool = ThreadPoolExecutor(1)
  285. threadPool.submit(controlMain.robot_.ClampClose)
  286. threadPool.shutdown(wait=True)
  287. # 整车入库---------------------------------
  288. self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], 2)
  289. controlMain.robot_.GeneratePath(entrance_id, target_id)
  290. print("begin:%s target:%s wheelBase:%f" % (entrance_id, target_id, wheel_base))
  291. threadPool = ThreadPoolExecutor(1)
  292. threadPool.submit(controlMain.robot_.Navigatting,
  293. entrance_id, target_id, autoDirect, wheel_base)
  294. threadPool.shutdown(wait=True)
  295. # 整车松夹池------------------------------------
  296. threadPool = ThreadPoolExecutor(1)
  297. threadPool.submit(controlMain.robot_.ClampOpen)
  298. threadPool.shutdown(wait=True)
  299. threadPool = ThreadPoolExecutor(1)
  300. threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
  301. threadPool.shutdown(wait=True)
  302. # 双单车出库
  303. controlMain, control1, control2 = self.GetMainFrontBackAGV()
  304. self.djks_map_.Reset() # 重置地图
  305. control1.robot_.GeneratePath(target_id, "FInput_R147")
  306. control2.robot_.GeneratePath(target_id, "BInput_R147")
  307. threadPool = ThreadPoolExecutor(2)
  308. threadPool.submit(control1.robot_.Navigatting,
  309. target_id, "FInput_R147", autoDirect, wheel_base)
  310. threadPool.submit(control2.robot_.Navigatting,
  311. target_id, "BInput_R147", autoDirect, wheel_base)
  312. threadPool.shutdown(wait=True)
  313. self.g_rabbitmq.publish()
  314. def updateMap_pick(self, export_id, type): # 0:前车轨迹,1:后车轨迹,2:添加整车轨迹
  315. self.djks_map_.Reset() # 重置地图
  316. # export_id = "S1103"
  317. # 添加出口车位点
  318. self.djks_map_.AddVertex_t(mp.SpaceNode(export_id, self.djks_map_.map_t.graph_.points["S1100"][0],
  319. self.djks_map_.map_t.graph_.points["S1100"][1], 90)) # 更新库位点对应马路点
  320. if type == 0:
  321. self.djks_map_.AddEdge_t(export_id, "FInput_R1100")
  322. elif type == 1:
  323. self.djks_map_.AddEdge_t(export_id, "BInput_R1100")
  324. elif type == 2:
  325. export_street_node_x = self.djks_map_.map_t.graph_.points["CInput_R1100"][0]
  326. export_street_node_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][1]
  327. self.djks_map_.AddVertex_t(mp.StreetNode("CInput_R1103", export_street_node_x,
  328. export_street_node_y)) # 更新库位点对应马路点
  329. self.djks_map_.AddEdge_t(export_id, "CInput_R1103")
  330. for node_id, value in self.djks_map_.VertexDict().items():
  331. if node_id.find("CInput") >= 0:
  332. self.djks_map_.AddEdge_t("CInput_R1103", node_id)
  333. print("C entrance_street pose ", self.djks_map_.map_t.graph_.points["CInput_R1103"])
  334. def AutoPickTask(self, pick_table: message.pick_table = None):
  335. print("AutoPickTask:---------------------\n")
  336. controlMain, control1, control2 = self.GetMainFrontBackAGV()
  337. self.djks_map_.Reset() # 重置地图
  338. space_id = "S" + str(pick_table.actually_space_info.table_id) # "S147"
  339. export_id = "S" + str(pick_table.terminal_id) # "S1103"
  340. print(space_id, export_id)
  341. # 轴距
  342. wheel_base = pick_table.actually_measure_info.measure_info_to_plc_forward.wheelbase
  343. control1.WheelBaseEdit.setText(str(wheel_base))
  344. control2.WheelBaseEdit.setText(str(wheel_base))
  345. # 双单车入库
  346. threadPool = ThreadPoolExecutor(2)
  347. threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
  348. threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
  349. threadPool.shutdown(wait=True)
  350. cur_pose1 = control1.robot_.timedRobotStatu_.statu
  351. [label, pt] = mp.MapManager().findNeastNode([cur_pose1.x, cur_pose1.y])
  352. control1.robot_.GeneratePath(label, space_id)
  353. print("Main: begin:%s target:%s wheelBase:%f" % (label, space_id, wheel_base))
  354. cur_pose2 = control2.robot_.timedRobotStatu_.statu
  355. [label, pt] = mp.MapManager().findNeastNode([cur_pose2.x, cur_pose2.y])
  356. control2.robot_.GeneratePath(label, space_id)
  357. print("Child: begin:%s target:%s wheelBase:%f" % (label, space_id, wheel_base))
  358. autoDirect = True
  359. threadPool = ThreadPoolExecutor(2)
  360. threadPool.submit(control1.robot_.Navigatting,
  361. label, space_id, autoDirect, wheel_base)
  362. threadPool.submit(control2.robot_.Navigatting,
  363. label, space_id, autoDirect, wheel_base)
  364. threadPool.shutdown(wait=True)
  365. # 整车夹持------------------------------------
  366. print(" input space completed!!!!")
  367. threadPool = ThreadPoolExecutor(1)
  368. threadPool.submit(controlMain.robot_.SwitchMoveMode, 1, wheel_base)
  369. threadPool.shutdown(wait=True)
  370. threadPool = ThreadPoolExecutor(1)
  371. threadPool.submit(controlMain.robot_.ClampClose)
  372. threadPool.shutdown(wait=True)
  373. # 整车到出口---------------------------------
  374. self.updateMap_pick(export_id, 2)
  375. controlMain.robot_.GeneratePath(space_id, export_id)
  376. print("Total: begin:%s target:%s wheelBase:%f" % (space_id, export_id, wheel_base))
  377. threadPool = ThreadPoolExecutor(1)
  378. threadPool.submit(controlMain.robot_.Navigatting,
  379. space_id, export_id, True, wheel_base)
  380. threadPool.shutdown(wait=True)
  381. # 整车松夹池------------------------------------
  382. threadPool = ThreadPoolExecutor(1)
  383. threadPool.submit(controlMain.robot_.ClampOpen)
  384. threadPool.shutdown(wait=True)
  385. threadPool = ThreadPoolExecutor(1)
  386. threadPool.submit(controlMain.robot_.SwitchMoveMode, 0, wheel_base)
  387. threadPool.shutdown(wait=True)
  388. # 双单车出库
  389. controlMain, control1, control2 = self.GetMainFrontBackAGV()
  390. self.updateMap_pick(export_id, 0)
  391. control1.robot_.GeneratePath(export_id, "FInput_R1100")
  392. self.updateMap_pick(export_id, 1)
  393. control2.robot_.GeneratePath(export_id, "BInput_R1100")
  394. threadPool = ThreadPoolExecutor(2)
  395. threadPool.submit(control1.robot_.Navigatting,
  396. export_id, "FInput_R1100", True, wheel_base)
  397. threadPool.submit(control2.robot_.Navigatting,
  398. export_id, "BInput_R1100", True, wheel_base)
  399. threadPool.shutdown(wait=True)
  400. def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
  401. """
  402. """
  403. n_x, n_y = 0, 0
  404. if s_id == "S1101":
  405. n_y = self.djks_map_.map_t.graph_.points["CInput_R1100"][1]
  406. k = math.tan(s_theta)
  407. n_x = (n_y - s_y) / k + s_x # 弧度
  408. n_yF = self.djks_map_.map_t.graph_.points["FInput_R1100"][1]
  409. n_xF = (n_yF - s_y) / k + s_x # 弧度
  410. n_yB = self.djks_map_.map_t.graph_.points["BInput_R1100"][1]
  411. n_xB = (n_yB - s_y) / k + s_x # 弧度
  412. # print(n_x, n_y)
  413. return [[n_xF, n_yF], [n_x, n_y], [n_xB, n_yB]]
  414. def LoadMap(self):
  415. self.gl = MapGLWidget() # 将opengl例子嵌入GUI
  416. map = self.load_map("./map.json")
  417. for node in map['street_nodes'].items():
  418. [id, point] = node
  419. street_node = mp.StreetNode(id, point[0], point[1])
  420. self.djks_map_.AddVertex(street_node)
  421. self.gl.AddNode([id, "street_node", point])
  422. self.ui_nodes["street_nodes"].append(id)
  423. for node in map['space_nodes'].items():
  424. [id, point] = node
  425. [x, y, yaw] = point
  426. space_node = mp.SpaceNode(id, point[0], point[1], yaw)
  427. self.djks_map_.AddVertex(space_node)
  428. glNode = [id, "space_node", [x, y]]
  429. self.gl.AddNode(glNode)
  430. self.ui_nodes["space_nodes"].append(id)
  431. for road in map['roads'].items():
  432. self.gl.AddRoad(road)
  433. [_, points] = road
  434. for pt1 in points:
  435. for pt2 in points:
  436. if not pt1 == pt2:
  437. self.djks_map_.AddEdge(pt1, pt2)
  438. def load_map(self, file):
  439. with open(file, 'r', encoding='utf-8') as fp:
  440. map = json.load(fp)
  441. return map
  442. def Update(self):
  443. self.gl.update()
  444. if self.isAutoDispatchOnline:
  445. # self.Controller1.setEnabled(False)
  446. # self.Controller2.setEnabled(False)
  447. pass
  448. else:
  449. self.Controller1.setVisible(True)
  450. self.Controller2.setVisible(True)
  451. # 窗口基础属性
  452. def basic(self):
  453. # 设置标题,大小,图标
  454. self.setWindowTitle("GT")
  455. self.resize(1100, 650)
  456. self.setWindowIcon(QIcon("./image/Gt.png"))
  457. # 居中显示
  458. screen = QDesktopWidget().geometry()
  459. self_size = self.geometry()
  460. self.move(int((screen.width() - self_size.width()) / 2), int((screen.height() - self_size.height()) / 2))
  461. def closeEvent(self, QCloseEvent):
  462. self.gl.close()
  463. print("close...")
  464. # 分割窗口
  465. def split_(self):
  466. splitter_main = QSplitter(Qt.Horizontal)
  467. splitter = QSplitter(Qt.Vertical)
  468. splitter.addWidget(self.count_frame_)
  469. splitter.addWidget(self.Controller1)
  470. splitter.addWidget(self.Controller2)
  471. splitter.addWidget(self.ManualOperationWidget)
  472. splitter.setStretchFactor(0, 1)
  473. splitter.setStretchFactor(1, 3)
  474. splitter.setStretchFactor(2, 3)
  475. splitter.setStretchFactor(3, 1)
  476. splitter_main.addWidget(splitter)
  477. splitter_main.addWidget(self.gl)
  478. splitter_main.setStretchFactor(0, 1)
  479. splitter_main.setStretchFactor(2, 4)
  480. return splitter_main
  481. if __name__ == "__main__":
  482. app = QApplication(sys.argv)
  483. win = MainWindow()
  484. win.show()
  485. sys.exit(app.exec_())
  486. # ===============================================================================
  487. # Main
  488. # ===============================================================================
  489. '''app = QApplication(sys.argv)
  490. mainWindow = MapGLWidget()
  491. mainWindow.show()
  492. mainWindow.raise_() # Need this at least on OS X, otherwise the window ends up in background
  493. sys.exit(app.exec_())'''
  494. # ===============================================================================
  495. #
  496. # Local Variables:
  497. # mode: Python
  498. # indent-tabs-mode: nil
  499. # End:
  500. #
  501. # ===============================================================================