AGVUI_python/RobotData.py

import enum
import time
import message_pb2 as message
from mqtt_async import MqttAsync
import google.protobuf.json_format as jtf
from dijkstra.Map import DijikstraMap,SpaceNode,StreetNode
import uuid
import random
import math
import Count

class ActType(enum.Enum):
    eReady=0
    eRotation=1
    eHorizon=2
    eVertical=3
    eMPC=4
    eClampOpen=5
    eClampClose=6


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 Robot(MqttAsync):

    def __init__(self, name=""):
        MqttAsync.__init__(self)
        self.timedRobotStatu_ = TimeStatu(message.RobotStatu, 0)
        self.timedNavStatu_ = TimeStatu(message.NavStatu, 0)
        self.dataTopic_ = {}
        self.messageArrivedSignal = None
        self.currentNavData_ = None
        self.navCmdTopic_ = None
        self.currentNavPathNodes_ = None
        self.currentNavPath_ = None

        self.pathColor_ = [1, 1, 0]
        self.robotColor_ = [0.7, 0.2, 0.3]  # agv当前颜色
        self.name_ = name

        self.begId_="------"
        self.targetId_="------"

        self.autoTest_=False

        self.l_ = 0.8  # 轮长
        self.L_ = 1.3  # 轴距
        self.W_ = 2.5  # 宽

    def Color(self):
        if self.IsMainModeStatu():
            return [0, 0, 0]
        return self.robotColor_

    def IsMainAgv(self):
        if self.timedNavStatu_.timeout() == False:
            if self.timedNavStatu_.statu.main_agv == True:
                return True
        return False

    def IsMainModeStatu(self):
        if self.IsMainAgv():
            if self.timedNavStatu_.timeout() == False:
                if self.timedNavStatu_.statu.move_mode == 2:
                    return True
        return False

    def SetSubDataTopic(self, match: dict, messageSignal=None):
        self.dataTopic_ = match
        self.messageArrivedSignal = messageSignal
        for item in match.items():
            [topic, _] = item
            self.subscribe(topic, self.on_message)

    def SetCmdTopic(self, topic):
        self.navCmdTopic_ = topic

    def SetColor(self, pathColor, robotColor):
        self.pathColor_ = pathColor
        self.robotColor_ = robotColor

    def ResetPose(self, agv: message.RobotStatu):
        self.timedRobotStatu_ = TimeStatu(agv, 2)

    def ResetNavStatu(self, statu: message.NavStatu):
        self.timedNavStatu_ = TimeStatu(statu, 2)

    def on_message(self, client, userdata, msg):
        topic = msg.topic
        if self.dataTopic_.get(topic) is not None:
            dtype = self.dataTopic_[topic]
            if dtype == message.RobotStatu.__name__:
                proto = message.RobotStatu()
                jtf.Parse(msg.payload.decode(), proto)
                self.ResetPose(proto)
            if dtype == message.NavStatu.__name__:
                self.last_running = self.IsNavigating()
                proto = message.NavStatu()
                jtf.Parse(msg.payload.decode(), proto)
                self.ResetNavStatu(proto)
                if self.last_running == True and self.IsNavigating() == False:
                    self.currentNavPathNodes_ = None
                    self.currentNavPath_ = None
            if self.messageArrivedSignal is not None:
                self.messageArrivedSignal()

    def MpcSelectTraj(self):
        traj = []
        if self.timedNavStatu_.timeout() == False:
            nav = self.timedNavStatu_.statu
            for pose2d in nav.selected_traj.poses:
                traj.append([pose2d.x, pose2d.y, pose2d.theta])
        return traj

    def MpcPredictTraj(self):
        traj = []
        if self.timedNavStatu_.timeout() == False:
            nav = self.timedNavStatu_.statu
            for pose2d in nav.predict_traj.poses:
                traj.append([pose2d.x, pose2d.y, pose2d.theta])
        return traj

    def Connected(self):
        return self.timedNavStatu_.timeout() == False and self.timedRobotStatu_.timeout() == False

    def IsNavigating(self):
        return not self.ActionType()==ActType.eReady
        '''if self.timedNavStatu_.timeout() == False:
            key = self.timedNavStatu_.statu.key
            if key == "" or key == None:
                return False
        return True'''

    def GeneratePath(self, begID, endID):
        self.currentNavPathNodes_ = DijikstraMap().GetShortestPath(begID, endID)
        self.currentNavPath_ = DijikstraMap().CreatePath(self.currentNavPathNodes_, 0.5)
        if self.currentNavPathNodes_ is not None and self.currentNavPath_ is not None:
            self.begId_=begID
            self.targetId_=endID
            return True
        return False


    def PositionId(self):
        if self.timedRobotStatu_.timeout():
            return None
        x=self.timedRobotStatu_.statu.x
        y=self.timedRobotStatu_.statu.y
        djks_map = DijikstraMap()
        [label, pt] = djks_map.findNeastNode([x, y])
        return [label,pt]
    '''
    阻塞直到导航完成
    '''
    def Navigatting(self, begId, targetId,autoDirect):
        print("nav")
        self.GeneratePath(begId, targetId)
        #self.ExecPathNodes(autoDirect)
        self.ExecNavtask(autoDirect)
        while self.IsNavigating() == True:
            '''if self.Connected() == False:
                print("robot disconnected cancel task")
                self.CancelNavTask()
                return False'''
            time.sleep(0.5)
        print(" Nav completed!!!")

        if self.IsMainModeStatu():
            Count.TestCount().loadCountAdd()
        else:
            Count.TestCount().singleCountAdd()

        return True

    def AutoTestNavClamp(self,begId,targetId):
        beg=begId
        target=targetId
        while self.autoTest_:
            if self.TestNavClampOnce(beg,target)==False:
                print("  quit auto Test")
                break
            print("1111")
            posInfo=self.PositionId()
            if posInfo is not None:
                [label,pt]=posInfo
                beg=label
                print("beg",beg)
                node=DijikstraMap().GetVertex(beg)

                if node is not None:
                    if isinstance(node,(SpaceNode)):
                        target="O"
                    if isinstance(node,(StreetNode)):
                        end_nodes=["E","B","C","D"]
                        id=random.randint(0,1000)%4
                        target=end_nodes[id]
                    print("3333",target)
            print("  Next nav clamp cmd,%s to %s"%(beg,target))

    def ActionType(self):
        if self.timedNavStatu_.timeout() == False:
            runningStatu=self.timedNavStatu_.statu
            if runningStatu.statu==0:
                return ActType.eReady
            if runningStatu.statu==1:
                return ActType.eRotation
            if runningStatu.statu==2:
                return ActType.eHorizon
            if runningStatu.statu==3:
                return ActType.eVertical
            if runningStatu.statu==4:
                return ActType.eMPC
            if runningStatu.statu==5:
                return ActType.eClampOpen
            if runningStatu.statu==6:
                return ActType.eClampClose
        else:
            return ActType.eReady

    @staticmethod
    def generateProtoNode(node,accuracy):
        [dx,dy,dyaw]=accuracy
        protoNode=message.PathNode()
        protoNode.pose.x=node.x_
        protoNode.pose.y=node.y_
        protoNode.accuracy.x=dx
        protoNode.accuracy.y=dx
        protoNode.accuracy.theta=dyaw
        return protoNode
    def ExecNavtask(self,autoDirect):
        cmd=message.NavCmd()
        key = str(uuid.uuid4())
        cmd.key = key
        cmd.action=0

        limitMPC_v=message.SpeedLimit()
        limitRotate=message.SpeedLimit()
        limAdjustV=message.SpeedLimit()
        limitAdjustH=message.SpeedLimit()
        limitInOutV=message.SpeedLimit()
        limitMPC_v.min=0.03
        limitMPC_v.max=1.2
        limitRotate.min=3*math.pi/180.0
        limitRotate.max=25*math.pi/180.0
        limAdjustV.min=0.03
        limAdjustV.max=0.5
        limitAdjustH.min=0.03
        limitAdjustH.max=0.3
        limitInOutV.min=0.03
        limitInOutV.max=0.5

        actions=self.SplitPath(self.currentNavPathNodes_)
        for action in actions:
            [type,nodes]=action
            if type=="input":
                [street,space]=nodes
                protoSpace=self.generateProtoNode(space,[0.01,0.01,0.3*math.pi/180.0])
                protoStreet=self.generateProtoNode(street,[0.02,0.02,1*math.pi/180.0])
                act=message.NewAction()
                act.type=1
                act.spaceNode.CopyFrom(protoSpace)
                act.streetNode.CopyFrom(protoStreet)
                act.NodeAngularLimit.CopyFrom(limitRotate)
                act.InOutVLimit.CopyFrom(limitInOutV)
                act.adjustVelocitylimit.CopyFrom(limAdjustV)
                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
                cmd.newActions.add().CopyFrom(act)
            if type=="output":
                [space,street]=nodes
                protoSpace=self.generateProtoNode(space,[0.02,0.2,0.5*math.pi/180.0])
                protoStreet=self.generateProtoNode(street,[0.05,0.05,1*math.pi/180.0])
                act=message.NewAction()
                act.type=2
                act.spaceNode.CopyFrom(protoSpace)
                act.streetNode.CopyFrom(protoStreet)
                act.InOutVLimit.CopyFrom(limitInOutV)
                act.NodeAngularLimit.CopyFrom(limitRotate)
                act.adjustVelocitylimit.CopyFrom(limAdjustV)
                act.adjustHorizonLimit.CopyFrom(limitAdjustH)
                cmd.newActions.add().CopyFrom(act)
            if type=="nav":
                action =self.CreateNavPathNodesAction(nodes,autoDirect)
                if action is None:
                    print("Nav cmd is None")
                    return False
                else:
                    cmd.newActions.add().CopyFrom(action)

        if cmd is None:
            print("Nav cmd is None")
            return False

        count=3
        while self.ActionType() == ActType.eReady and count>0:
            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            count-=1
            time.sleep(1)
        print("send nav cmd completed!!!")
        return True
    def CreateNavPathNodesAction(self,path,autoDirect):
        if path is not None and self.timedRobotStatu_.timeout() == False:
            for node in path:
                if isinstance(node,(SpaceNode)):
                    print(" nodes must all street node")
                    return None
            statu = self.timedRobotStatu_.statu
            if statu is not None:
                newAction=message.NewAction()
                limitMPC_v=message.SpeedLimit()
                limitRotate=message.SpeedLimit()
                limAdjustV=message.SpeedLimit()
                limitAdjustH=message.SpeedLimit()
                limitMPC_v.min=0.03
                limitMPC_v.max=1.5
                limitRotate.min=3*math.pi/180.0
                limitRotate.max=25*math.pi/180.0
                limAdjustV.min=0.03
                limAdjustV.max=0.3
                limitAdjustH.min=0.03
                limitAdjustH.max=0.3

                count=0
                size=len(path)
                for i in range(size):
                    node=path[i]
                    pose=message.Pose2d()
                    accuracy=message.Pose2d()
                    pose.x=node.x_
                    pose.y=node.y_
                    if i+1<size:
                        next=path[i+1]
                        vector = [next.x_ - node.x_, next.y_ - 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
                        pose.theta=yaw
                    else:
                        pose.theta=0

                    if self.IsMainModeStatu() and count==0:
                        accuracy.x=0.05
                        accuracy.y=0.1
                        accuracy.theta=0.5*math.pi/(180.0)

                    if count==size-1:
                        accuracy.x=0.02
                        accuracy.y=0.02
                        accuracy.theta=3*math.pi/(180.0)
                    else:
                        accuracy.x=0.05
                        accuracy.y=0.1
                        accuracy.theta=5*math.pi/180.0
                    count+=1

                    pathNode=message.PathNode()
                    pathNode.pose.CopyFrom(pose)
                    pathNode.accuracy.CopyFrom(accuracy)
                    newAction.type=4
                    if autoDirect:
                        newAction.type=3
                    newAction.pathNodes.add().CopyFrom(pathNode)
                    newAction.NodeVelocityLimit.CopyFrom(limitMPC_v)
                    newAction.NodeAngularLimit.CopyFrom(limitRotate)
                    newAction.adjustVelocitylimit.CopyFrom(limAdjustV)
                    newAction.adjustHorizonLimit.CopyFrom(limitAdjustH)
                return newAction
            else:
                print("statu is None")
        else:
            print("current path is none")
        return None
    def ExecPathNodes(self,autoDirect):
        if self.navCmdTopic_ == None:
            print("Robot has not set nav cmd topic")
            return False
        if not self.ActionType()==ActType.eReady:
            print(" robot is navigating ,key:%s" % (self.timedNavStatu_.statu.key))
            return False

        cmd = message.NavCmd()
        key = str(uuid.uuid4())
        cmd.key = key
        cmd.action=0

        action =self.CreateNavPathNodesAction(self.currentNavPathNodes_,autoDirect)
        if action is None:
            print("Nav cmd is None")
            return False
        cmd.newActions.add().CopyFrom(action)

        print(cmd)
        published=False
        while self.IsNavigating() == False:
            if not self.ActionType() == ActType.eReady:
                published=True
            if published==False:
                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            time.sleep(1)
        print("send nav cmd completed!!!")
        return True

    '''
    拆分路径，生成动作集,input:入库动作，nav:导航动作，output:出库动作
    '''
    def SplitPath(self,path):

        space_count=0
        for i in range(len(path)):
            if isinstance(path[i],(SpaceNode)):
                space_count+=1
                if i==0 or i==len(path)-1:
                    pass
                else:
                    print("Error: space node is not first/last of path")
                    return None

        actions=[]
        lastNode=None

        navIndex=-1
        for node in path:
            if lastNode==None:
                lastNode=node
                continue

            if isinstance(node,(SpaceNode)):    #当前点是车位点
                if isinstance(lastNode,(SpaceNode)):  #上一点为车位点
                    print("Error: last node and current node is space node ")
                    lastNode=None
                    continue
                elif isinstance(lastNode,(StreetNode)):    #上一点为马路点,则动作为入库
                    actions.append(["input",[lastNode,node]])
                    lastNode=None
                    continue

            if isinstance(node,(StreetNode)):
                if isinstance(lastNode,(SpaceNode)):  #上一点为车位点, 出库
                    actions.append(["output",[lastNode,node]])

                elif isinstance(lastNode,(StreetNode)):
                    if navIndex<0:
                        actions.append(["nav",[lastNode]])
                        navIndex=len(actions)-1
                    actions[navIndex][1].append(node)
            lastNode=node

        return actions

    def SwitchMoveMode(self, mode, wheelbase):
        if self.IsMainAgv() == False:
            print(" this agv is single can not switch mode")
            return False
        cmd = message.NavCmd()
        key = str(uuid.uuid4())
        cmd.key = key
        cmd.action=0

        action=message.NewAction()
        action.type=8
        action.changedMode = mode
        if mode == 2:
            action.wheelbase = wheelbase
        cmd.newActions.add().CopyFrom(action)
        loop=3
        while loop>0:
            if mode == 2:
                if self.IsMainModeStatu():
                    return True
            if mode == 1:
                if self.IsMainModeStatu() == False:
                    return True
            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            loop-=1
            time.sleep(0.5)
        return False

    def IsClampClosed(self):
        if self.timedRobotStatu_.timeout() == False:
            if self.IsMainModeStatu():
                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
                return (clamp == 1 and clamp_other == 1)
            else:
                return self.timedRobotStatu_.statu.agvStatu.clamp == 1

        return False
    def IsClampRunning(self):
        if self.timedRobotStatu_.timeout() == False:
            if self.IsMainModeStatu():
                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
                return (clamp == 0 or clamp_other == 0)
            else:
                return self.timedRobotStatu_.statu.agvStatu.clamp == 0

        return False

    def IsClampOpened(self):
        if self.timedRobotStatu_.timeout() == False:
            if self.IsMainModeStatu():
                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
                return (clamp == 2 and clamp_other == 2)
            else:
                return self.timedRobotStatu_.statu.agvStatu.clamp == 2

        return False

    def ClampClose(self):
        if self.IsClampClosed() == True:
            print("clamp closed")
            return True
        cmd = message.NavCmd()
        cmd.action=0
        key = str(uuid.uuid4())
        cmd.key = (key)
        act = message.NewAction()
        act.type = 6
        cmd.newActions.add().CopyFrom(act)
        published=False
        while self.IsClampClosed()==False:
            if self.ActionType()==ActType.eClampClose:
                published=True
            if published==False:
                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            time.sleep(0.5)
        return True

    def ClampOpen(self):
        if self.IsClampOpened() == True:
            print("clamp opended")
            return True
        cmd = message.NavCmd()
        cmd.action=0
        key = str(uuid.uuid4())
        cmd.key = (key)
        act = message.NewAction()
        act.type = 7
        cmd.newActions.add().CopyFrom(act)
        published=False
        while self.IsClampOpened()==False:
            if self.ActionType()==ActType.eClampOpen:
                published=True
            if published==False:
                self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            time.sleep(0.5)
        return True

    def CancelNavTask(self):
        cmd = message.NavCmd()
        cmd.action=3

        while self.IsNavigating() == True:
            self.publish(self.navCmdTopic_, jtf.MessageToJson(cmd))
            time.sleep(1)
        print(" Cancel task completed!!!")
        return True