|
|
@@ -324,6 +324,41 @@ class MainWindow(QMainWindow):
|
|
|
print(" Park Failed ")
|
|
|
return False
|
|
|
|
|
|
+ # # ------------------------------------------
|
|
|
+ # # 双单车驶至入口前巷道点
|
|
|
+ # streetNode = entrance_id.replace("S_", "R_")
|
|
|
+ # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
|
|
|
+ # return False
|
|
|
+ # cur_pose1 = control1.robot_.timedRobotStatu_.statu
|
|
|
+ # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
|
|
|
+ # control1.robot_.GeneratePath(label, streetNode, frontMap)
|
|
|
+ # print("Front: begin:%s target:%s wheelBase:%f" % (label, streetNode, wheel_base))
|
|
|
+ #
|
|
|
+ # cur_pose2 = control2.robot_.timedRobotStatu_.statu
|
|
|
+ # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
|
|
|
+ # control2.robot_.GeneratePath(label, streetNode, backMap)
|
|
|
+ # print("Back: begin:%s target:%s wheelBase:%f" % (label, streetNode, wheel_base))
|
|
|
+ #
|
|
|
+ # autoDirect = True
|
|
|
+ # threadPool = ThreadPoolExecutor(2)
|
|
|
+ # features = [threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base),
|
|
|
+ # threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base)]
|
|
|
+ # if self.asyncExecute(features) == False:
|
|
|
+ # print(" Pick Failed ")
|
|
|
+ # return False
|
|
|
+ #
|
|
|
+ # # 整车驶进入口---------------------------------
|
|
|
+ # if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
|
|
|
+ # return False
|
|
|
+ #
|
|
|
+ # map_name = "Front"
|
|
|
+ # if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
|
|
|
+ # map_name = "Back"
|
|
|
+ # controlMain.robot_.GeneratePath(streetNode, entrance_id, map_name)
|
|
|
+ # if controlMain.robot_.Navigatting(streetNode, entrance_id, True, wheel_base) == False:
|
|
|
+ # return False
|
|
|
+ # # ---------------------------------
|
|
|
+
|
|
|
print(" input entrance completed!!!!")
|
|
|
if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
|
|
|
return False
|
|
|
@@ -420,6 +455,41 @@ class MainWindow(QMainWindow):
|
|
|
print(" Pick Failed ")
|
|
|
return False
|
|
|
|
|
|
+ # # ------------------------------------------
|
|
|
+ # # 双单车驶至车位前巷道点
|
|
|
+ # streetNode = space_id.replace("S_", "R_")
|
|
|
+ # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
|
|
|
+ # return False
|
|
|
+ # cur_pose1 = control1.robot_.timedRobotStatu_.statu
|
|
|
+ # [label, pt] = self.mapManager.findNeastNode(frontMap, [cur_pose1.x, cur_pose1.y])
|
|
|
+ # control1.robot_.GeneratePath(label, streetNode, frontMap)
|
|
|
+ # print("Front: begin:%s target:%s wheelBase:%f" % (label, streetNode, wheel_base))
|
|
|
+ #
|
|
|
+ # cur_pose2 = control2.robot_.timedRobotStatu_.statu
|
|
|
+ # [label, pt] = self.mapManager.findNeastNode(backMap, [cur_pose2.x, cur_pose2.y])
|
|
|
+ # control2.robot_.GeneratePath(label, streetNode, backMap)
|
|
|
+ # print("Back: begin:%s target:%s wheelBase:%f" % (label, streetNode, wheel_base))
|
|
|
+ #
|
|
|
+ # autoDirect = True
|
|
|
+ # threadPool = ThreadPoolExecutor(2)
|
|
|
+ # features = [threadPool.submit(control1.robot_.Navigatting, label, streetNode, autoDirect, wheel_base),
|
|
|
+ # threadPool.submit(control2.robot_.Navigatting, label, streetNode, autoDirect, wheel_base)]
|
|
|
+ # if self.asyncExecute(features) == False:
|
|
|
+ # print(" Pick Failed ")
|
|
|
+ # return False
|
|
|
+ #
|
|
|
+ # # 整车驶进车位---------------------------------
|
|
|
+ # if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
|
|
|
+ # return False
|
|
|
+ #
|
|
|
+ # map_name = "Front"
|
|
|
+ # if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
|
|
|
+ # map_name = "Back"
|
|
|
+ # controlMain.robot_.GeneratePath(streetNode, space_id, map_name)
|
|
|
+ # if controlMain.robot_.Navigatting(streetNode, space_id, True, wheel_base) == False:
|
|
|
+ # return False
|
|
|
+ # # ---------------------------------
|
|
|
+
|
|
|
# 整车夹持------------------------------------
|
|
|
print(" input space completed!!!!")
|
|
|
if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
|
