提交代码

ControllWidget.py

@@ -0,0 +1,73 @@
+
+from PyQt5.QtWidgets import QWidget, QApplication,QMainWindow,QLabel,QLineEdit,QPushButton,QComboBox
+from PyQt5.QtGui import QPixmap,QPainter,QResizeEvent,QCloseEvent,QPaintEvent,QFont
+from PyQt5.QtCore import QSize,QTimer,QRect,Qt
+
+
+class Frame(QMainWindow):
+    def __init__(self,UI_data):
+        super(Frame, self).__init__()
+        self.setGeometry(0,0,400,500)
+        self.btnCreateTrajFunc=None
+        self.btnSendClickFunc=None
+        self.btnCancelFunc=None
+        self.InitUI(UI_data)
+
+
+    def SetBtnFunc(self,funcs):
+        if not funcs.get("CreateTraj")==None:
+            self.btnCreateTrajFunc=funcs["CreateTraj"]
+        if not funcs.get("SendCmd")==None:
+            self.btnSendClickFunc=funcs["SendCmd"]
+        if not funcs.get("CancelCmd")==None:
+            self.btnCancelFunc=funcs["CancelCmd"]
+
+
+    def InitUI(self,UI_data):
+
+        self.begstatic=QLabel(self)
+        self.begstatic.setText("  起点")
+        self.begstatic.setGeometry(0, 0, 50, 30)
+        self.begQc=QComboBox(self)
+        self.begQc.setGeometry(60, 0, 80, 30)
+
+        self.endstatic=QLabel(self)
+        self.endstatic.setText("  终点")
+        self.endstatic.setGeometry(0, 50, 50, 30)
+        self.endQc=QComboBox(self)
+        self.endQc.setGeometry(60, 50, 80, 30)
+
+        self.btn=QPushButton(self)
+        self.btn.setGeometry(20, 100, 100, 40)
+        self.btn.setText("  生成轨迹")
+        self.btn.clicked.connect(self.btnCreateClick)
+
+        self.btnSend=QPushButton(self)
+        self.btnSend.setGeometry(150, 100, 100, 40)
+        self.btnSend.setText("  启动导航")
+        self.btnSend.clicked.connect(self.btnSendClick)
+
+        self.btnCancel=QPushButton(self)
+        self.btnCancel.setGeometry(150, 150, 100, 40)
+        self.btnCancel.setText("  取消导航")
+        self.btnCancel.clicked.connect(self.btnCancelClick)
+
+        cb_list=UI_data['nodes']
+        self.begQc.addItems(cb_list)
+        self.endQc.addItems(cb_list)
+
+
+    def btnCreateClick(self):
+        if not self.btnCreateTrajFunc==None:
+            id1=self.begQc.currentText()
+            id2=self.endQc.currentText()
+            self.btnCreateTrajFunc(id1,id2)
+
+    def btnSendClick(self):
+        if not self.btnSendClickFunc==None:
+            self.btnSendClickFunc()
+    def btnCancelClick(self):
+        if not self.btnCancelFunc==None:
+            self.btnCancelFunc()
+
+

MapGLWidget.py

