1.更新指令协议

2.FCB地图不添加OutLink点
3.获取RPC控制导航程序返回值（未测试）

RobotData.py

@@ -187,11 +187,13 @@ class Robot(MqttAsync):
 
     def Navigatting(self, begId, targetId, autoDirect, wheelbase=0, task_type="None"):
         if wheelbase < 2.4 or wheelbase > 3.2:
-            return print("wheelbase is out of range!\n")
+            print("wheelbase is out of range!\n")
+            return False
         print("nav")
 
         # self.ExecPathNodes(autoDirect)
-        self.ExecNavtask(autoDirect, wheelbase)
+        if self.ExecNavtask(autoDirect, wheelbase)==False:
+            return False
 
         if self.IsMainModeStatu():
             Count.TestCount().loadCountAdd()
@@ -306,9 +308,10 @@ class Robot(MqttAsync):
         print("client received: ", response)
         if not response.ret == 0:
             print("nav failed :%s" % (response.info))
+            return False
         else:
             print("nav completed !!!")
-        return True
+            return True
 
     def CreateNavPathNodesAction(self, path, autoDirect):
         # if path is not None and self.timedRobotStatu_.timeout() == False:

message.proto

@@ -32,7 +32,8 @@ message ToAgvCmd {
     float D1 = 12; //距目标点距离
     float Y1=13;    //前车小w
     float Y2=14;    //后车小w
-    int32 end = 15;
+    int32 CL =15;
+    int32 end = 16;
 }
 
 message Pose2d

test.py

@@ -55,6 +55,8 @@ from mytool.txt_helper.txt_operation import TXTOperation
 from custom_define import RobotName
 from mytool.RabbitMq_helper import async_communication as rabitmq
 
+import concurrent.futures
+
 
 # ===============================================================================
 
@@ -170,7 +172,7 @@ class MainWindow(QMainWindow):
             [id, point] = node
             street_node = mp.StreetNode(id, point[0], point[1])
             if street_node.id_.find(flag) >= 0:
-                if street_node.id_.find("OutLink")>=0:
+                if street_node.id_.find("OutLink") >= 0:
                     continue
                 self.mapManager.AddVertex(mapName, street_node)
 
@@ -185,6 +187,8 @@ class MainWindow(QMainWindow):
             for pt1 in points:
                 for pt2 in points:
                     if not pt1 == pt2:
+                        if pt1.find("OutLink") >= 0 or pt2.find("OutLink") >= 0:
+                            continue
                         if roadName.find(flag) >= 0:
                             self.mapManager.AddEdge(mapName, pt1, pt2)
 
@@ -221,7 +225,7 @@ class MainWindow(QMainWindow):
                 for pt2 in points:
                     if not pt1 == pt2:
                         self.mapManager.AddEdge("Base", pt1, pt2)
-                        if pt1.find("OutLink")>=0 or pt2.find("OutLink")>=0:
+                        if pt1.find("OutLink") >= 0 or pt2.find("OutLink") >= 0:
                             continue
                         if roadName.find("F_") >= 0:
                             self.mapManager.AddEdge("Front", pt1, pt2)
@@ -236,7 +240,7 @@ class MainWindow(QMainWindow):
         trans_x, trans_y, trans_a = pose
 
         # 加车位点
-        self.mapManager.maps[mapName].AddVertex(mp.SpaceNode("S1101", trans_x, trans_y,trans_a))
+        self.mapManager.maps[mapName].AddVertex(mp.SpaceNode("S1101", trans_x, trans_y, trans_a))
 
         entrance_street_nodes = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
         print("entrance_space pose: ", self.mapManager.maps[mapName].graph_.points[entrance_id])
@@ -249,7 +253,7 @@ class MainWindow(QMainWindow):
         if mapName == "Main":
             street_name = "C_InputLink"
             self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[1][0],
