1.Graph()添加重置点方法

2.Map()添加地图备份，并修改地图使用调用
3.手动操作做斜坡，并更新通讯协议
4.与入口坐标系做标定参数,[dx, dy, da] = [-0.223411843181, -0.643030941486, 178.9478 / 180 * math.pi]
new_x = -0.99983137 * entrance_x - 0.01836309 * entrance_y

ControllWidget.py

@@ -147,7 +147,7 @@ class ControlFrame(QFrame):
             yaw = self.robot_.timedRobotStatu_.statu.theta
             self.posestatic.setText("x : %.3f\ny : %.3f\nΘ : %.2f°" % (x, y, yaw * 180 / math.pi))
             if self.btnAutoBegChanged.checkState()==Qt.Checked:
-                [label,pt]=mp.DijikstraMap().findNeastNode([x,y])
+                [label,pt]=mp.MapManager().findNeastNode([x,y])
                 self.begQc.setCurrentText(label)
 
         statu = "min-width: 32px; min-height: 32px;max-width:32px;\
@@ -172,8 +172,8 @@ class ControlFrame(QFrame):
             self.btnLifterCheck.setEnabled(True)
         self.LedLabel.setStyleSheet(statu)
 
-        self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
-        self.WheelBaseEdit.setVisible(self.robot_.IsMainAgv())
+        # self.wheelbasestatic.setVisible(self.robot_.IsMainAgv())
+        # self.WheelBaseEdit.setVisible(self.robot_.IsMainAgv())
         self.btnModelCheck.setVisible(self.robot_.IsMainAgv())
         if self.robot_.IsMainModeStatu():
             self.btnModelCheck.setCheckState(Qt.Checked)
@@ -193,7 +193,7 @@ class ControlFrame(QFrame):
         if self.robot_.IsNavigating():
             self.begId_ = self.robot_.begId_
             self.targetId_ = self.robot_.targetId_
-            djks_map = mp.DijikstraMap()
+            djks_map = mp.MapManager()
             beg_node = djks_map.GetVertex(self.begId_)
             end_node = djks_map.GetVertex(self.targetId_)
             if beg_node is not None:
@@ -262,7 +262,7 @@ class ControlFrame(QFrame):
                 print("自由方向")
                 autoDirect=True
             self.threadPool_.submit(self.robot_.Navigatting,
-                                    self.begId_, self.targetId_,autoDirect,float(self.WheelBaseEdit.text()))
+                                    self.begId_, self.targetId_, autoDirect, float(self.WheelBaseEdit.text()))
 
     def btnCancelClick(self):
         self.threadPool_.submit(self.robot_.CancelNavTask)

ManualOperationWidget.py

@@ -170,7 +170,7 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
             speed = 0.0
             if self.speed_lineEdit_check():
                 speed = float(self.speed_lineEdit.text())
-            speed = min(speed, 3)
+            speed = min(speed, 30)
             self.speed_lineEdit.setText(str(speed))
             self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, self.step_acc, speed=speed)
             self.step_acc += 1
@@ -189,6 +189,8 @@ class ManualOperationWidget(QFrame, Ui_manual_operation_widget):
             self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation)
         elif self.current_operation == ManualOperationType.eReady and \
                 self.operation_count[ManualOperationType.eReady] != 0:
+            speed = float(self.speed_lineEdit.text())
+            self.robot_dict[self.selected_robot_name].ManualTask(self.current_operation, speed=speed)
             # self.threadPool_.shutdown(False)
             self.threadPool_.submit(self.robot_dict[self.selected_robot_name].CancelNavTask)
             self.current_operation = ManualOperationType.eReady

RobotData.py

@@ -1,6 +1,9 @@
 import copy
 import enum
 import time
+
+from numpy import fabs
+
 import message_pb2 as message
 import grpc
 import message_pb2_grpc as msg_rpc
@@ -9,7 +12,7 @@ from custom_define import RobotName, ActType
 from mqtt_async import MqttAsync
 import google.protobuf.json_format as jtf
 import json
-from dijkstra.Map import DijikstraMap, SpaceNode, StreetNode
+from dijkstra.Map import SpaceNode, StreetNode, MapManager, DijikstraMap
 import uuid
 import random
 import math
@@ -62,6 +65,7 @@ class Robot(MqttAsync):
         self.heat_ = 0  # 心跳
 
         self.manual_status = ManualOperationType.eReady  # 正在进行的手动操作
+        self.manual_speed = [0.0, 0]
 
     def Color(self):
         if self.IsMainModeStatu():
