1.放宽轴距限制到4.0m

2.整车空载驶进入口前，优化两单车起点位置

RobotData.py

@@ -189,7 +189,7 @@ class Robot(MqttAsync):
     '''
 
     def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type: int = 0):
-        if wheelbase < 2.4 or wheelbase > 3.2:
+        if wheelbase < 2.4 or wheelbase > 4:
             print("wheelbase is out of range!\n")
             return False
         print("nav")
@@ -199,7 +199,7 @@ class Robot(MqttAsync):
         if self.ExecNavtask(autoDirect, wheelbase) == False:
             self.ResetSequence()
             return False
-        self.ResetSequence()
+        # self.ResetSequence()
 
         if self.IsMainModeStatu():
             Count.TestCount().loadCountAdd()
@@ -272,6 +272,9 @@ class Robot(MqttAsync):
         cmd.action = 0
         cmd.sequence = self.current_sequence_
 
+        if len(self.currentNavPathNodes_)==1:
+            self.currentNavPathNodes_.append(self.currentNavPathNodes_[0])
+
         actions = self.SplitPath(self.currentNavPathNodes_)
         for action in actions:
             [type, nodes] = action

test.py

@@ -77,8 +77,8 @@ class MainWindow(QMainWindow):
         self.basic()
         self.showMaximized()
         self.count_frame_ = ControllWidget.CountFrame()
-        self.isLocalTest = True
-        self.isAutoDispatchOnline = False
+        self.isLocalTest = False
+        self.isAutoDispatchOnline = True
 
         self.Cancel_ = False
 
@@ -195,19 +195,27 @@ class MainWindow(QMainWindow):
         return False
 
     def updateMap_park(self, entrance_id, pose, wheelbase):  # 0:前车轨迹，1：后车轨迹，2：添加整车轨迹
+        # 修改Front地图
         self.mapManager.ResetMap("Front", 0, wheelbase / 2)
         [trans_x, trans_y, trans_a] = pose
         self.mapManager.maps["Front"].ResetVertexValue(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))
 
-        [id, x, y] = self.ComputeStreetNode("Front", entrance_id, trans_x, trans_y, trans_a)
-        self.mapManager.maps["Front"].ResetVertexValue(mp.StreetNode(id, x, y))
+        [f_id, f_x, f_y] = self.ComputeStreetNode("Front", entrance_id, trans_x, trans_y, trans_a)
 
+        # 修改Back地图
         self.mapManager.ResetMap("Back", 0, -wheelbase / 2)
         [trans_x, trans_y, trans_a] = pose
         self.mapManager.maps["Back"].ResetVertexValue(mp.SpaceNode(entrance_id, trans_x, trans_y, trans_a))
 
-        [id, x, y] = self.ComputeStreetNode("Back", entrance_id, trans_x, trans_y, trans_a)
-        self.mapManager.maps["Back"].ResetVertexValue(mp.StreetNode(id, x, y))
+        [b_id, b_x, b_y] = self.ComputeStreetNode("Back", entrance_id, trans_x, trans_y, trans_a)
+
+        # 修正两个地图巷道点
+        fff = False # 是否是整车空载进 出入口
+        if fff == True:
+            f_x = (f_x + b_x) / 2
+            b_x = (f_x + b_x) / 2
+        self.mapManager.maps["Front"].ResetVertexValue(mp.StreetNode(f_id, f_x, f_y))
+        self.mapManager.maps["Back"].ResetVertexValue(mp.StreetNode(b_id, b_x, b_y))
 
     def asyncExecute(self, tasks):
         results = [feature.result() for feature in concurrent.futures.as_completed(tasks)]
@@ -269,7 +277,8 @@ class MainWindow(QMainWindow):
         return (ret != None)
 
     def AutoParkTask(self, park_table: message.park_table = None):
-        print("AutoParkTask:---------------------\n")
+        beg_time = time.time()
+        print(beg_time, "AutoParkTask:---------------------\n")
         task_flag = 1  # 存车
 
         [controlMain, control1, control2] = self.GetMainFrontBackAGV()
@@ -280,9 +289,14 @@ class MainWindow(QMainWindow):
                                                   (
                                                           90 + park_table.entrance_measure_info.measure_info_to_plc_forward.theta) / 180 * math.pi]
         # 变换到地图坐标系
-        [dx, dy, da] = [-0.184040126204, -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
+        # [dx, dy, da] = [-0.184040126204, -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
+
+        [dx, dy, da] = [-0.184040126204, -0.647539079189, 178.86625855601127 / 180 * math.pi]
+        trans_x = -0.9998042333928334 * entrance_x - 0.01978622980177775 * entrance_y + dx
+        trans_y = 0.01978622980177775 * entrance_x - 0.9998042333928334 * entrance_y + dy
+
         trans_a = entrance_theta + da - math.pi
         while trans_a < 0:
             trans_a += math.pi
@@ -299,7 +313,7 @@ class MainWindow(QMainWindow):
         trans_x += wheel_base / 2 * math.cos(trans_a)
         trans_y += wheel_base / 2 * math.sin(trans_a)
 
-        # # # 双单车入库-----------------------------------------------------
+        # # 双单车入库-----------------------------------------------------
         # if (controlMain.robot_.SwitchMoveMode(0, wheel_base) == False):
         #     return False
         # autoDirect = True
@@ -320,7 +334,6 @@ class MainWindow(QMainWindow):
         #     threadPool.submit(control1.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag),
         #     threadPool.submit(control2.robot_.Navigatting, label, entrance_id, autoDirect, wheel_base, task_flag)
         # ]
-        #
         # if self.asyncExecute(features) == False:
         #     print(" Park Failed ")
         #     return False
@@ -350,24 +363,25 @@ class MainWindow(QMainWindow):
         if self.asyncExecute(features) == False:
             print(" Park 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(entrance_streetNode, entrance_id, map_name)
         if controlMain.robot_.Navigatting(entrance_streetNode, entrance_id, True, wheel_base, task_flag) == False:
             return False
-        # # ---------------------------------
+        # ---------------------------------
+
 
         print(" input entrance completed!!!!")
+        # 整车夹持
         if (controlMain.robot_.SwitchMoveMode(1, wheel_base) == False):
             return False
         controlMain.robot_.ClampClose()
 
+
         # 整车去车位---------------------------------
         # map_name = "Front"
         # if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
@@ -377,6 +391,7 @@ class MainWindow(QMainWindow):
         # controlMain.robot_.Navigatting(entrance_id, target_id, autoDirect, wheel_base, task_flag)
 
         # 整车驶离入口到入口航道点
+        entrance_streetNode = entrance_id.replace("S_", "R_")
         map_name = "Front"
         if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
             map_name = "Back"
@@ -394,7 +409,7 @@ class MainWindow(QMainWindow):
 
 
         print("-------------------------")
-        # 整车松夹池------------------------------------
+        # 整车松夹持------------------------------------
         if controlMain.robot_.ClampOpen() == False:
             return False
 
@@ -433,7 +448,8 @@ class MainWindow(QMainWindow):
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_park_command_response_port",
                                 tf.MessageToString(park_table, as_utf8=True))
-        print("Publish_park_response------------------\n", park_table)
+        end_time = time.time()
+        print(end_time, "Park time=", end_time - beg_time, "Publish_park_response------------------\n", park_table)
         return True
 
     def updateMap_pick(self, wheelBase):  # 轴距
@@ -441,7 +457,8 @@ class MainWindow(QMainWindow):
         self.mapManager.ResetMap("Back", 0, -wheelBase / 2)
 
     def AutoPickTask(self, pick_table: message.pick_table = None):
-        print("AutoPickTask:---------------------\n")
+        beg_time = time.time()
+        print(beg_time,"AutoPickTask:---------------------\n")
         task_flag = 2  # 取车
 
         controlMain, control1, control2 = self.GetMainFrontBackAGV()
@@ -478,7 +495,7 @@ class MainWindow(QMainWindow):
         #     print(" Pick Failed ")
         #     return False
 
-        # # ------------------------------------------
+        # ------------------------------------------
         # # 双单车驶至车位前巷道点
         self.updateMap_pick(wheel_base)
         space_streetNode = space_id.replace("S_", "R_")
@@ -514,7 +531,7 @@ class MainWindow(QMainWindow):
         controlMain.robot_.GeneratePath(space_streetNode, space_id, map_name)
         if controlMain.robot_.Navigatting(space_streetNode, space_id, True, wheel_base, task_flag) == False:
             return False
-        # # ---------------------------------
+        # ---------------------------------
 
         # 整车夹持------------------------------------
         print(" input space completed!!!!")
@@ -574,7 +591,8 @@ class MainWindow(QMainWindow):
         # 反馈
         self.g_rabbitmq.publish("command_ex", "agv_pick_command_response_port",
                                 tf.MessageToString(pick_table, as_utf8=True))
-        print("Publish_park_response------------------\n", pick_table)
+        end_time = time.time()
+        print(end_time, "Pick time=", end_time - beg_time, "Publish_pick_response------------------\n", pick_table)
         return True
 
     def ComputeStreetNode(self, mapName, s_id, s_x, s_y, s_theta):