@@ -0,0 +1,145 @@
+
+import math
+import random
+import time
+from PyGLWidget import PyGLWidget
+from OpenGL.GL import *
+
+import message_pb2 as message
+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 MapGLWidget(PyGLWidget):
+
+    colors={"agv1":[1,0,0],
+            "agv2":[0,1,0],
+            "agv":[0,1,1]
+            }
+
+    nodes={}
+    roads={}
+    paths={}
+    timedAgv1=TimeStatu(None,0.5)
+    timedAgv2=TimeStatu(None,0.5)
+    def ResetAgv1(self,statu):
+        self.timedAgv1=TimeStatu(statu,0.5)
+        self.update()
+    def ResetAgv2(self,statu):
+        self.timedAgv2=TimeStatu(statu,0.5)
+        self.update()
+    def AddNode(self,node):
+        [id,type,point]=node
+        self.nodes[id]=[type,point]
+    def AddRoad(self,road):
+        [key,value]=road
+        self.roads[key]=value
+
+    def AddTraj(self,name,path):
+        self.paths[name]=path
+
+
+    def drawPath(self,poses,width,color):
+        glColor3f(color[0],color[1],color[2])
+        glLineWidth(2)
+        glBegin(GL_LINES)
+        for pose in poses:
+            [x,y,yaw]=pose
+            point1=[x,y]
+
+            [dx,dy]=self.RotatePoint([-width/2,-width/2],yaw)
+            point2=[x+dx,y+dy]
+
+            [dx,dy]=self.RotatePoint([-width/2,width/2],yaw)
+            point3=[x+dx,y+dy]
+
+            glVertex2d(point1[0],point1[1])
+            glVertex2d(point3[0],point3[1])
+
+            glVertex2d(point1[0],point1[1])
+            glVertex2d(point2[0],point2[1])
+        glEnd()
+
+    def drawEdge(self,pt1,pt2,width,color):
+
+        glColor4f(color[0],color[1],color[2],0)
+        glLineWidth(width)
+        glBegin(GL_LINES)
+        glVertex2d(pt1[0],pt1[1])
+        glVertex2d(pt2[0],pt2[1])
+        glEnd()
+
+    @staticmethod
+    def RotatePoint(point,yaw):
+        [x,y]=point
+        nx=math.cos(yaw)*x-y*math.sin(yaw)
+        ny=x*math.sin(yaw)+y*math.cos(yaw)
+        return [nx,ny]
+    def drawAxis(self,pose,len,width):
+        [x,y,yaw]=pose
+        x_source=[x+len*math.cos(yaw),y+math.sin(yaw)*len]
+        y_source=[x-math.sin(yaw)*len,y+len*math.cos(yaw)]
+
+        glDepthMask(GL_FALSE)
+        glColor3f(1,0,0)
+
+        glLineWidth(width)
+        glBegin(GL_LINES)
+        glVertex2d(x,y)
+        glVertex2d(x_source[0],x_source[1])
+        glEnd()
+
+        glColor3f(0,1,0)
+        glBegin(GL_LINES)
+        glVertex2d(x,y)
+        glVertex2d(y_source[0],y_source[1])
+        glEnd()
+    def drawNode(self,node,size,color):
+        [x,y]=node
+        glPointSize(size)
+        glDepthMask(GL_FALSE)
+        glBegin(GL_POINTS)
+        glColor4f(color[0],color[1],color[2],0.5)
+        glVertex2d(x,y)
+        glEnd()
+        '''glColor3f(1,1,1);
+        #glBegin(GL_TEXTURE)
+        glText'''
+
+    def paintGL(self):
+        PyGLWidget.paintGL(self)
+
+        self.drawAxis([0,0,0],5,5)
+        for node in self.nodes.items():
+            [_,[type,point]]=node
+            if type=="street_node":
+                self.drawNode(point,8,[0,0,1])
+            if type=="space_node":
+                self.drawNode(point,8,[1,1,0])
+        for road in self.roads.items():
+            [_,value]=road
+            for nodeId in value:
+                for nextId in value:
+                    if not nodeId==nextId:
+                        [_,point1]=self.nodes[nodeId]
+                        [_,point2]=self.nodes[nextId]
+                        self.drawEdge(point1,point2,3.,[0,0.5,0.5])
+
+        for [id,path] in self.paths.items():
+
+            rgb=self.colors.get(id)
+            if rgb is not None:
+                for traj in path:
+                    self.drawPath(traj,0.5,rgb)
+
+        #绘制agv
+        if self.timedAgv1.timeout()==False:
+            x=self.timedAgv1.statu.x
+            y=self.timedAgv1.statu.y
+            theta=self.timedAgv1.statu.theta
+            self.drawAxis([x,y,theta],3,5)

PyGLWidget.py