@@ -148,18 +152,18 @@ class Robot(MqttAsync):
         return True'''
 
     def GeneratePath(self, begID, endID, task_type="None"):
-        self.currentNavPathNodes_ = DijikstraMap().GetShortestPath(begID, endID)
-
-        if task_type == "DoubleAGV":
-            print("AdjustPath------------------")
-            if self.IsMainModeStatu() is False:
-                print("原始路径：")
-                for node in self.currentNavPathNodes_:
-                    print(node.id_, node.y_)
-                self.currentNavPathNodes_ = self.AdjustPath(self.currentNavPathNodes_)
-                print("修改路径：")
-                for node in self.currentNavPathNodes_:
-                    print(node.id_, node.y_)
+        self.currentNavPathNodes_ = MapManager().GetShortestPath(begID, endID)
+
+        # if task_type == "DoubleAGV":
+        #     print("AdjustPath------------------")
+        #     if self.IsMainModeStatu() is False:
+        #         print("原始路径：")
+        #         for node in self.currentNavPathNodes_:
+        #             print(node.id_, node.y_)
+        #         self.currentNavPathNodes_ = self.AdjustPath(self.currentNavPathNodes_)
+        #         print("修改路径：")
+        #         for node in self.currentNavPathNodes_:
+        #             print(node.id_, node.y_)
 
         self.currentNavPath_ = DijikstraMap().CreatePath(self.currentNavPathNodes_, 0.5)
         if self.currentNavPathNodes_ is not None and self.currentNavPath_ is not None:
@@ -173,7 +177,7 @@ class Robot(MqttAsync):
             return None
         x = self.timedRobotStatu_.statu.x
         y = self.timedRobotStatu_.statu.y
-        djks_map = DijikstraMap()
+        djks_map = MapManager()
         [label, pt] = djks_map.findNeastNode([x, y])
         return [label, pt]
 
@@ -207,7 +211,7 @@ class Robot(MqttAsync):
                 [label, pt] = posInfo
                 beg = label
                 print("beg", beg)
-                node = DijikstraMap().GetVertex(beg)
+                node = MapManager().GetVertex(beg)
 
                 if node is not None:
                     if isinstance(node, (SpaceNode)):
@@ -302,7 +306,6 @@ class Robot(MqttAsync):
             print("nav failed :%s" % (response.info))
         else:
             print("nav completed !!!")
-
         return True
 
     def CreateNavPathNodesAction(self, path, autoDirect):
@@ -598,51 +601,66 @@ class Robot(MqttAsync):
         return self.manual_status != ManualOperationType.eReady
 
     def ManualTask(self, current_operation, step_acc=0, speed=0):
-        print("speed: ", speed)
         if self.IsNavigating():
             print("AGV is navigating!!!")
             return
-        if self.IsManualTaskRunning():
-            return print("ManualTask | ManualTask is running...")
+
+        # slowly stop
+        if current_operation == ManualOperationType.eReady and \
+                ManualOperationType.eReady < self.manual_status <= ManualOperationType.eRightMove:
+            print("Manual_Unit_SlowlyStop")
+            self.Manual_Unit_SlowlyStop(current_operation, speed)
+            return True
+
+        # if self.IsManualTaskRunning():
+        #     return print("ManualTask | ManualTask is running...")
+
         self.manual_status = current_operation
 
         if ManualOperationType.eReady < current_operation <= ManualOperationType.eRightMove:
             cmd = message.ToAgvCmd()
             self.heat_ += 1
             self.heat_ %= 255
-            cmd.H = self.heat_
+            cmd.H1 = self.heat_
             if self.IsMainModeStatu():
-                cmd.M = 1
+                cmd.M1 = 1
             else:
-                cmd.M = 0
+                cmd.M1 = 0
 
             if current_operation == ManualOperationType.eCounterclockwiseRotate:
-                cmd.T = 1
-                cmd.V = 0
-                cmd.W = speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
+                cmd.T1 = 1
+                cmd.V1 = 0
+                cmd.W1 = speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
             if current_operation == ManualOperationType.eClockwiseRotate:
-                cmd.T = 1
-                cmd.V = 0
-                cmd.W = -speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
+                cmd.T1 = 1
+                cmd.V1 = 0
+                cmd.W1 = -speed / 180 * math.pi * sigmoid(step_acc, 0, 1)
             if current_operation == ManualOperationType.eForwardMove:
-                cmd.T = 3
-                cmd.V = speed * sigmoid(step_acc, 0, 1)
-                cmd.W = 0
+                cmd.T1 = 3
+                cmd.V1= speed * sigmoid(step_acc, 0, 1)
+                cmd.W1 = 0
             if current_operation == ManualOperationType.eBackwardMove:
-                cmd.T = 3
-                cmd.V = -speed * sigmoid(step_acc, 0, 1)
-                cmd.W = 0
+                cmd.T1 = 3
+                cmd.V1 = -speed * sigmoid(step_acc, 0, 1)
+                cmd.W1 = 0
             if current_operation == ManualOperationType.eLeftMove:
-                cmd.T = 2
-                cmd.V = speed * sigmoid(step_acc, 0, 1)
-                cmd.W = 0
+                cmd.T1 = 2
+                cmd.V1 = speed * sigmoid(step_acc, 0, 1)
+                cmd.W1 = 0
             if current_operation == ManualOperationType.eRightMove:
-                cmd.T = 2
-                cmd.V = -speed * sigmoid(step_acc, 0, 1)
-                cmd.W = 0
-            cmd.L = self.L_
+                cmd.T1 = 2
+                cmd.V1 = -speed * sigmoid(step_acc, 0, 1)
+                cmd.W1 = 0
+            cmd.V2 = cmd.V1
+            cmd.V3 = cmd.V1
+            cmd.W2 = cmd.W1
+            cmd.W3 = cmd.W1
+
+            cmd.L1 = self.L_
+            cmd.P1 = 0
+            cmd.D1 = 0.0
             cmd.end = 1
-
+            self.manual_speed = [cmd.V1, cmd.W1]
             # 处理为json格式
             cmd_dict = {}
             for field in cmd.DESCRIPTOR.fields:
@@ -658,8 +676,9 @@ class Robot(MqttAsync):
             # stub = msg_rpc.NavExcutorStub(channel)
             # response = stub.ManualOperation(cmd_0)
             # print("client received: ", response)
-            self.manual_status = ManualOperationType.eReady
+            # self.manual_status = ManualOperationType.eReady
             # print("ManualOperation {op_type} task completed!!!".format(op_type=hex(current_operation)))
+            time.sleep(0.05)
             return True
 
         elif ManualOperationType.eClampClose <= current_operation <= ManualOperationType.eClampOpen:
@@ -669,6 +688,7 @@ class Robot(MqttAsync):
         elif ManualOperationType.eLifterRise <= current_operation <= ManualOperationType.eLifterDown:
             self.Manual_Unit_Lifter(current_operation)
             return True
+
         return False
 
     def Manual_Unit_Lifter(self, current_operation: ManualOperationType):
@@ -676,12 +696,12 @@ class Robot(MqttAsync):
         cmd = message.ToAgvCmd()
         self.heat_ += 1
         self.heat_ %= 255
-        cmd.H = self.heat_
+        cmd.H1 = self.heat_
         if self.IsMainModeStatu():
-            cmd.M = 1
+            cmd.M1 = 1
         else:
-            cmd.M = 0
-        cmd.T = self.ManualOperationType2AgvCmdType(current_operation)
+            cmd.M1 = 0
+        cmd.T1 = self.ManualOperationType2AgvCmdType(current_operation)
 
         # 处理为json格式  # jtf.MessageToJson(cmd)
         cmd_dict = {}
@@ -692,7 +712,7 @@ class Robot(MqttAsync):
         cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
         self.publish(self.speedCmdTopic, cmd_json)
         print(cmd_json)
-        self.manual_status = ManualOperationType.eReady
+        # self.manual_status = ManualOperationType.eReady
         return True
 
     def Manual_Unit_Clamp(self, current_operation: ManualOperationType):
@@ -700,12 +720,12 @@ class Robot(MqttAsync):
         cmd = message.ToAgvCmd()
         self.heat_ += 1
         self.heat_ %= 255
-        cmd.H = self.heat_
+        cmd.H1 = self.heat_
         if self.IsMainModeStatu():
-            cmd.M = 1
+            cmd.M1 = 1
         else:
-            cmd.M = 0
-        cmd.T = self.ManualOperationType2AgvCmdType(current_operation)
+            cmd.M1 = 0
+        cmd.T1 = self.ManualOperationType2AgvCmdType(current_operation)
 
         # 处理为json格式  # jtf.MessageToJson(cmd)
         cmd_dict = {}
@@ -716,7 +736,7 @@ class Robot(MqttAsync):
         cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
         self.publish(self.speedCmdTopic, cmd_json)
         print(cmd_json)
-        self.manual_status = ManualOperationType.eReady
+        # self.manual_status = ManualOperationType.eReady
         return True
 
     def ManualOperationType2AgvCmdType(self, mo_type: ManualOperationType):
@@ -728,3 +748,74 @@ class Robot(MqttAsync):
             return ActType.eClampClose
         if mo_type == ManualOperationType.eClampOpen:
             return ActType.eClampOpen
+
+    def Manual_Unit_SlowlyStop(self, current_operation: ManualOperationType, speed):
+        step_sum = 10
+        step_acc = 1
+        speed_v = max(fabs(self.manual_speed[0]), 0.01)
+        speed_w = max(fabs(self.manual_speed[1]), 1)
+        while step_acc <= step_sum:
+            if ManualOperationType.eReady < self.manual_status <= ManualOperationType.eRightMove:
+                cmd = message.ToAgvCmd()
+                self.heat_ += 1
+                self.heat_ %= 255
+                cmd.H1 = self.heat_
+                if self.IsMainModeStatu():
+                    cmd.M1 = 1
+                else:
+                    cmd.M1 = 0
+
+                if self.manual_status == ManualOperationType.eCounterclockwiseRotate:
+                    cmd.T1 = 1
+                    cmd.V1 = 0
+                    cmd.W1 = speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                if self.manual_status == ManualOperationType.eClockwiseRotate:
+                    cmd.T1 = 1
+                    cmd.V1 = 0
+                    cmd.W1 = -speed_w / 180 * math.pi * (step_sum-step_acc) / step_sum
+                if self.manual_status == ManualOperationType.eForwardMove:
+                    cmd.T1 = 3
+                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.W1 = 0
+                if self.manual_status == ManualOperationType.eBackwardMove:
+                    cmd.T1 = 3
+                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.W1 = 0
+                if self.manual_status == ManualOperationType.eLeftMove:
+                    cmd.T1 = 2
+                    cmd.V1 = speed_v * (step_sum-step_acc) / step_sum
+                    cmd.W1 = 0
+                if self.manual_status == ManualOperationType.eRightMove:
+                    cmd.T1 = 2
+                    cmd.V1 = -speed_v * (step_sum-step_acc) / step_sum
+                    cmd.W1 = 0
+                cmd.V2 = cmd.V1
+                cmd.V3 = cmd.V1
+                cmd.W2 = cmd.W1
+                cmd.W3 = cmd.W1
+                cmd.L1 = self.L_
+                cmd.end = 1
+
+                # 处理为json格式
+                cmd_dict = {}
+                for field in cmd.DESCRIPTOR.fields:
+                    cmd_dict[field.name] = field.default_value
+                for field, value in cmd.ListFields():
+                    cmd_dict[field.name] = value
+                cmd_json = json.dumps(cmd_dict, indent=None, separators=(',', ':'))
+                self.publish(self.speedCmdTopic, cmd_json)
+
+                print(cmd_json)
+                step_acc += 1
+                time.sleep(0.1)
+        self.manual_status = ManualOperationType.eReady
+        self.manual_speed = [0, 0]
+        print("Manual_Unit_SlowlyStop completed.")
+        return True
+
+    def GenerateActionMPC(self, beg_name, end_name, autoDirect, wheelbase=0, task_type="None"):
+        self.GeneratePath(beg_name, end_name, task_type=task_type)
+
+
+
+

count.json

@@ -1 +1 @@
-{"load_count": 661, "single_count": 1926}
+{"load_count": 824, "single_count": 2516}

dijkstra/Map.py

@@ -1,10 +1,13 @@
 import math
+from copy import deepcopy
+
 from dijkstra.dijkstra_algorithm import Graph
 import numpy as np
 import scipy.spatial as spt
 import message_pb2 as message
 import uuid
 
+
 class Node(object):
     def __init__(self, id, x, y):
         self.id_ = id
@@ -27,14 +30,16 @@ class SpaceNode(Node):
     def __init__(self, id, x, y, yaw):
         Node.__init__(self, id, x, y)
         self.yaw_ = yaw
-    def frontPoint(self,wheelBase):
-        x=self.x_+wheelBase/2.0*math.cos(self.yaw_)
-        y=self.y_+wheelBase/2.0*math.sin(self.yaw_)
-        return [x,y]
-    def backPoint(self,wheelBase):
-        x=self.x_-wheelBase/2.0*math.cos(self.yaw_)
-        y=self.y_-wheelBase/2.0*math.sin(self.yaw_)
-        return [x,y]
+
+    def frontPoint(self, wheelBase):
+        x = self.x_ + wheelBase / 2.0 * math.cos(self.yaw_)
+        y = self.y_ + wheelBase / 2.0 * math.sin(self.yaw_)
+        return [x, y]
+
+    def backPoint(self, wheelBase):
+        x = self.x_ - wheelBase / 2.0 * math.cos(self.yaw_)
+        y = self.y_ - wheelBase / 2.0 * math.sin(self.yaw_)
+        return [x, y]
 
 
 def singleton(cls):
@@ -52,14 +57,15 @@ def singleton(cls):
 map 采用单例
 '''
 