-                                                                entrance_street_nodes[1][1]))  # 更新库位点对应马路点
+                                                                  entrance_street_nodes[1][1]))  # 更新库位点对应马路点
         if mapName == "Back":
             street_name = "B_InputLink"
             self.mapManager.maps[mapName].AddVertex(mp.StreetNode(street_name, entrance_street_nodes[2][0],
@@ -266,45 +270,12 @@ class MainWindow(QMainWindow):
         print(mapName + " Map entrance_street pose ", self.mapManager.maps[mapName].graph_.points[street_name])
         self.gl.SetMap(mapName, self.mapManager.maps[mapName])
 
-    def GenerateSpecifiedMap(self, map_type):  # 0:前车地图，1：后车地图，2：整车地图
-        pass
-        '''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.mapManager.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.mapManager.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.mapManager.AddEdge(pt1, pt2)
-        }
-
-
-
-        if type == 0:
-            pass
-        elif type == 1:
-            pass
-        elif type == 2:
-            pass
-        else:
-            print("type of specified map is wrong!\n")'''
+    def asyncExecute(self, tasks):
+        results = [feature.result() for feature in concurrent.futures.as_completed(tasks)]
+        ret = True
+        for result in results:
+            ret = ret and result
+        return ret
 
     def GetMainFrontBackAGV(self):
         controlMain = self.Controller1
@@ -355,9 +326,11 @@ class MainWindow(QMainWindow):
         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)
+        features = [threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base),
+                    threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)]
+        if self.asyncExecute(features) == False:
+            print(" Park Failed ")
+            return False
 
         # # 双单车入库-----------------------------------------------------
         self.ReLoadMap(frontMap, "./map.json")
@@ -379,36 +352,29 @@ class MainWindow(QMainWindow):
         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)
+        features = [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)
+        if self.asyncExecute(features) == False:
+            print(" Park Failed ")
+            return False
 
         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)
+
+        controlMain.robot_.SwitchMoveMode(1, wheel_base)
+        controlMain.robot_.ClampClose()
 
         # 整车入库---------------------------------
-        map_name="Front"
-        if controlMain.robot_.timedRobotStatu_.statu.theta<0:
-            map_name="Back"
+        map_name = "Front"
+        if controlMain.robot_.timedRobotStatu_.statu.theta < 0:
+            map_name = "Back"
 
         self.ReLoadMap(map_name, "./map.json")
         self.updateMap_park(entrance_id, [trans_x, trans_y, trans_a], map_name)
         controlMain.robot_.GeneratePath(entrance_id, target_id, map_name)
         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)
+        controlMain.robot_.Navigatting(entrance_id, target_id, autoDirect, wheel_base)
 
         # 整车松夹池------------------------------------
         threadPool = ThreadPoolExecutor(1)
@@ -485,14 +451,12 @@ class MainWindow(QMainWindow):
         control1.WheelBaseEdit.setText(str(wheel_base))
         control2.WheelBaseEdit.setText(str(wheel_base))
 
-
         # 双单车入库
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.SwitchMoveMode, 0, wheel_base)
         threadPool.submit(control2.robot_.SwitchMoveMode, 0, wheel_base)
         threadPool.shutdown(wait=True)
 
-
         self.ReLoadMap(frontMap, "./map.json")
         self.updateMap_pick(export_id, frontMap)
         cur_pose1 = control1.robot_.timedRobotStatu_.statu
@@ -507,7 +471,6 @@ class MainWindow(QMainWindow):
         control2.robot_.GeneratePath(label, space_id, backMap)
         print("Child: begin:%s   target:%s    wheelBase:%f" % (label, space_id, wheel_base))
 
-
         autoDirect = True
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.Navigatting,
@@ -535,14 +498,11 @@ class MainWindow(QMainWindow):
         controlMain.robot_.GeneratePath(space_id, export_id, map_name)
         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)
@@ -562,7 +522,6 @@ class MainWindow(QMainWindow):
         self.updateMap_pick(export_id, backMap)
         control2.robot_.GeneratePath(export_id, "B_exportLink", backMap)
 
-
         threadPool = ThreadPoolExecutor(2)
         threadPool.submit(control1.robot_.Navigatting,
                           export_id, "F_exportLink", True, wheel_base)
@@ -576,7 +535,6 @@ class MainWindow(QMainWindow):
         print("Publish_park_response------------------\n", pick_table)
         return True
 
-
     def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
         """
 
@@ -655,17 +613,6 @@ if __name__ == "__main__":
     win = MainWindow()
     win.show()
     sys.exit(app.exec_())
-
-# ===============================================================================
-# Main
-# ===============================================================================
-
-'''app = QApplication(sys.argv)
-mainWindow = MapGLWidget()
-mainWindow.show()
-mainWindow.raise_() # Need this at least on OS X, otherwise the window ends up in background
-sys.exit(app.exec_())'''
-
 # ===============================================================================
 #
 # Local Variables: