AGVUI_python/dijkstra/Map.py

import math
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
        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 DijikstraMap(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 VertexDict(self):
        return self.nodes_
    def Edges(self):
        return self.graph_.graph_edges
    def findNeastNode(self,pt):
        labels=[]
        pts=[]
        for item in self.nodes_.items():
            [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]]

    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

    @staticmethod
    def CreatePath(pathNodes,delta):
        last_node=None
        trajectry=[]
        for node in pathNodes:
            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,(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
    @staticmethod
    def CreateNavCmd(pose, path):
        if len(path) <= 1:
            return None

        cmd = message.NavCmd()
        cmd.action=0  # 新导航
        key = str(uuid.uuid4())
        cmd.key=(key)
        adjustdiff = message.Pose2d()
        node_mpcdiff = message.Pose2d()
        enddiff = message.Pose2d()
        lastAdjustDiff=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=(10 * math.pi / 180.0)

        enddiff.x=(0.1)
        enddiff.y=(0.1)
        enddiff.theta=(5 * math.pi / 180.0)

        lastAdjustDiff.x=(0.03)
        lastAdjustDiff.y=(0.03)
        lastAdjustDiff.theta=(0.3 * 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)
            #最后一个调整点
            if count==len(path)-2:
                act_adjust.target_diff.CopyFrom(lastAdjustDiff)
            else:
                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