@@ -0,0 +1,326 @@
+# -*- coding: utf-8 -*-
+#===============================================================================
+#
+# PyGLWidget.py
+#
+# A simple GL Viewer.
+#
+# Copyright (c) 2011, Arne Schmitz <arne.schmitz@gmx.net>
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the <organization> nor the
+#       names of its contributors may be used to endorse or promote products
+#       derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+#===============================================================================
+
+from PyQt5 import QtCore, QtGui, QtOpenGL
+import math
+import numpy
+import numpy.linalg as linalg
+import OpenGL
+OpenGL.ERROR_CHECKING = True
+from OpenGL.GL import *
+from OpenGL.GLU import *
+
+class PyGLWidget(QtOpenGL.QGLWidget):
+
+    # Qt signals
+    signalGLMatrixChanged = QtCore.pyqtSignal()
+    rotationBeginEvent = QtCore.pyqtSignal()
+    rotationEndEvent = QtCore.pyqtSignal()
+
+    def __init__(self, parent = None):
+        format = QtOpenGL.QGLFormat()
+        format.setSampleBuffers(True)
+        QtOpenGL.QGLWidget.__init__(self, format, parent)
+        self.setCursor(QtCore.Qt.OpenHandCursor)
+        self.setMouseTracking(True)
+
+        self.modelview_matrix_  = [
+            [1,0,0,0],
+            [0,1,0,0],
+            [0,0,1,0],
+            [-10.53889084,-2.01535511,-25.95999146,1. ]]
+        self.translate_vector_  = [0, 0, 0.0]
+        self.viewport_matrix_   = []
+        self.projection_matrix_ = []
+        self.near_   = 0.1
+        self.far_    = 100.0
+        self.fovy_   = 45.0
+        self.radius_ = 2.0
+        self.last_point_2D_ = QtCore.QPoint()
+        self.last_point_ok_ = False
+        self.last_point_3D_ = [1.0, 0.0, 0.0]
+        self.isInRotation_  = False
+
+
+
+        # connections
+        #self.signalGLMatrixChanged.connect(self.printModelViewMatrix)
+
+    @QtCore.pyqtSlot()
+    def printModelViewMatrix(self):
+        print (self.modelview_matrix_)
+
+    def initializeGL(self):
+        # OpenGL state
+        glClearColor(0.8, 0.8, 0.8, 0.5)
+        glEnable(GL_DEPTH_TEST)
+        self.reset_view()
+        self.translate([-10,-10,-25.])
+
+
+
+    def resizeGL(self, width, height):
+        glViewport(0, 0, width, height );
+        self.set_projection( self.near_, self.far_, self.fovy_ );
+        self.updateGL()
+
+    def paintGL(self):
+        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
+
+        glMatrixMode(GL_MODELVIEW)
+        glLoadMatrixd(self.modelview_matrix_)
+        #self.printModelViewMatrix()
+
+    def set_projection(self, _near, _far, _fovy):
+        self.near_ = _near
+        self.far_ = _far
+        self.fovy_ = _fovy
+        self.makeCurrent()
+        glMatrixMode( GL_PROJECTION )
+        glLoadIdentity()
+        gluPerspective( self.fovy_, float(self.width()) / float(self.height()),
+                        self.near_, self.far_ )
+        self.updateGL()
+
+    def set_center(self, _cog):
+        self.center_ = _cog
+        self.view_all()
+
+    def set_radius(self, _radius):
+        self.radius_ = _radius
+        self.set_projection(_radius / 100.0, _radius * 100.0, self.fovy_)
+        self.reset_view()
+        self.translate([0, 0, -_radius * 2.0])
+        self.view_all()
+        self.updateGL()
+
+    def reset_view(self):
+        # scene pos and size
+        glMatrixMode( GL_MODELVIEW )
+        glLoadIdentity();
+        self.modelview_matrix_ = glGetDoublev( GL_MODELVIEW_MATRIX )
+        self.set_center([0.0, 0.0, 0.0])
+
+
+    def reset_rotation(self):
+        self.modelview_matrix_[0] = [1.0, 0.0, 0.0, 0.0]
+        self.modelview_matrix_[1] = [0.0, 1.0, 0.0, 0.0]
+        self.modelview_matrix_[2] = [0.0, 0.0, 1.0, 0.0]
+        glMatrixMode(GL_MODELVIEW)
+        glLoadMatrixd(self.modelview_matrix_)
+        self.updateGL()
+   
+    def translate(self, _trans):
+        # Translate the object by _trans
+        # Update modelview_matrix_
+        self.makeCurrent()
+        glMatrixMode(GL_MODELVIEW)
+        glLoadIdentity()
+        glTranslated(_trans[0], _trans[1], _trans[2])
+        glMultMatrixd(self.modelview_matrix_)
+        self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
+        self.translate_vector_[0] = self.modelview_matrix_[3][0]
+        self.translate_vector_[1] = self.modelview_matrix_[3][1]
+        self.translate_vector_[2] = self.modelview_matrix_[3][2]
+        self.signalGLMatrixChanged.emit()
+
+    def rotate(self, _axis, _angle):
+        t = [self.modelview_matrix_[0][0] * self.center_[0] +
+             self.modelview_matrix_[1][0] * self.center_[1] +
+             self.modelview_matrix_[2][0] * self.center_[2] +
+             self.modelview_matrix_[3][0],
+             self.modelview_matrix_[0][1] * self.center_[0] +
+             self.modelview_matrix_[1][1] * self.center_[1] +
+             self.modelview_matrix_[2][1] * self.center_[2] +
+             self.modelview_matrix_[3][1],
+             self.modelview_matrix_[0][2] * self.center_[0] +
+             self.modelview_matrix_[1][2] * self.center_[1] +
+             self.modelview_matrix_[2][2] * self.center_[2] +
+             self.modelview_matrix_[3][2]]
+
+        self.makeCurrent()
+        glLoadIdentity()
+        glTranslatef(t[0], t[1], t[2])
+        glRotated(_angle, _axis[0], _axis[1], _axis[2])
+        glTranslatef(-t[0], -t[1], -t[2])
+        glMultMatrixd(self.modelview_matrix_)
+        self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
+        self.signalGLMatrixChanged.emit()
+
+    def view_all(self):
+        self.translate( [ -( self.modelview_matrix_[0][0] * self.center_[0] +
+                             self.modelview_matrix_[0][1] * self.center_[1] +
+                             self.modelview_matrix_[0][2] * self.center_[2] +
+                             self.modelview_matrix_[0][3]),
+                           -( self.modelview_matrix_[1][0] * self.center_[0] +
+                              self.modelview_matrix_[1][1] * self.center_[1] +
+                              self.modelview_matrix_[1][2] * self.center_[2] +
+                              self.modelview_matrix_[1][3]),
+                           -( self.modelview_matrix_[2][0] * self.center_[0] +
+                              self.modelview_matrix_[2][1] * self.center_[1] +
+                              self.modelview_matrix_[2][2] * self.center_[2] +
+                              self.modelview_matrix_[2][3] +
+                              self.radius_ / 2.0 )])
+
+    def map_to_sphere(self, _v2D):
+        _v3D = [0.0, 0.0, 0.0]
+        # inside Widget?
+        if (( _v2D.x() >= 0 ) and ( _v2D.x() <= self.width() ) and
+            ( _v2D.y() >= 0 ) and ( _v2D.y() <= self.height() ) ):
+            # map Qt Coordinates to the centered unit square [-0.5..0.5]x[-0.5..0.5]
+            x  = float( _v2D.x() - 0.5 * self.width())  / self.width()
+            y  = float( 0.5 * self.height() - _v2D.y()) / self.height()
+
+            _v3D[0] = x;
+            _v3D[1] = y;
+            # use Pythagoras to comp z-coord (the sphere has radius sqrt(2.0*0.5*0.5))
+            z2 = 2.0*0.5*0.5-x*x-y*y;
+            # numerical robust sqrt
+            _v3D[2] = math.sqrt(max( z2, 0.0 ))
+
+            # normalize direction to unit sphere
+            n = linalg.norm(_v3D)
+            _v3D = numpy.array(_v3D) / n
+
+            return True, _v3D
+        else:
+            return False, _v3D
+
+    def wheelEvent(self, _event):
+        # Use the mouse wheel to zoom in/out
+        
+        d =  (_event.angleDelta().y()) / 250.0 * self.radius_
+        self.translate([0.0, 0.0, d])
+        self.updateGL()
+        _event.accept()
+
+    def mousePressEvent(self, _event):
+        self.last_point_2D_ = _event.pos()
+        self.last_point_ok_, self.last_point_3D_ = self.map_to_sphere(self.last_point_2D_)
+
+    def mouseMoveEvent(self, _event):
+        newPoint2D = _event.pos()
+
+        if ((newPoint2D.x() < 0) or (newPoint2D.x() > self.width()) or
+            (newPoint2D.y() < 0) or (newPoint2D.y() > self.height())):
+            return
+        
+        # Left button: rotate around center_
+        # Middle button: translate object
+        # Left & middle button: zoom in/out
+
+        value_y = 0
+        newPoint_hitSphere, newPoint3D = self.map_to_sphere(newPoint2D)
+
+        dx = float(newPoint2D.x() - self.last_point_2D_.x())
+        dy = float(newPoint2D.y() - self.last_point_2D_.y())
+
+        w  = float(self.width())
+        h  = float(self.height())
+
+        # enable GL context
+        self.makeCurrent()
+
+        # move in z direction
+        if (((_event.buttons() & QtCore.Qt.LeftButton) and (_event.buttons() & QtCore.Qt.MidButton))
+            or (_event.buttons() & QtCore.Qt.LeftButton and _event.modifiers() & QtCore.Qt.ControlModifier)):
+            value_y = self.radius_ * dy * 2.0 / h;
+            self.translate([0.0, 0.0, value_y])
+
+        #rotate
+        elif (_event.buttons() & QtCore.Qt.MidButton
+              or (_event.buttons() & QtCore.Qt.LeftButton and _event.modifiers() & QtCore.Qt.ShiftModifier)):
+            #中间键  旋转
+            '''if (not self.isInRotation_):
+                self.isInRotation_ = True
+                self.rotationBeginEvent.emit()
+
+            axis = [0.0, 0.0, 0.0]
+            angle = 0.0
+
+            if (self.last_point_ok_ and newPoint_hitSphere):
+                axis = numpy.cross(self.last_point_3D_, newPoint3D)
+                cos_angle = numpy.dot(self.last_point_3D_, newPoint3D)
+                if (abs(cos_angle) < 1.0):
+                    angle = math.acos(cos_angle) * 180.0 / math.pi
+                    angle *= 2.0
+                self.rotate(axis, angle)'''
+
+        #  move in x,y direction
+        elif (_event.buttons() & QtCore.Qt.LeftButton):
+            #左键移动
+
+            z = - (self.modelview_matrix_[0][2] * self.center_[0] +
+                   self.modelview_matrix_[1][2] * self.center_[1] +
+                   self.modelview_matrix_[2][2] * self.center_[2] +
+                   self.modelview_matrix_[3][2]) / (self.modelview_matrix_[0][3] * self.center_[0] +
+                                                    self.modelview_matrix_[1][3] * self.center_[1] +
+                                                    self.modelview_matrix_[2][3] * self.center_[2] +
+                                                    self.modelview_matrix_[3][3])
+
+            fovy   = 45.0
+            aspect = w / h
+            n      = 0.01 * self.radius_
+            up     = math.tan(fovy / 2.0 * math.pi / 180.0) * n
+            right  = aspect * up
+
+            self.translate( [2.0 * dx / w * right / n * z,
+                             -2.0 * dy / h * up / n * z,
+                             0.0] )
+
+
+
+
+        # remember this point
+        self.last_point_2D_ = newPoint2D
+        self.last_point_3D_ = newPoint3D
+        self.last_point_ok_ = newPoint_hitSphere
+
+        # trigger redraw
+        self.updateGL()
+
+    def mouseReleaseEvent(self, _event):
+        if (self.isInRotation_):
+            self.isInRotation_ = False
+            self.rotationEndEvent.emit()
+        last_point_ok_ = False
+
+#===============================================================================
+#
+# Local Variables:
+# mode: Python
+# indent-tabs-mode: nil
+# End:
+#
+#===============================================================================