-@singleton
+
 class DijikstraMap(object):
     def __init__(self):
-        self.nodes_ = {}
+        self.nodes_ = {} #dict ,{id: StreetNode/SpaceNode}
         self.graph_ = Graph()
 
-    def GetVertex(self,id):
+    def GetVertex(self, id):
         return self.nodes_.get(id)
+
     def AddVertex(self, node):
         if isinstance(node, (StreetNode)):
             print("add street node :%s " % (node.id_))
@@ -72,8 +78,14 @@ class DijikstraMap(object):
         self.graph_.AddVertex(node.id_, [node.x_, node.y_])
         return True
 
+    def ResetVertexValue(self, node):
+        self.graph_.ResetVertexValue(node.id_, [node.x_, node.y_])
+        self.nodes_[node.id_].x_ = node.x_
+        self.nodes_[node.id_].y_ = node.y_
+
     def AddEdge(self, id1, id2, direct=False):
         if self.nodes_.get(id1) == None or self.nodes_.get(id2) == None:
+            print("Exceptin: Add edge failed")
             raise ("Add edge failed %s or %s node is not exist" % (id1, id2))
         print("Add Edge :%s-%s" % (id1, id2))
         self.graph_.AddEdge(id1, id2)
@@ -82,21 +94,23 @@ class DijikstraMap(object):
 
     def VertexDict(self):
         return self.nodes_
+
     def Edges(self):
         return self.graph_.graph_edges
-    def findNeastNode(self,pt):
-        labels=[]
-        pts=[]
+
+    def findNeastNode(self, pt):
+        labels = []
+        pts = []
         for item in self.nodes_.items():
-            [label,node]=item
+            [label, node] = item
             labels.append(label)
-            pts.append([node.x_,node.y_])
-        points=np.array(pts)
-        ckt=spt.KDTree(data=points,leafsize=10)
-        find_pt=np.array(pt)
-        d,i=ckt.query(find_pt)
-        if i>=0 and i<len(pts):
-            return [labels[i],pts[i]]
+            pts.append([node.x_, node.y_])
+        points = np.array(pts)
+        ckt = spt.KDTree(data=points, leafsize=10)
+        find_pt = np.array(pt)
+        d, i = ckt.query(find_pt)
+        if i >= 0 and i < len(pts):
+            return [labels[i], pts[i]]
 
     def GetShortestPath(self, beg, end):
         [pathId, distance] = self.graph_.shortest_path(beg, end)
@@ -109,97 +123,98 @@ class DijikstraMap(object):
         return path
 
     @staticmethod
-    def CreatePath(pathNodes,delta):
-        last_node=None
-        trajectry=[]
+    def CreatePath(pathNodes, delta):
+        last_node = None
+        trajectry = []
         for node in pathNodes:
-            if last_node==None:
-                last_node=node
+            if last_node == None:
+                last_node = node
                 continue
-            dis=last_node.distance(node)
-            if dis<0.5:
-                last_node=node
-                continue    #同一点
+            dis = last_node.distance(node)
+            if dis < 0.5:
+                last_node = node
+                continue  # 同一点
             else:
-                vector=[node.x_-last_node.x_,node.y_-last_node.y_]
-
-                dx=vector[0]
-                dy=vector[1]
-                yaw=math.asin(dy/math.sqrt(dx*dx+dy*dy))
-                if yaw>=0:
-                    if dx<0:
-                        yaw=math.pi-yaw
-                if yaw<0:
-                    if dx<0:
-                        yaw=-math.pi-yaw
-                len=int(math.sqrt(dx*dx+dy*dy)/delta)
-                ax=math.cos(yaw)*delta
-                ay=math.sin(yaw)*delta
-                poses=[]
-                if isinstance(last_node,(SpaceNode)):
-                    yaw=yaw+math.pi
-                for i in range(len+1):
-                    pose=[last_node.x_+i*ax,last_node.y_+i*ay,yaw]
+                vector = [node.x_ - last_node.x_, node.y_ - last_node.y_]
+
+                dx = vector[0]
+                dy = vector[1]
+                yaw = math.asin(dy / math.sqrt(dx * dx + dy * dy))
+                if yaw >= 0:
+                    if dx < 0:
+                        yaw = math.pi - yaw
+                if yaw < 0:
+                    if dx < 0:
+                        yaw = -math.pi - yaw
+                len = int(math.sqrt(dx * dx + dy * dy) / delta)
+                ax = math.cos(yaw) * delta
+                ay = math.sin(yaw) * delta
+                poses = []
+                if isinstance(last_node, (SpaceNode)):
+                    yaw = yaw + math.pi
+                for i in range(len + 1):
+                    pose = [last_node.x_ + i * ax, last_node.y_ + i * ay, yaw]
                     poses.append(pose)
                 trajectry.append(poses)
-                last_node=node
+                last_node = node
         return trajectry
+
     @staticmethod
     def CreateNavCmd(pose, path):
         if len(path) <= 1:
             return None
 
         cmd = message.NavCmd()
-        cmd.action=0  # 新导航
+        cmd.action = 0  # 新导航
         key = str(uuid.uuid4())
-        cmd.key=(key)
+        cmd.key = (key)
         adjustdiff = message.Pose2d()
         node_mpcdiff = message.Pose2d()
         enddiff = message.Pose2d()
-        lastAdjustDiff=message.Pose2d()
+        lastAdjustDiff = message.Pose2d()
 
         # 目标点精度设置
         # 原地调整精度
-        adjustdiff.x=(0.1)
-        adjustdiff.y=(0.1)
-        adjustdiff.theta=(0.5 * math.pi / 180.0)
+        adjustdiff.x = (0.1)
+        adjustdiff.y = (0.1)
+        adjustdiff.theta = (0.5 * math.pi / 180.0)
 
-        #过程点巡线目标精度
-        node_mpcdiff.x=(0.05)
-        node_mpcdiff.y=(0.05)
-        node_mpcdiff.theta=(10 * math.pi / 180.0)
+        # 过程点巡线目标精度
+        node_mpcdiff.x = (0.05)
+        node_mpcdiff.y = (0.05)
+        node_mpcdiff.theta = (10 * math.pi / 180.0)
 
-        #最后一个巡线目标点精度
-        enddiff.x=(0.02)
-        enddiff.y=(0.02)
-        enddiff.theta=(0.5 * math.pi / 180.0)
+        # 最后一个巡线目标点精度
+        enddiff.x = (0.02)
+        enddiff.y = (0.02)
+        enddiff.theta = (0.5 * math.pi / 180.0)
 
-        #最后一个原地调整精度
-        lastAdjustDiff.x=(0.03)
-        lastAdjustDiff.y=(0.01)
-        lastAdjustDiff.theta=(0.7 * math.pi / 180.0)
+        # 最后一个原地调整精度
+        lastAdjustDiff.x = (0.03)
+        lastAdjustDiff.y = (0.01)
+        lastAdjustDiff.theta = (0.7 * math.pi / 180.0)
 
         # 速度限制
         v_limit = message.SpeedLimit()
         angular_limit = message.SpeedLimit()
         horize_limit = message.SpeedLimit()
-        v_limit.min=(0.1)
-        v_limit.max=(0.2)
-        horize_limit.min=(0.05)
-        horize_limit.max=(0.2)
-        angular_limit.min=(2)
-        angular_limit.max=(40.0)
+        v_limit.min = (0.1)
+        v_limit.max = (0.2)
+        horize_limit.min = (0.05)
+        horize_limit.max = (0.2)
+        angular_limit.min = (2)
+        angular_limit.max = (40.0)
         # mpc速度限制
         mpc_x_limit = message.SpeedLimit()
-        last_MPC_v=message.SpeedLimit()
+        last_MPC_v = message.SpeedLimit()
         mpc_angular_limit = message.SpeedLimit()
-        mpc_x_limit.min=(0.05)
-        mpc_x_limit.max=(1.2)
-        last_MPC_v.min=0.03
-        last_MPC_v.max=0.4
+        mpc_x_limit.min = (0.05)
+        mpc_x_limit.max = (1.2)
+        last_MPC_v.min = 0.03
+        last_MPC_v.max = 0.4
 
-        mpc_angular_limit.min=(0 * math.pi / 180.0)
-        mpc_angular_limit.max=(3 * math.pi / 180.0)
+        mpc_angular_limit.min = (0 * math.pi / 180.0)
+        mpc_angular_limit.max = (3 * math.pi / 180.0)
 
         # 创建动作集----------------------
 
@@ -208,7 +223,7 @@ class DijikstraMap(object):
         for node in path:
             if last_node == None:
                 last_node = node
-                count+=1
+                count += 1
                 continue
 
             # 运动到上一点
@@ -226,13 +241,13 @@ class DijikstraMap(object):
                 yaw = yaw + math.pi
             # 添加调整动作
             act_adjust = message.Action()
-            act_adjust.type=(1)
+            act_adjust.type = (1)
 
-            act_adjust.target.x=(last_node.x_)
-            act_adjust.target.y=(last_node.y_)
-            act_adjust.target.theta=(yaw)
-            #最后一个调整点
-            if count==len(path)-2:
+            act_adjust.target.x = (last_node.x_)
+            act_adjust.target.y = (last_node.y_)
+            act_adjust.target.theta = (yaw)
+            # 最后一个调整点
+            if count == len(path) - 2:
                 act_adjust.target_diff.CopyFrom(lastAdjustDiff)
             else:
                 act_adjust.target_diff.CopyFrom(adjustdiff)
@@ -243,15 +258,15 @@ class DijikstraMap(object):
 
             # 添加mpc动作
             act_along = message.Action()
-            act_along.type=(2)
-
-            act_along.begin.x=(last_node.x_)
-            act_along.begin.y=(last_node.y_)
-            act_along.begin.theta=(yaw)
-            act_along.target.x=(node.x_)
-            act_along.target.y=(node.y_)
-            act_along.target.theta=(yaw)
-            if count==len(path)-1:
+            act_along.type = (2)
+
+            act_along.begin.x = (last_node.x_)
+            act_along.begin.y = (last_node.y_)
+            act_along.begin.theta = (yaw)
+            act_along.target.x = (node.x_)
+            act_along.target.y = (node.y_)
+            act_along.target.theta = (yaw)
+            if count == len(path) - 1:
                 act_along.target_diff.CopyFrom(enddiff)
 
             else:
@@ -264,6 +279,55 @@ class DijikstraMap(object):
             cmd.actions.add().CopyFrom(act_along)
 
             last_node = node
-            count+=1
+            count += 1
 
         return cmd
+
+
+@singleton
+class MapManager(object):
+    def __init__(self):
+        self.map = DijikstraMap()
+        self.map_t = DijikstraMap()
+
+    def GetVertex(self, id):
+        return self.map_t.GetVertex(id)
+
+    def AddVertex(self, node):
+        self.map.AddVertex(node)
+        self.map_t.AddVertex(node)
+
+    def AddEdge(self, id1, id2, direct=False):
+        self.map.AddEdge(id1, id2, direct)
+        self.map_t.AddEdge(id1, id2, direct)
+
+    def AddVertex_t(self, node):
+        self.map_t.AddVertex(node)
+
+    def AddEdge_t(self, id1, id2, direct=False):
+        self.map_t.AddEdge(id1, id2, direct)
+
+    def Reset(self):
+        self.map_t = deepcopy(self.map)
+
+    def VertexDict(self):
+        return self.map_t.VertexDict()
+
+    def Edges(self):
+        return self.map_t.Edges()
+
+    def findNeastNode(self, pt):
+        return self.map_t.findNeastNode(pt)
+
+    def GetShortestPath(self, beg, end):
+        print("beg: ", self.map_t.graph_.points[beg])
+        print("end: ", self.map_t.graph_.points[end])
+        return self.map_t.GetShortestPath(beg, end)
+
+    def ResetSpaceNode(self, space_id, x, y):
+        space = SpaceNode(space_id, x, y, 0)
+        return self.map_t.ResetVertexValue(space)
+
+    def ResetStreetNode(self, space_id, x, y):
+        street = StreetNode(space_id, x, y)
+        return self.map_t.ResetVertexValue(street)

dijkstra/dijkstra_algorithm.py