dijkstra/Map.py

@@ -0,0 +1,187 @@
+import math
+from dijkstra.dijkstra_algorithm import Graph
+import message_pb2 as message
+
+
+class Node(object):
+    def __init__(self, id, x, y):
+        self.id_ = id
+        self.x_ = x
+        self.y_ = y
+
+    def distance(self, other):
+        if not isinstance(other, (Node, StreetNode, SpaceNode)):
+            print(" node must be Node,street_node,space_node")
+            return -1
+        return math.sqrt(math.pow(other.x_ - self.x_, 2) + math.pow(other.y_ - self.y_, 2))
+
+
+class StreetNode(Node):
+    def __init__(self, id, x, y):
+        Node.__init__(self, id, x, y)
+
+
+class SpaceNode(Node):
+    def __init__(self, id, x, y, connect_nodeId):
+        Node.__init__(self, id, x, y)
+        self.connect_nodeId_ = connect_nodeId
+
+
+def singleton(cls):
+    _instance = {}
+
+    def inner():
+        if cls not in _instance:
+            _instance[cls] = cls()
+        return _instance[cls]
+
+    return inner
+
+
+'''
+map 采用单例
+'''
+
+
+@singleton
+class Map(object):
+    def __init__(self):
+        self.nodes_ = {}
+        self.graph_ = Graph()
+
+    def AddVertex(self, node):
+        if isinstance(node, (StreetNode)):
+            print("add street node :%s " % (node.id_))
+            self.nodes_[node.id_] = node
+
+        if isinstance(node, (SpaceNode)):
+            if self.nodes_.get(node.connect_nodeId_) == None:
+                print("add space node failed ,connect street node not exist")
+                return False
+            print("add space node :%s  connect_id:%s" % (node.id_, node.connect_nodeId_))
+            self.nodes_[node.id_] = node
+
+        self.graph_.AddVertex(node.id_, [node.x_, node.y_])
+        return True
+
+    def AddEdge(self, id1, id2, direct=False):
+        if self.nodes_.get(id1) == None or self.nodes_.get(id2) == None:
+            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)
+        if direct == False:
+            self.graph_.AddEdge(id2, id1)
+
+    def GetShortestPath(self, beg, end):
+        [pathId, distance] = self.graph_.shortest_path(beg, end)
+        print("distance:", distance)
+        print("path:", pathId)
+        path = []
+        for nodeId in pathId:
+            node = self.nodes_[nodeId]
+            path.append(node)
+        return path
+
+    def CreateNavCmd(self, pose, path):
+        if len(path) <= 1:
+            return None
+
+        cmd = message.NavCmd()
+        cmd.action=0  # 新导航
+        key = "12354-ahsbd-kkkk"
+        cmd.key=(key)
+        adjustdiff = message.Pose2d()
+        node_mpcdiff = message.Pose2d()
+        enddiff = message.Pose2d()
+
+        # 目标点精度设置
+        adjustdiff.x=(0.1)
+        adjustdiff.y=(0.05)
+        adjustdiff.theta=(0.5 * math.pi / 180.0)
+
+        node_mpcdiff.x=(0.1)
+        node_mpcdiff.y=(0.1)
+        node_mpcdiff.theta=(2 * math.pi / 180.0)
+
+        enddiff.x=(0.1)
+        enddiff.y=(0.1)
+        enddiff.theta=(5 * 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=(1)
+        angular_limit.max=(25.0)
+        # mpc速度限制
+        mpc_x_limit = message.SpeedLimit()
+        mpc_angular_limit = message.SpeedLimit()
+        mpc_x_limit.min=(0.05)
+        mpc_x_limit.max=(1.5)
+
+        mpc_angular_limit.min=(0 * math.pi / 180.0)
+        mpc_angular_limit.max=(3 * math.pi / 180.0)
+
+        # 创建动作集----------------------
+
+        last_node = None
+        count = 0
+        for node in path:
+            if last_node == None:
+                last_node = node
+                count+=1
+                continue
+
+            # 运动到上一点
+            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
+            if isinstance(last_node, (SpaceNode)):
+                yaw = yaw + math.pi
+            # 添加调整动作
+            act_adjust = message.Action()
+            act_adjust.type=(1)
+
+            act_adjust.target.x=(last_node.x_)
+            act_adjust.target.y=(last_node.y_)
+            act_adjust.target.theta=(yaw)
+
+            act_adjust.target_diff.CopyFrom(adjustdiff)
+            act_adjust.velocity_limit.CopyFrom(v_limit)
+            act_adjust.horize_limit.CopyFrom(horize_limit)
+            act_adjust.angular_limit.CopyFrom(angular_limit)
+            cmd.actions.add().CopyFrom(act_adjust)
+
+            # 添加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.target_diff.CopyFrom(enddiff)
+            else:
+                act_along.target_diff.CopyFrom(node_mpcdiff)
+            act_along.velocity_limit.CopyFrom(mpc_x_limit)
+            act_along.angular_limit.CopyFrom(mpc_angular_limit)
+            cmd.actions.add().CopyFrom(act_along)
+
+            last_node = node
+            count+=1
+
+        return cmd

dijkstra/dijkstra_algorithm.py

@@ -0,0 +1,161 @@
+"""Python 3 implementation of Djikstra's algorithm for finding the shortest
+path between nodes in a graph. Written as a learning exercise, so lots of
+comments and no error handling.
+"""
+from collections import deque
+import math
+INFINITY = float("inf")
+
+
+class Graph:
+    def __init__(self):
+        """Reads graph definition and stores it. Each line of the graph
+        definition file defines an edge by specifying the start node,
+        end node, and distance, delimited by spaces.
+
+        Stores the graph definition in two properties which are used by
+        Dijkstra's algorithm in the shortest_path method:
+        self.nodes = set of all unique nodes in the graph
+        self.adjacency_list = dict that maps each node to an unordered set of
+        (neighbor, distance) tuples.
+        """
+
+        # Read the graph definition file and store in graph_edges as a list of
+        # lists of [from_node, to_node, distance]. This data structure is not
+        # 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()
+
+    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))
+            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
+        self.graph_edges.append((name1, name2, distance))
+        self.nodes.update([name1, name2])
+
+        self.adjacency_list = {node: set() for node in self.nodes}
+        for edge in self.graph_edges:
+            self.adjacency_list[edge[0]].add((edge[1], edge[2]))
+        return True
+
+    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).
+        """
+
+        unvisited_nodes = self.nodes.copy()  # All nodes are initially unvisited.
+
+        # Create a dictionary of each node's distance from start_node. We will
+        # update each node's distance whenever we find a shorter path.
+        distance_from_start = {
+            node: (0 if node == start_node else INFINITY) for node in self.nodes
+        }
+
+        # Initialize previous_node, the dictionary that maps each node to the
+        # node it was visited from when the the shortest path to it was found.
+        previous_node = {node: None for node in self.nodes}
+
+        while unvisited_nodes:
+            # Set current_node to the unvisited node with shortest distance
+            # calculated so far.
+            current_node = min(
+                unvisited_nodes, key=lambda node: distance_from_start[node]
+            )
+            unvisited_nodes.remove(current_node)
+
+            # If current_node's distance is INFINITY, the remaining unvisited
+            # nodes are not connected to start_node, so we're done.
+            if distance_from_start[current_node] == INFINITY:
+                break
+
+            # For each neighbor of current_node, check whether the total distance
+            # to the neighbor via current_node is shorter than the distance we
+            # currently have for that node. If it is, update the neighbor's values
+            # for distance_from_start and previous_node.
+            for neighbor, distance in self.adjacency_list[current_node]:
+                new_path = distance_from_start[current_node] + distance
+                if new_path < distance_from_start[neighbor]:
+                    distance_from_start[neighbor] = new_path
+                    previous_node[neighbor] = current_node
+
+            if current_node == end_node:
+                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
+        # appendleft with O(1) performance.
+        path = deque()
+        current_node = end_node
+        while previous_node[current_node] is not None:
+            path.appendleft(current_node)
+            current_node = previous_node[current_node]
+        path.appendleft(start_node)
+
+        return path, distance_from_start[end_node]
+
+'''
+def main():
+    """Runs a few simple tests to verify the implementation.
+    """
+    verify_algorithm(
+        filename="simple_graph.txt",
+        start="A",
+        end="G",
+        path=["A", "D", "E", "G"],
+        distance=11,
+    )
+    verify_algorithm(
+        filename="seattle_area.txt",
+        start="Renton",
+        end="Redmond",
+        path=["Renton", "Factoria", "Bellevue", "Northup", "Redmond"],
+        distance=16,
+    )
+    verify_algorithm(
+        filename="seattle_area.txt",
+        start="Seattle",
+        end="Redmond",
+        path=["Seattle", "Eastlake", "Northup", "Redmond"],
+        distance=15,
+    )
+    verify_algorithm(
+        filename="seattle_area.txt",
+        start="Eastlake",
+        end="Issaquah",
+        path=["Eastlake", "Seattle", "SoDo", "Factoria", "Issaquah"],
+        distance=21,
+    )
+
+
+def verify_algorithm(filename, start, end, path, distance):
+    """Helper function to run simple tests and print results to console.
+
+    filename = graph definition file
+    start/end = path to be calculated
+    path = expected shorted path
+    distance = expected distance of path
+    """
+    graph = Graph(filename)
+    returned_path, returned_distance = graph.shortest_path(start, end)
+
+    assert list(returned_path) == path
+    assert returned_distance == distance
+
+    print('\ngraph definition file: {0}'.format(filename))
+    print('      start/end nodes: {0} -> {1}'.format(start, end))
+    print('        shortest path: {0}'.format(path))
+    print('       total distance: {0}'.format(distance))
+
+
+if __name__ == "__main__":
+    main()'''

map.json

@@ -0,0 +1,25 @@
+
+{
+  "street_nodes":{
+    "node1":[3.3,3.5],
+    "node2":[10.3,3.5],
+    "node3": [15.4,3.5],
+    "node4":[17.9,3.5],
+    "node5":[10.3,17.7],
+    "node6":[14.95,17.7],
+    "node7":[17.9,17.7],
+    "node8":[10.3,-0.18],
+    "node11":[18.8,3.5]
+    
+  },
+  
+  "space_nodes":{
+  	"A1":[15.4,7.5,"node3"],
+  	"B1":[14.85,13.7,"node6"]
+  },
+  "roads":{
+    "road1":["node1","node2","node3","node4","node11"],
+    "road2":["node8","node5","node2"],
+    "road3":["node5","node6","node7"]
+  }
+}

message.proto

@@ -0,0 +1,65 @@
+syntax = "proto3";
+package NavMessage;
+message AGVStatu {
+  float x=1;
+  float y=2;
+  float theta=3;
+  float v=4;
+  float vth=5;
+}
+
+message Speed {
+  int32 H=1;  //心跳
+  int32 T=2; // 1:原地旋转，2:横移，3:MPC巡线/前进, 其他/未接收到：停止
+  float V=3;  //角速度
+  float W=4;  //线速度
+  int32 end=5;
+}
+
+message SpeedLimit
+{
+  float min=1;
+  float max=2;
+}
+
+message Pose2d
+{
+  float x=1;
+  float y=2;
+  float theta=3;
+}
+
+message Trajectory
+{
+  repeated Pose2d poses=1;
+}
+
+message Action
+{
+  int32 type = 1; // 1:原地调整，2：巡线
+  Pose2d begin = 2;
+  Pose2d target = 3;
+  Pose2d target_diff = 4;
+  SpeedLimit velocity_limit=5;
+  SpeedLimit angular_limit=6;
+  SpeedLimit horize_limit=7;
+}
+
+
+message NavCmd
+{
+  int32 action=1;  //  0 开始导航，1 pause， 2 continue ，3 cancel
+  string key=2;
+  repeated Action actions=3;
+}
+
+message NavStatu
+{
+  bool statu = 1; //0:ready  1:running
+  string key = 2; // 任务唯一码
+  repeated Action unfinished_actions = 3;  //未完成的动作，第一个即为当前动作
+  Trajectory selected_traj = 4;
+  Trajectory predict_traj = 5;
+
+}
+

mqtt_async.py

@@ -0,0 +1,40 @@
+from paho.mqtt import client as mqtt_client
+
+
+class MqttAsync(object):
+    def __init__(self,client_id):
+        self.client_id=client_id
+        self.connected=False
+    def connect(self,ip,port,user,password):
+        def on_connect(client, userdata, flags, rc):
+            if rc == 0:
+                print("Connected to MQTT Broker!")
+            else:
+                print("Failed to connect, return code %d\n", rc)
+
+        self.client = mqtt_client.Client(self.client_id)
+        self.client.username_pw_set(user, password)
+        self.client.on_connect = on_connect
+        self.client.connect(ip, port)
+        self.connected=True
+
+    def publish(self,topic,msg):
+        if self.connected==False:
+            print("mqtt is not connected Failed to send message to topic {topic}")
+        result = self.client.publish(topic, msg,qos=1)
+        status = result[0]
+        if status == 0:
+            print("Send :%s"%(msg))
+        else:
+            print(f"Failed to send message to topic {topic}")
+
+
+    def subscribe(self,topic,callback):
+        def on_message(client, userdata, msg):
+            pass
+        self.client.subscribe(topic)
+        self.client.on_message = callback
+
+    def loop_start(self):
+        self.client.loop_start()
+

parameter.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package AGV;
+message AGV_parameter
+{
+    string NodeId=1;
+    string ip=2;
+    int32 port=3;
+    string subStatuTopic=4;
+    string pubNavCmdTopic=5;
+    string pubClusterNavCmdTopic=6;		
+
+}
+

proto.sh

@@ -0,0 +1,2 @@
+
+protoc -I=./ message.proto --python_out=./

test.py

@@ -0,0 +1,243 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#===============================================================================
+#
+# test.py
+#
+# Test program for the simple GL Viewer.
+#
+# Copyright (c) 2011, Arne Schmitz <arne.schmitz@gmx.net>
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the <organization> nor the
+#       names of its contributors may be used to endorse or promote products
+#       derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+#===============================================================================
+import math
+import time
+import sys
+from PyQt5.QtGui import *
+from PyQt5.QtWidgets import *
+from PyQt5.QtCore import *
+from MapGLWidget import MapGLWidget,TimeStatu
+import json
+import dijkstra.Map as mp
+import ControllWidget
+import mqtt_async as mqtt
+import message_pb2 as message
+import google.protobuf.json_format as jtf
+#===============================================================================
+
+
+class MainWindow(QMainWindow):
+    """docstring for Mainwindow"""
+
+    def __init__(self, parent = None):
+        super(MainWindow,self).__init__(parent)
+        self.basic()
+
+
+        splitter_main = self.split_()
+        self.setCentralWidget(splitter_main)
+
+        self.path=None
+
+        self.agv1_statu=TimeStatu(None,0.1)
+        self.agv2_statu=TimeStatu(None,0.1)
+        self.mqtt_=mqtt.MqttAsync("UI")
+        self.mqtt_.connect("127.0.0.1",1883,"admin","zx123456")
+        self.mqtt_.subscribe("agv1_child/agv_statu",self.agv_statu_arrived)
+        self.mqtt_.loop_start()
+
+
+
+    def agv_statu_arrived(self,client,user_data,msg):
+        statu=message.AGVStatu()
+        jtf.Parse(msg.payload.decode(),statu)
+        self.gl.ResetAgv1(statu)
+        self.agv1_statu=TimeStatu(statu,0.3)
+
+
+    def load_map(self,file):
+        with open(file,'r',encoding='utf-8') as fp:
+            map=json.load(fp)
+            return map
+
+
+    #窗口基础属性
+    def basic(self):
+        #设置标题，大小，图标
+        self.setWindowTitle("GT")
+        self.resize(1100,650)
+        self.setWindowIcon(QIcon("./image/Gt.png"))
+        #居中显示
+        screen = QDesktopWidget().geometry()
+        self_size = self.geometry()
+        self.move(int((screen.width() - self_size.width())/2),int((screen.height() - self_size.height())/2))
+
+    def closeEvent(self, QCloseEvent):
+        self.gl.close()
+
+
+    @staticmethod
+    def Create_traj(path,delta):
+        last_node=None
+        trajectry=[]
+        for node in path:
+            if last_node==None:
+                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,(mp.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
+        return trajectry
+
+    def generater_path(self,beg,end):
+        self.path=self.djks_map_.GetShortestPath(beg,end)
+        self.traj=self.Create_traj(self.path,0.5)
+        self.gl.AddTraj("agv1",self.traj)
+        self.gl.update()
+
+    def send_cmd(self):
+        if self.path is not None and self.agv1_statu.timeout()==False:
+            statu=self.agv1_statu.statu
+            if statu is not None:
+                cmd=self.djks_map_.CreateNavCmd([statu.x,statu.y,statu.theta],self.path)
+                if cmd is not None:
+                    self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))
+                else:
+                    print(" NavCmd is None")
+
+    def Cancel_cmd(self):
+        cmd=message.NavCmd()
+        cmd.action=3
+        self.mqtt_.publish("agv1_child/nav_cmd",jtf.MessageToJson(cmd))
+
+
+    #分割窗口
+    def split_(self):
+        splitter = QSplitter(Qt.Vertical)
+        self.gl = MapGLWidget()   #将opengl例子嵌入GUI
+        map=self.load_map("./map.json")
+        UI_data={}
+        UI_data["nodes"]=[]
+        self.djks_map_=mp.Map()
+
+        for node in map['street_nodes'].items():
+            [id,point]=node
+            street_node=mp.StreetNode(id,point[0],point[1])
+            self.djks_map_.AddVertex(street_node)
+            self.gl.AddNode([id,"street_node",point])
+            UI_data["nodes"].append(id)
+        for node in map['space_nodes'].items():
+            [id,point]=node
+            [x,y,connectId]=point
+            space_node=mp.SpaceNode(id,point[0],point[1],connectId)
+            self.djks_map_.AddVertex(space_node)
+            self.djks_map_.AddEdge(id,connectId)
+            glNode=[id,"space_node",[x,y]]
+            self.gl.AddNode(glNode)
+            self.gl.AddRoad(["conn%s-%s"%(id,connectId),[id,connectId]])
+            UI_data["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.djks_map_.AddEdge(pt1,pt2)
+
+        splitter.addWidget(self.gl)
+
+        testedit = QTextEdit()
+        splitter.addWidget(testedit)
+        splitter.setStretchFactor(0,3)
+        splitter.setStretchFactor(1,2)
+        splitter_main = QSplitter(Qt.Horizontal)
+        Controller = ControllWidget.Frame(UI_data)
+
+        callbacks={}
+        callbacks["CreateTraj"]=self.generater_path
+        callbacks["SendCmd"]=self.send_cmd
+        callbacks["CancelCmd"]=self.Cancel_cmd
+
+
+        Controller.SetBtnFunc(callbacks)
+        splitter_main.addWidget(Controller)
+
+
+        splitter_main.addWidget(splitter)
+        splitter_main.setStretchFactor(0,1)
+        splitter_main.setStretchFactor(1,4)
+        return splitter_main
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+    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:
+# mode: Python
+# indent-tabs-mode: nil
+# End:
+#
+#===============================================================================