@@ -4,6 +4,7 @@ comments and no error handling.
 """
 from collections import deque
 import math
+
 INFINITY = float("inf")
 
 
@@ -25,19 +26,26 @@ class Graph:
         # used by Dijkstra's algorithm, it's just an intermediate step in the
         # create of self.nodes and self.adjacency_list.
 
-        self.points={}
-        self.graph_edges = []
-        self.nodes = set()
+        self.points = {}  # dict,{id:[x, y]}
+        self.graph_edges = []  # list, [(node_id1, node_id2, distance)]
+        self.nodes = set()  # dict,{id:[node_id1, node_id2]}
+
+    def AddVertex(self, name, point):
+        self.points[name] = point
 
-    def AddVertex(self,name,point):
-        self.points[name]=point
-    def AddEdge(self,name1,name2):
-        if self.points.get(name1)==None or self.points.get(name2)==None:
-            print("node :%s or %s not exis"%(name1,name2))
+    def ResetVertexValue(self, name, point):
+        """
+        point : [x, y]
+        """
+        self.points[name] = point
+
+    def AddEdge(self, name1, name2):
+        if self.points.get(name1) == None or self.points.get(name2) == None:
+            print("node :%s or %s not exis" % (name1, name2))
             return False
-        pt1=self.points[name1]
-        pt2=self.points[name2]
-        distance=math.sqrt(math.pow(pt1[0]-pt2[0],2)+math.pow(pt1[1]-pt2[1],2))+0.000001
+        pt1 = self.points[name1]
+        pt2 = self.points[name2]
+        distance = math.sqrt(math.pow(pt1[0] - pt2[0], 2) + math.pow(pt1[1] - pt2[1], 2)) + 0.000001
         self.graph_edges.append((name1, name2, distance))
         self.nodes.update([name1, name2])
 
@@ -48,6 +56,7 @@ class Graph:
 
     def __getitem__(self, item):
         return self.points[item]
+
     def shortest_path(self, start_node, end_node):
         """Uses Dijkstra's algorithm to determine the shortest path from
         start_node to end_node. Returns (path, distance).
@@ -89,7 +98,7 @@ class Graph:
                     previous_node[neighbor] = current_node
 
             if current_node == end_node:
-                break # we've visited the destination node, so we're done
+                break  # we've visited the destination node, so we're done
 
         # To build the path to be returned, we iterate through the nodes from
         # end_node back to start_node. Note the use of a deque, which can
@@ -103,6 +112,7 @@ class Graph:
 
         return path, distance_from_start[end_node]
 
+
 '''
 def main():
     """Runs a few simple tests to verify the implementation.

map.json

@@ -1,31 +1,39 @@
 {
   "street_nodes": {
-    "R1": [
-      4.25,
-      -2.37
+    "Input_R147": [
+      -10.98, -7.16
     ],
-    "R2": [
-      20.47,
-      -2.37
+    "Input_R1100": [
+      -0.32, -7.16
+    ],
+    "Input_R1101": [
+      -0.32, -7.16
     ]
   },
 
   "space_nodes": {
-    "S1": [
-      20.47,
-      6.8,
-      1.57
+    "S147": [
+      -10.97, -13.42, -90
+    ],
+    "S1101": [
+      -0.32, 1.0, 90
+    ],
+    "S1100": [
+      -0.32, 1.0, 90
     ]
-
   },
   "roads": {
-    "road_1": [
-      "R1",
-      "R2"
+    "road_Input": [
+      "Input_R147",
+      "Input_R1100"
+    ],
+    "road_147": [
+      "S147",
+      "Input_R147"
     ],
-    "road_2": [
-      "S1",
-      "R2"
+    "road_1100": [
+      "S1100",
+      "Input_R1100"
     ]
   }
 }

message.proto

@@ -18,13 +18,19 @@ message AgvStatu{
 }
 
 message ToAgvCmd {
-  int32 H=1;  //心跳
-  int32 M=2;	//模式：2整车模式，1：单车
-  int32 T=3;  // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持, 7:提升机构抬升, 8:抬升机构下降, 其他/未接收到：停止
-  float V=4;  //角速度
-  float W=5;  //线速度
-  float L=6;  //轴距
-  int32 end=7;
+    int32 H1 = 1;  //心跳
+    int32 M1 = 2;    //模式：2整车模式，1：单车
+    int32 T1 = 3; // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持, 7:提升机构抬升, 8:抬升机构下降, 其他/未接收到：停止
+    float V1 = 4;  //线速度
+    float W1 = 5;  //角速度
+    float V2 = 6;  //线速度
+    float W2 = 7;  //角速度
+    float V3 = 8;  //线速度
+    float W3 = 9;  //角速度
+    float L1 = 10;  //轴距
+    int32 P1 = 11; //车位编号
+    float D1 = 12; //距目标点距离
+    int32 end = 13;
 }
 
 message Pose2d
@@ -107,3 +113,305 @@ message RobotStatu
 }
 
 
+
+
+
+
+
+
+
+
+
+
+enum Range_status {
+    Range_correct = 0x0000;                 // 未超界
+    Range_front = 0x0001;                   //前超界
+    Range_back = 0x0002;                    //后超界
+    Range_left = 0x0004;                    // 左超界
+    Range_right = 0x0008;                   // 右超界
+    Range_bottom = 0x0010;                  //底盘超界
+    Range_top = 0x0020;                     // 车顶超界
+    Range_car_width = 0x0040;               // 车宽超界
+    Range_car_wheelbase = 0x0080;           // 轴距超界
+    Range_angle_anti_clock = 0x0100;        // 左(逆时针)旋转超界
+    Range_angle_clock = 0x0200;             // 右(顺时针)旋转超界
+    Range_steering_wheel_nozero = 0x0400;   // 方向盘未回正
+    Range_car_moving = 0x0800;              // 车辆移动为1,静止为0
+}
+
+enum MeasureStatu {
+    Measure_OK = 0;                 // 测量完成
+    Measure_Empty_Clouds = 1;       // 测量结果：测量区域数据为空
+    Measure_Failture = 2;           // 测量结果：测量区域数据无法检测出车辆
+    Measure_Border = 3;             // 测量结果：对于PLC, 车辆存在超界，具体超界状态对比Range_status
+    Measure_Terminal_Border = 4;    // 测量结果：对于终端, 车辆存在超界，具体超界状态对比Range_status
+    Lidar_Disconnect = 5;           // 测量雷达：失去连接
+
+    Measure_Statu_Max = 6;
+}
+
+/*测量信息*/
+message measure_info {
+    float cx=1;                                 // 车辆中心坐标x
+    float cy=2;                                 // 车辆中心坐标y
+    float theta=3;                              // 车身偏转角度（相对于y轴，左正右负）
+    float length=4;                             // 车身长度（厦门四个雷达，含有该值，楚天两个雷达，该值为0）
+    float width=5;                              // 车身宽度（左右两侧轮子最大宽度）
+    float height=6;                             // 车身高度
+    float wheelbase=7;                          // 车辆前后轴距
+    float front_theta=8;                        // 车辆前轮偏转角度
+    int32 border_statu=9;                       // 超界状态， 位运算
+    MeasureStatu ground_status=10;              // 测量状态，0=正常, 1=空, 2=测量失败, 3=超界, 4=终端超界， 5=雷达断连
+    int32 is_stop=11;                           // <是否可停> 1为可停，0为不可停
+    int32 motion_statu=12;                      // 运动状态，0=运动， 1=静止（只有三秒内都是静止才会写1，只要瞬间触发运动就会写0）
+    float move_distance=13;                     // 汽车需要往前移动的距离。
+}
+
+/*分配的车位信息*/
+message parkspace_info{
+    int32 id=1;
+    int32 serial_id=2;    //排序id
+    int32 table_id=3;     //标签id
+    int32 unit_id=4;      //单元号
+    int32 floor=5;        //楼层号
+    int32 room_id=6;      //同层编号
+    float height=7;       //车高档位
+}
+
+//任务表单状态
+enum STATU{
+    eNormal=0;        // 正常
+    eWarning=1;       // 警告
+    eError=2;         // 错误
+    eCritical=3;      // 严重错误
+}
+
+//表单流程模式
+enum Table_process_mode
+{
+    PROCESS_NORMAL = 0;             // 0：正常模式， 检查节点会向收费系统发送请求，收费系统的答复 通过后，再向调度发送请求
+    PROCESS_ONLY_TO_DISPATCH = 1;   // 1：强制存取车，检查节点会向收费系统发送请求，忽略收费系统的答复，直接向调度发送请求。
+    PROCESS_ONLY_TO_PAY = 2;        // 2：虚拟存取车， 检查节点会向收费系统发送请求，忽略收费系统的答复。
+}
+
+/*
+表单执行状态
+ */
+message table_statu{
+    STATU execute_statu=1;                    // 执行状态
+    string statu_description=2;               // 状态描述
+    Table_process_mode table_process_mod=3;   // 表单流程模式
+}
+/*
+号牌信息
+ */
+message plate_number_info
+{
+    string plate_number = 1;        // 号牌
+    string plate_color = 2;         // 号牌颜色
+    string plate_type = 3;          // 号牌类型, 车辆类型
+    int32  plate_confidence = 4;    // 号牌可信度, 1-100 值越高可信度越高
+    string recognition_time = 5;    // 识别时间点, yyyyMMddHHmmss
+    string plate_full_image = 6;    // 号牌全景图, base64编码
+    string plate_clip_image = 7;    // 号牌特写图, base64编码
+}
+/*
+停车表单
+ */
+message park_table{
+    table_statu statu=1;                      //表单状态
+    int32 queue_id=2;                         //指令排队编号
+
+    string car_number=3;
+    int32 unit_id=4;
+    int32 terminal_id=5;
+    string primary_key=6;
+
+    measure_buffer entrance_measure_info=7;     // 入口测量信息
+    parkspace_info allocated_space_info=8;    // 分配的车位信息
+    measure_buffer actually_measure_info=9;     // 实际测量信息或者叫二次测量信息
+    parkspace_info actually_space_info=10;    // 实际停放的车位
+    int32 import_id =11;                      // 入口id, 1~2
+
+    plate_number_info car_number_info = 12;   // 车牌号信息
+}
+
+/*
+取车表单
+ */
+message pick_table{
+    table_statu statu=1;      // 表单状态
+    int32 queue_id=2;         // 指令排队编号
+
+    string car_number=3;
+    int32 unit_id=4;
+    int32 terminal_id=5;
+    string primary_key=6;
+
+    parkspace_info actually_space_info=7;     // 实际停放的车位信息
+
+    measure_buffer actually_measure_info=8;     // 存车时的实际测量信息（轴距）
+
+    int32 export_id=9;                        // 出口id, 3~4
+    bool  is_leaved=10;                       // 是否离开
+    plate_number_info car_number_info = 11;   // 车牌号信息
+}
+
+/*
+以下是状态消息
+ */
+
+/*
+单片机节点状态
+ */
+message out_mcpu_statu{     //数值+1后
+    int32 door_statu=1;       //外门状态       0无效, 1无效, 2开到位, 3 关到位, 4开关中, 5 故障
+    int32 outside_safety=2;    //是否有车      0无效, 1无车, 2有车
+}
+
+message in_mcpu_statu{      //数值+1后
+    int32 door_statu=1;       //外门状态       0无效, 1无效, 2开到位, 3 关到位, 4开关中, 5 故障
+    int32 back_io=2;          //后超界       0无效, 1后超界, 2正常
+    int32 is_occupy=3;        //是否有车      0无效, 1无车, 2有车
+    int32 heighth=4;          //车高状态      0无效, 1无效, 2小车, 3中车, 4大车, 5故障, 6故障
+}
+/*
+测量节点状态
+ */
+message measure_buffer{
+    measure_info measure_info_to_plc_forward=1;     //雷达数据给plc，正向
+    measure_info measure_info_to_plc_reverse=2;     //雷达数据给plc，反向
+    measure_info measure_info_to_terminal=3;        //雷达数据给终端，边界较小
+}
+
+//搬运器状态枚举
+enum CarrierStatu{
+    eIdle=0;
+    eBusy=1;
+    eFault=2;
+}
+
+
+
+
+/*
+//楚天调度入口汽车范围的修正信息
+message dispatch_region_info
+{
+    int32 terminal_id=1;                //入口终端编号, 1~6
+    float turnplate_angle_min=2;        //转盘角度最小值, 负值, 例如 -5度
+    float turnplate_angle_max=3;        //转盘角度最大值, 正值, 例如 +5度
+}
+
+//楚天搬运器状态消息
+message dispatch_node_statu{
+    CarrierStatu statu=1;
+    int32 idle_stop_floor=2;  //空闲时停留位置
+    park_table  running_pack_info=3;  //正在执行的停车表单
+    pick_table  running_pick_info=4;  //正在执行的取车表单
+
+    int32                                   unit_id = 5;                            //单元号, 1~3
+    int32                                   plc_heartbeat = 6;                      //plc心跳信息
+    int32                                   plc_status_info = 7;                    //plc状态的集合
+                                                                                            //0 bit, 手动模式
+                                                                                            //1 bit, 自动模式
+                                                                                            //2 bit, 自动运行中
+                                                                                            //3 bit, 复位
+                                                                                            //4 bit, 1号口可以进车
+                                                                                            //5 bit, 2号口可以进车
+                                                                                            //6 bit, 预留
+                                                                                            //7 bit, 预留
+    repeated dispatch_region_info           dispatch_region_info_vector = 8;        //调度入口汽车范围的修正信息
+}
+ */
+
+
+
+//plc出入口状态结构体
+message dispatch_plc_passway_status
+{
+    int32			car_height = 1;			            //车高 0=无车 1=小车 2=中车 3=大车 4=超高车
+    int32			outside_door_status = 2;	        //外门状态 0=无状态 1=开到位 2=关到位
+    int32			inside_door_status = 3;	            //内门状态 0=无状态 1=开到位 2=关到位
+    int32			comb_body_status = 4;	            //梳体状态 0=无状态 1=上到位 2=下到位, AB特有
+    float			turnplate_angle_min = 5;	        //转盘角度最小值, C特有, 负值, 例如 -5度
+    float			turnplate_angle_max = 6;	        //转盘角度最大值, C特有, 正值, 例如 +5度
+    int32			sensor_1 = 7;				        //传感器状态的集合1
+                                                        //0 bit, 地感 0=无车 1=有车
+                                                        //1 bit, 移动检测 0=运动 1=静止
+                                                        //2 bit, 动态超限 0=遮挡 1=正常
+                                                        //3 bit, 后超界 0=遮挡 1=正常
+                                                        //4 bit, 前超界 0=遮挡 1=正常
+                                                        //5 bit, 左超界 0=遮挡 1=正常
+                                                        //6 bit, 右超界 0=遮挡 1=正常
+                                                        //7 bit, 车高小车， 0=遮挡 1=正常，AB单元为1480， C单元为1780，
+    int32			sensor_2 = 8;				        //传感器状态的集合1
+                                                        //0 bit, 车高中车， 0=遮挡 1=正常，AB单元为1500， C单元为1800，
+                                                        //1 bit, 车高大车， 0=遮挡 1=正常，AB单元为2050， C单元为2050，
+                                                        //2 bit, 有车检测 0=无车 1=有车
+                                                        //3 bit, 车轮1检测 0=无车 1=有车, AB特有
+                                                        //4 bit, 车轮2检测 0=无车 1=有车, AB特有
+                                                        //5 bit, 预留
+                                                        //6 bit, 预留
+                                                        //7 bit, 预留
+    int32           plc_passway_enable=9;               //出入口 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+}
+
+//厦门搬运器状态消息
+message dispatch_node_statu{
+    CarrierStatu                            statu=1;
+    int32                                   idle_stop_floor=2;          //空闲时停留位置
+    park_table                              running_pack_info=3;        //正在执行的停车表单
+    pick_table                              running_pick_info=4;        //正在执行的取车表单
+    int32                                   unit_id = 5;                //单元号, 1~3
+
+    int32                                   plc_heartbeat = 6;                      //plc心跳信息
+    int32                                   plc_mode_status = 7;                    //plc状态的集合
+                                                                                    //0 bit, 手动模式, 维修模式
+                                                                                    //1 bit, 自动模式
+                                                                                    //2 bit, 自动运行中
+                                                                                    //3 bit, 复位
+                                                                                    //4 bit, 预留
+                                                                                    //5 bit, 预留
+                                                                                    //6 bit, 预留
+                                                                                    //7 bit, 预留
+    int32                                   plc_passway_status = 8;                 //plc 出入口状态
+                                                                                    //0 bit, 入口1 可进车
+                                                                                    //1 bit, 入口1 维护
+                                                                                    //2 bit, 入口2 可进车
+                                                                                    //3 bit, 入口2 维护
+                                                                                    //4 bit, 出口1 可出车
+                                                                                    //5 bit, 出口1 维护
+                                                                                    //6 bit, 出口2 可出车
+                                                                                    //7 bit, 出口2 维护
+    int32                                   plc_carrier_status = 9;                 //搬运器状态 0=故障 1=存车 2=取车 3=空闲 4=维护 5=小故障 6=大故障 7=月度保养
+    int32                                   plc_inlet_1_status = 10;                //入口1 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    int32                                   plc_inlet_2_status = 11;                //入口2 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    int32                                   plc_outlet_3_status = 12;               //出口3 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    int32                                   plc_outlet_4_status = 13;               //出口4 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+
+    repeated dispatch_plc_passway_status    dispatch_plc_passway_status_vector = 14; //plc出入口状态结构体,  数组下标0~1是入口, 数组下标2~3是出口
+
+}
+
+message terminal_node_statu{
+    int32 terminal_id = 1;
+    int32 import_id = 2;
+    string car_number = 3;
+}
+
+//厦门C单元夹车杆
+//夹车杆数据
+message Clamp_lidar_info
+{
+    int32           has_wheel_flag=1;             //是否检测到车轮
+    float           distance_y=2;                 //轮子中心和夹车杆中心的距离y，修正小Y，对比前后轴距
+    float           distance_x=3;                 //轮子中心和夹车杆中心的距离x，修正轿厢x，对比2个大Y的左右偏差
+    int32           has_clamp_flag=4;             //夹车杆是否夹到位
+}
+//plc给夹车杆雷达, plc把大小Y伸出后，在夹车之前，会把9070复制到9071，从而保存一份数据
+message Clamp_lidar_buffer
+{
+    repeated Clamp_lidar_info clamp_lidar_info_vector=1;
+}
+

mytool/RabbitMq_helper/async_communication.py

@@ -0,0 +1,138 @@
+import threading
+import time
+import asyncio
+from concurrent.futures import ThreadPoolExecutor
+
+import aio_pika
+import queue
+
+class TimeStatu:
+    def __init__(self,statu=None,timeout=3):
+        self.statu=statu
+        self.time=time.time()
+        self.timeout_ms=timeout
+
+    def timeout(self):
+        tm=time.time()
+        return tm-self.time>self.timeout_ms or self.statu==None
+
+class RabbitAsyncCommunicator(threading.Thread):
+    def __init__(self, host,port, user, password):
+        threading.Thread.__init__(self)
+        self._host = host
+        self._port=port
+        self._user = user
+        self._password = password
+        self._connection = None
+        self._channel_consumer = None
+        self._channel_send = None
+        self._channel_statu = None
+
+        self._consumer_callbacks= None
+        self._recv_status=None
+        self._queue_callbacks= {}
+        self._publish_msg_queue=queue.Queue()
+        self._status={}
+        self._statu_callbacks={}
+        self._closing = False
+        self._receiver = None
+        self._pool = ThreadPoolExecutor()
+
+    def Init(self,consumer_callbacks,recv_status,receiver=None):
+        self._consumer_callbacks=consumer_callbacks
+        self._recv_status=recv_status
+        self._receiver = receiver
+        if self._recv_status==None:
+            return
+        for ex_name,key in self._recv_status:
+            self._status[ex_name+":"+key]=TimeStatu(None,0.1)
+
+    def publish(self,ex_name,key,msg):
+        self._publish_msg_queue.put([ex_name,key,msg])
+
+    def bind_statu_callback(self,ex_name,key,callback):
+        self._statu_callbacks[ex_name+":"+key]=callback
+
+    def close(self):
+        self._closing=True
+        self._pool.shutdown(wait=True)
+
+    async def init(self):
+        connection_string="amqp://%s:%s@%s/"%(self._user,self._password,self._host)
+        self._connection = await aio_pika.connect_robust(connection_string)
+        self._channel_consumer = await self._connection.channel()
+        self._channel_send = await self._connection.channel()
+        self._channel_statu = await self._connection.channel()
+        # Will take no more than 10 messages in advance
+        await self._channel_consumer.set_qos(prefetch_count=1)
+        if self._consumer_callbacks==None:
+            return
+        for queue_name,callback in self._consumer_callbacks:
+            queue= await self._channel_consumer.get_queue(queue_name)
+            self._queue_callbacks[queue]=callback
+
+
+    def statu_callback(self,ex,key,msg):
+        id=ex+":"+key
+        self._status[id]=TimeStatu(msg)
+        callback=self._statu_callbacks.get(id)
+        if not callback==None:
+            callback(self._status[id],ex,key)
+
+    def __getitem__(self, key):
+        return self._status[key]
+    def __setitem__(self, key, value):
+        self._status[key]=value
+
+    async def recv(self,queue,callback):
+        await asyncio.sleep(2)
+        print("   订阅队列：%s" % queue)
+        async with queue.iterator() as queue_iter:
+            async for message in queue_iter:
+                if not callback==None:
+                    self._pool.submit(callback,message.body.decode())
+                    await message.ack()
+                if self._closing==True:
+                    return
+    async def recv_statu(self,ex_name,key,ttl=200):
+        statu_ex=await self._channel_statu.get_exchange(ex_name)
+        arg={}
+        arg["x-message-ttl"]=ttl
+        if self._receiver is not None:
+            queue=await self._channel_statu.declare_queue(key+"_for_"+self._receiver, auto_delete=True,arguments=arg)
+        else:
+            queue=await self._channel_statu.declare_queue('', auto_delete=True,arguments=arg)
+        await queue.bind(statu_ex,routing_key=key)
+        print("订阅状态端口：%s" % key)
+        async with queue.iterator() as queue_iter:
+            async for message in queue_iter:
+                async with message.process():
+                    self.statu_callback(ex_name,key,message.body.decode())
+                if self._closing==True:
+                    return
+
+    async def send(self):
+        while self._closing==False:
+            if self._publish_msg_queue.qsize()>0:
+                msg_bag=self._publish_msg_queue.get(False)
+                if not msg_bag==None:
+                    ex_name,key,msg=msg_bag
+                    ex= await self._channel_send.get_exchange(ex_name)
+                    await ex.publish(aio_pika.Message(body=msg.encode()),routing_key=key)
+            await asyncio.sleep(0.001)
+            #time.sleep(0.001)
+
+
+    async def main(self):
+        await self.init()
+        tasks=[]
+        tasks.append(self.send())
+        if not self._consumer_callbacks==None:
+            for queue,callback in self._queue_callbacks.items():
+                tasks.append(self.recv(queue,callback))
+        if not self._recv_status==None:
+            for ex_name,key in self._recv_status:
+                tasks.append(self.recv_statu(ex_name,key))
+        await asyncio.gather(*tasks)
+    def run(self):
+        asyncio.run(self.main())

mytool/RabbitMq_helper/main.py

@@ -0,0 +1,27 @@
+# This is a sample Python script.
+
+# Press Shift+F10 to execute it or replace it with your code.
+# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings.
+
+import async_communication as rabitmq
+def print_hi(name):
+    # Use a breakpoint in the code line below to debug your script.
+    print(f'Hi, {name}')  # Press Ctrl+F8 to toggle the breakpoint.
+
+
+def func(msg):
+    print(msg)
+
+# Press the green button in the gutter to run the script.
+if __name__ == '__main__':
+
+    rabit=rabitmq.RabbitAsyncCommunicator("192.168.8.201",5672,"zx","123456")
+    calbbacks=[["AGV_park_command_request_queue",func]]
+    rabit.Init(calbbacks,None)
+
+    rabit.start()
+    rabit.join()
+
+    print_hi('PyCharm')
+
+# See PyCharm help at https://www.jetbrains.com/help/pycharm/

sigmoid.py

@@ -20,26 +20,27 @@ def sigmoid(x, min_value=0.3, max_value=5, right_offset=5, span=10):
     return (max_value - min_value) / (1 + np.exp(-x)) + min_value
 
 
-# def test():
-#     plt.rcParams["font.family"] = "SimHei"
-#     plt.rcParams["axes.unicode_minus"] = False
-#     plt.rcParams["font.size"] = 12
-#
-#     z = np.linspace(-10, 10, 200)
-#     plt.plot(z, sigmoid(z))
-#
-#     plt.axvline(x=0, ls="--", c="k")
-#     plt.axhline(ls=":", c="k")
-#     plt.axhline(y=0.5, ls=":", c="k")
-#     plt.axhline(y=1, ls=":", c="k")
-#     plt.xlabel("z值")
-#     plt.ylabel("sigmoid(z)值")
+def test():
+    plt.rcParams["font.family"] = "SimHei"
+    plt.rcParams["axes.unicode_minus"] = False
+    plt.rcParams["font.size"] = 12
+
+    z = np.linspace(-10, 10, 200)
+    plt.plot(z, sigmoid(z))
+
+    plt.axvline(x=0, ls="--", c="k")
+    plt.axhline(ls=":", c="k")
+    plt.axhline(y=0.5, ls=":", c="k")
+    plt.axhline(y=1, ls=":", c="k")
+    plt.xlabel("z值")
+    plt.ylabel("sigmoid(z)值")
 
 
 if __name__ == "__main__":
-    for i in range(20):
-        x = i + 1
-        print(x, sigmoid(x, span=10))
+    test()
+    # for i in range(20):
+    #     x = i + 1
+    #     print(x, sigmoid(x, span=10))
 
 # cd grpc
 # mkdir -p cmake/build

test.py

@@ -33,6 +33,7 @@
 #
 # ===============================================================================
 import math
+import threading
 import time
 import sys
 from PyQt5.QtGui import *
@@ -47,9 +48,12 @@ import ManualOperationWidget
 import RobotData as RD
 import message_pb2 as message
 import google.protobuf.json_format as jtf
-import uuid
+import google.protobuf.text_format as tf
 
+import uuid
+from mytool.txt_helper.txt_operation import TXTOperation
 from custom_define import RobotName
+from mytool.RabbitMq_helper import async_communication as rabitmq
 
 
 # ===============================================================================
@@ -57,7 +61,7 @@ from custom_define import RobotName
 
 class MainWindow(QMainWindow):
     """docstring for Mainwindow"""
-    djks_map_ = mp.DijikstraMap()
+    djks_map_ = mp.MapManager()
     ui_nodes = {}
     ui_nodes["street_nodes"] = []
     ui_nodes["space_nodes"] = []
@@ -67,8 +71,8 @@ class MainWindow(QMainWindow):
         self.basic()
         self.count_frame_ = ControllWidget.CountFrame()
         self.LoadMap()
-        self.isLocalTest = True
-        self.isAutoDispatch = False
+        self.isLocalTest = False
+        self.isAutoDispatchOnline = True
 
         rpc_1 = "192.168.0.70:9090"
         mqtt_1 = ["pyui1", "192.168.0.70", 1883, "admin", "zx123456"]
@@ -116,6 +120,107 @@ class MainWindow(QMainWindow):
         self.timer_.timeout.connect(self.Update)
         self.timer_.start(100)
 
+        # 启动RabbitMQ
+        self.g_rabbitmq = None
+        if self.isAutoDispatchOnline:
+            self.g_rabbitmq = rabitmq.RabbitAsyncCommunicator("192.168.0.201", 5672, "zx", "123456")
+            calbbacks = [["agv_park_command_request_queue", self.recv_park_request],
+                         ["agv_pick_command_request_queue", self.recv_pick_request]]
+            self.g_rabbitmq.Init(calbbacks, None)
+            self.g_rabbitmq.setDaemon(True)  # 守护线程随主线程结束
+            self.g_rabbitmq.start()
+
+    def recv_park_request(self, msg):
+        print("Recv_park_request------------------\n", msg)
+        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)
+        park_table = message.park_table()
+        return
+
+    def AutoParkTask(self, park_table: message.park_table = None):
+        print("AutoParkTask:---------------------\n")
+        self.djks_map_.Reset()  # 重置地图
+        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,
+                                                  park_table.entrance_measure_info.measure_info_to_plc_forward.cy,
+                                                  (
+                                                              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
+        trans_a = entrance_theta + da - math.pi
+        while trans_a < 0:
+            trans_a += math.pi
+        print("entrance:", entrance_id, trans_x, trans_y, trans_a / math.pi * 180)
+
+        target_id = "S" + str(park_table.allocated_space_info.table_id)  # "S147"
+        print("target:", target_id)
+
+        wheel_base = park_table.entrance_measure_info.measure_info_to_plc_forward.wheelbase
+        self.Controller1.WheelBaseEdit.setText(str(wheel_base))
+        self.Controller2.WheelBaseEdit.setText(str(wheel_base))
+
+        trans_x += wheel_base / 2 * math.cos(trans_a)
+        trans_y += wheel_base / 2 * math.sin(trans_a)
+        self.djks_map_.ResetSpaceNode(entrance_id, trans_x, trans_y)  # 更新库位点
+
+        entrance_street_node = self.ComputeStreetNode(entrance_id, trans_x, trans_y, trans_a)
+        self.djks_map_.ResetStreetNode("Input_R1101", entrance_street_node[0], entrance_street_node[1])  # 更新库位点对应马路点
+        print("entrance_space pose: ", self.djks_map_.map_t.graph_.points[entrance_id])
+        print("entrance_street pose ", self.djks_map_.map_t.graph_.points["Input_R1101"])
+
+        # 加临时边
+        self.djks_map_.AddEdge_t(entrance_id, "Input_R1101")
+        # 加临时边
+        for node_id, value in self.djks_map_.VertexDict().items():
+            if node_id.find("Input") >= 0:
+                self.djks_map_.AddEdge_t("Input_R1101", node_id)
+
+        # # 开始系列子流程-----------------------------------------------------
+        # # 主车进入口
+        # cur_main_pose = self.Controller1.robot_.timedRobotStatu_.statu
+        # [label, pt] = mp.MapManager().findNeastNode([cur_main_pose.x, cur_main_pose.y])
+        # 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_.Navigatting(entrance_id, target_id, True, wheel_base)
+        # #
+
+
+
+        # 恢复
+        # self.djks_map_.Reset()
+
+    def ComputeStreetNode(self, s_id, s_x, s_y, s_theta):
+        """
+
+        """
+        n_x, n_y = 0, 0
+        if s_id == "S1101":
+            n_y = self.djks_map_.map_t.graph_.points["Input_R1100"][1]
+            k = math.tan(s_theta)
+            n_x = (n_y - s_y) / k + s_x  # 弧度
+        # print(n_x, n_y)
+        return [n_x, n_y]
+
     def LoadMap(self):
         self.gl = MapGLWidget()  # 将opengl例子嵌入GUI
         map = self.load_map("./map.json")
@@ -151,14 +256,14 @@ class MainWindow(QMainWindow):
 
     def Update(self):
         self.gl.update()
-        if self.isAutoDispatch:
-            self.Controller1.setVisible(False)
-            self.Controller2.setVisible(False)
+        if self.isAutoDispatchOnline:
+            # self.Controller1.setEnabled(False)
+            # self.Controller2.setEnabled(False)
+            pass
         else:
             self.Controller1.setVisible(True)
             self.Controller2.setVisible(True)
 
-
     # 窗口基础属性
     def basic(self):
         # 设置标题，大小，图标
@@ -172,6 +277,7 @@ class MainWindow(QMainWindow):
 
     def closeEvent(self, QCloseEvent):
         self.gl.close()
+        print("close...")
 
     # 分割窗口
     def split_(self):