axisDevice.py

Go to the documentation of this file.

from __future__ import division, absolute_import
import re
import copy

import numpy
from coordConv import PVT
from twistedActor import TCPDevice, log, CommandQueue, LinkCommands, expandUserCmd
from RO.AddCallback import safeCall2
from RO.StringUtil import strFromException, quoteStr

from tcc.base import tai
from tcc.util import dateFromSecInDay, RunningStats

__all__ = ["AxisDevice"]

InitReplyDelay = 0.5 # time to wait after seeing OK to be sure it is for INIT
    # and not some other command that INIT superseded

TrackAdvTimeBufferSize = 20 # number of entries for <axis>TrackAdvTime rolling average
TrackAdvTimeReportEvery = 10 # output timing <axis>TrackAdvTime every this many MOVE P V T commands

class AxisStatus(object):
    """!Axis controller status
    """
    def __init__(self):
        """!Construct an AxisStatus
        """
        self.pvt = PVT()
        self.indexPos = numpy.nan
        self.statusWord = 0
        self.dTime = numpy.nan
        self.taiDate = 0 # date as reported by axis controller (expanded to full date)

    def copy(self):
        """!Return a copy of this object
        """
        return copy.copy(self)

    def setFromReply(self, reply):
        """!Set values from status reply

        @param[in] reply  reply from STATUS command, in format:
            pos vel secInDay statusWord indexPosition
            all values are float except statusWord is an int
        """
        log.info("%s.setFromReply(reply=%s)"%(self, reply))
        dataList = reply.split()
        if len(dataList) != 5:
            raise RuntimeError("Expected 5 values for status but got %r" % (reply,))
        try:
            pos = float(dataList[0])
            vel = float(dataList[1])
            secInDay = float(dataList[2])
            statusWord = int(dataList[3])
            indexPosition = float(dataList[4])
        except ValueError:
            raise RuntimeError("Could not parse %s as status" % (" ".join(dataList),))
        taiDate = dateFromSecInDay(secInDay)
        self.pvt = PVT(pos, vel, taiDate)
        self.statusWord = statusWord
        self.indexPosition = indexPosition
        self.dTime = tai() - taiDate
        self.taiDate = taiDate

    def wordOrDTimeChanged(self, rhs):
        """!Return True if the status word has changed or dTime has changed significantly

        @param[in] rhs  another AxisStatus, against which to compare

        Use to determine if new status should be reported. Thus "significantly" for dTime
        means that the formatted value will look different.
        """
        return (self.statusWord != rhs.statusWord) or (abs(self.dTime-rhs.dTime)>0.1)

    def formatStatus(self, name, axeLim):
        """!Format status message

        @param[in] name  prefix for keyword; full keyword is <i>Name</i>Stat,
            where name is cast to titlecase
        @param[in] axeLim  axeLim block;
            reads badStatusMask and warnStatusMask

        @return two items:
        - message severity ("i" if status OK, "w" if not)
        - status in keyword-variable format
        """
        titleName = name.title()
        statusElts = []
        if self.statusWord & axeLim.badStatusMask != 0:
            msgCode = "w"
            statusElts.append("Bad%sStatus" % (titleName,))
        elif self.statusWord & axeLim.warnStatusMask != 0:
            msgCode = "w"
            statusElts.append("Warn%sStatus" % (titleName,))
        else:
            msgCode = "i"
        statusElts.append("%sStat=%0.6f, %0.6f, %0.5f, 0x%08x; %sDTime=%0.2f" % \
                (titleName, self.pvt.pos, self.pvt.vel, self.pvt.t, self.statusWord, titleName, self.dTime))
        return msgCode, "; ".join(statusElts)

class AxisDevice(TCPDevice):
    """!An Axis Device

    You may modify where output is sent by setting writeToUsers to a function that accepts these arguments:
    msgCode, msgStr, cmd=None, userID=None, cmdID=None (see twistedActor.baseActor.writeToUsers for details)

    Several callback functions may be defined by setting the following attributes
    or cleared by setting them None. If specified, each receives one positional argument: this device.
    initCallFunc: a callback function called after INIT, MOVE (with no arguments) or STOP is started
    statusCallFunc: a callback function called after STATUS is successfully parsed
    driftCallFunc: a callback function called after the reply to DRIFT is successfully parsed
    """
    DefaultTimeLim = 5 # default time limit, in seconds
    DefaultInitTimeLim = 10 # default time limit for init, connection and disconnect
    def __init__(self, name, host, port, callFunc=None):
        """!Construct an AxisDevice

        Inputs:
        @param[in] name  name of device
        @param[in] host  host address of Galil controller
        @param[in] port  port of Galil controller
        @param[in] callFunc  function to call when state of device changes;
                note that it is NOT called when the connection state changes;
                register a callback with "conn" for that task.
        """
        TCPDevice.__init__(self,
            name = name,
            host = host,
            port = port,
            callFunc = callFunc,
            cmdInfo = (),
        )
        self._initialSetup()


    def _initialSetup(self):
        # break out initializatin here
        # so subclasses can use
        self.initCallFunc = None # called after MOVE or INIT are started
        self.statusCallFunc = None # called after STATUS is successfully parsed
        self.driftCallFunc = None # called after DRIFT reply is successfully parsed
        self.replyBuffer = []
        self.status = AxisStatus()

        self._lastMoveTime = 0 # time of last MOVE P V T (TAI, MJD sec), or 0 after a DRIFT, MOVE (with no arguments), STOP or INIT
        self._moveRE = re.compile(r"MOVE +([0-9.+-]+) +([0-9.+-]+) +([0-9.+-]+)\b", re.I) # for parsing move command echo
        self._clearRE = re.compile(r"(DRIFT|MOVE|STOP|INIT)\b", re.I)
        self.runningStats = RunningStats(
            bufferSize = TrackAdvTimeBufferSize,
            callEvery = TrackAdvTimeReportEvery,
            callFunc = self._printTrackAdvTime,
        )

        self.StatusInitVerb = "statusinit"
        self.cmdQueue = CommandQueue(
            priorityDict = {
                "init" : CommandQueue.Immediate,
                self.StatusInitVerb: 4,
                "set.time": 4,
                "stop": 3,
                "drift" : 2,
                "move" : 2,
                "+move" : 2,
                "ms.on" : 2,
                "ms.off" : 2,
                "status" : 1,
            }
        )
        allCmdsExceptStop = ["drift", "move", "+move", "ms.on", "ms.off", "status"]

        # a stop command will clear all other queued commands except "stop", "init", "statusinit", and "set.time"
        # "init", "set.time", and "statusinit" are all part of the axis initialization procedure.
        self.cmdQueue.addRule(
            action = CommandQueue.CancelQueued,
            newCmds = ["stop"],
            queuedCmds = allCmdsExceptStop,
        )

        # a drift command will clear all other queued commands except "stop" and "init"
        self.cmdQueue.addRule(
            action = CommandQueue.CancelQueued,
            newCmds = ["drift"],
            queuedCmds = allCmdsExceptStop,
        )

    def connect(self, userCmd=None, timeLim=DefaultInitTimeLim):
        """Connecting to the axes

        Overridden because connecting can be slow, presumably due to the multiplexor
        """
        return TCPDevice.connect(self, userCmd=userCmd, timeLim=timeLim)

    def disconnect(self, userCmd=None, timeLim=DefaultInitTimeLim):
        """Connecting to the axes

        Overridden because disconnecting can be slow, presumably due to the multiplexor
        """
        return TCPDevice.disconnect(self, userCmd=userCmd, timeLim=timeLim)

    def init(self, userCmd=None, timeLim=DefaultInitTimeLim, getStatus=True):
        """!Initialize the device and cancel all pending commands

        @param[in] userCmd  user command that tracks this command, if any
        @param[in] timeLim  maximum time before command expires, in sec; None for no limit
        @param[in] getStatus  if true then get status after init
        @return userCmd (a new one if the provided userCmd is None)
        """
        userCmd = expandUserCmd(userCmd)
        log.info("%s.init(userCmd=%s, timeLim=%s, getStatus=%s)" % (self, userCmd, timeLim, getStatus))
        if getStatus:
            # put a status on the queue after the init. Use StatusInitVerb
            # to run the status at high priority, so it cannot be interrupted by a MOVE
            # of similar command, which would make the returned userCmd fail
            devCmd0 = self.startCmd("INIT", timeLim=timeLim)
            devCmd1 = self.startCmd("STATUS", timeLim=timeLim, devCmdVerb=self.StatusInitVerb)
            LinkCommands(mainCmd=userCmd, subCmdList=[devCmd0, devCmd1])
        else:
            self.startCmd("INIT", userCmd=userCmd, timeLim=timeLim)
        return userCmd

    def checkCmdEcho(self, ind=0):
        """!Check command echo
        """
        if len(self.replyBuffer) == 0:
            raise RuntimeError("Cannot check echo of command=%r: no replies" % \
                (self.cmdQueue.currExeCmd.cmdStr,))

        if self.cmdQueue.currExeCmd.cmdStr != self.replyBuffer[ind]:
            raise RuntimeError("command=%r != echo=%r" % \
                (self.cmdQueue.currExeCmd.cmdStr, self.replyBuffer[ind]))

    def checkNumReplyLines(self, numLines):
        """!Check number of reply lines
        """
        if numLines is not None and len(self.replyBuffer) != numLines:
            raise RuntimeError("For command %r expected %d replies but got %s" % \
                (self.cmdQueue.currExeCmd.cmdStr, numLines, self.replyBuffer))

    def prepReplyStr(self, reply):
        """!Strip unwanted characters from reply string

        @param[in] reply: a reply string (from a device)
        """
        return reply.strip(' ;\r\n\x00\x01\x02\x03\x18\xfa\xfb\xfc\xfd\xfe\xff')

    def handleReply(self, reply):
        """!Handle a line of output from the device. Called whenever the device outputs a new line of data.

        @param[in] reply  the reply, minus any terminating \n
        """
        log.info('%s read %r; curr cmd=%r %s' % (self, reply, self.cmdQueue.currExeCmd.cmd.cmdStr, self.cmdQueue.currExeCmd.cmd.state))
        reply = self.prepReplyStr(reply)
        if (not reply) or self.cmdQueue.currExeCmd.isDone:
            # ignore blank lines and unsolicited input
            return

        self._checkRunStats(reply)
        # is there a terminating OK on the line?
        gotOK = False
        if reply == "OK":
            reply = ""
            gotOK = True
        elif reply.endswith(" OK"):
            gotOK = True
            reply = reply[:-3]
        if reply:
            self.replyBuffer.append(reply)
            log.info('%s replyBuffer=%r; curr cmd=%r %s'%(self, self.replyBuffer, self.cmdQueue.currExeCmd.cmd.cmdStr, self.cmdQueue.currExeCmd.cmd.state))
            if self.cmdQueue.currExeCmd.cmd.showReplies:
                msgStr = "%sReply=%s" % (self.name, quoteStr(reply),)
                self.writeToUsers(msgCode='i', msgStr=msgStr)
        if gotOK:
            # all expected output has been received
            # now parse command echo, and any additional info
            self.parseReplyBuffer()


    def parseReplyBuffer(self):
        """!Parse the contents of the reply buffer
        """
        log.info("%s.parseReplyBuffer replyBuffer=%r cmdVerb=%r" % (self, self.replyBuffer, self.cmdQueue.currExeCmd.cmdVerb))
        descrStr = "%s.parseReplyBuffer" % (self,)
        try:
            # use this inner try/finally block to make sure the reply buffer is emptied
            try:
                if self.cmdQueue.currExeCmd.isDone:
                    log.warn("parseReplyBuffer called with replies=%r for done command=%r" % \
                        (self.replyBuffer, self.cmdQueue.currExeCmd))
                    return

                cmdVerb = self.cmdQueue.currExeCmd.cmdVerb

                if cmdVerb == "init":
                    try:
                        # the reply buffer may have replies from other commands that were executing
                        # when the INIT was sent; only the last reply is relevant
                        self.checkCmdEcho(-1)
                    except RuntimeError:
                        # last reply in buffer should be init!, if not
                        # do nothing, wait for another reply
                        # this echo could have been associated with a
                        # previously running command
                        return
                    else:
                        if self.initCallFunc:
                            safeCall2(descrStr, self.initCallFunc, self)
                else:
                    self.checkCmdEcho()

                    if "status" in cmdVerb: #statusinit is it's own verb
                        self.checkNumReplyLines(2)
                        self.status.setFromReply(self.replyBuffer[1])
                        if self.statusCallFunc:
                            safeCall2(descrStr, self.statusCallFunc, self)

                    elif cmdVerb == "drift":
                        self.checkNumReplyLines(2)
                        driftReply = self.replyBuffer[1]
                        try:
                            pos, vel, secInDay = [float(strVal) for strVal in driftReply.split()]
                        except Exception as e:
                            raise RuntimeError("Could not parse %r as pos vel secInDay" % (driftReply,))
                        taiDate = dateFromSecInDay(secInDay)
                        self.status.pvt = PVT(pos, vel, taiDate)
                        if self.driftCallFunc:
                            safeCall2(descrStr, self.driftCallFunc, self)

                    elif cmdVerb in ("move", "+move", "ms.on", "ms.off", "stop"):
                        if cmdVerb == "stop" or self.cmdQueue.currExeCmd.cmdStr.lower() == "move":
                            if self.initCallFunc:
                                safeCall2(descrStr, self.initCallFunc, self)
                        self.checkNumReplyLines(1)
            finally:
                self.replyBuffer = []
            self.cmdQueue.currExeCmd.setState(self.cmdQueue.currExeCmd.Done)
        except Exception as e:
            log.error("%s %s"%(self, strFromException(e)))
            if not self.cmdQueue.currExeCmd.isDone:
                self.cmdQueue.currExeCmd.setState(self.cmdQueue.currExeCmd.Failed, strFromException(e))

    def startCmd(self, cmdStr, callFunc=None, userCmd=None, timeLim=DefaultTimeLim, showReplies=False,
        devCmdVerb=None):
        """!Queue a new command

        @param[in] cmdStr  command string
        @param[in] callFunc  callback function: function to call when command succeeds or fails, or None;
            if specified it receives one argument: a device command
        @param[in] userCmd  user command that should track this command, if any
        @param[in] timeLim  time limit, in seconds
        @param[in] showReplies  show all replies as plain text?
        @param[in] devCmdVerb  if specified, use to set device command verb,
            which in turn sets the priority of the command

        @return devCmd: the device command that was started (and may already have failed)
        """
        log.info("%s.startCmd(cmdStr=%s, userCmd=%s, timeLim=%s)" % (self, cmdStr, userCmd, timeLim))
        devCmd = self.cmdClass(
            cmdStr = cmdStr,
            callFunc = callFunc,
            userCmd = userCmd,
            timeLim = timeLim,
            dev = self,
            showReplies = showReplies,
        )
        if devCmdVerb:
            devCmd.cmdVerb = devCmdVerb
        else:
            devCmd.cmdVerb = cmdStr.partition(" ")[0].lower()
        self.cmdQueue.addCmd(devCmd, self.sendCmd)
        cmdQueueFmt = "[%s]"%(", ".join(["%s(%s)"%(cmd.cmdStr, cmd.cmd.state) for cmd in self.cmdQueue]))
        log.info("%s.startCmd(cmdQueue=%s)"%(self, cmdQueueFmt))
        return devCmd

    def sendCmd(self, devCmd):
        """!Execute the command

        @param[in] devCmd  a device command
        """
        # clear the buffer
        self.replyBuffer = []
        if not self.conn.isConnected:
            log.error("%s cannot write %r: not connected" % (self, devCmd.cmdStr))
            devCmd.setState(devCmd.Failed, "not connected")
            return

        # add failure callback after checking connection, because INIT is useless if not connected
        if "init" != devCmd.cmdVerb.lower():
            # only non-init commands get an automatic init-on-failure
            devCmd.addCallback(self._devCmdFailInit)
        self.writeToDev(devCmd.cmdStr)

    def writeToDev(self, line):
        log.info("%s writing %r" % (self, line))
        self.conn.writeLine(line)

    def _checkRunStats(self, cmdStr):
        """!Determine if cmdStr is a MOVE PVT command.  If so parse it, determine advance
        time, and update self.runningStats

        Note that advance time = time of previous MOVE P V T - tai(),
        and that it is reset based on DRIFT, MOVE (with no arguments), STOP or INIT
        """
        moveMatch = self._moveRE.match(cmdStr)
        if moveMatch:
            moveSecInDay = float(moveMatch.group(3))
            moveTime = dateFromSecInDay(moveSecInDay)
            if self._lastMoveTime > 0:
                # measure how early P V Ts of a path are, relative to last P V T sent
                advTime = self._lastMoveTime - tai()
            else:
                # measure how early the first P V T of a new path is, relative to "now"
                advTime = moveTime - tai()
            self.runningStats.addValue(advTime)
            self._lastMoveTime = moveTime
        else:
            clearMatch = self._clearRE.match(cmdStr)
            if clearMatch:
                self._lastMoveTime = 0

    def _connCallback(self, conn=None):
        """!Call when the connection state changes

        Kill all executing and queued commands, preferably without running the command queue
        """
        TCPDevice._connCallback(self, conn=conn)
        if self.conn.isDisconnected:
            self.cmdQueue.killAll()

    def _devCmdFailInit(self, devCmd):
        """!Call init if device command fails

        @warning only add this callback when the device command is ready to run
        """
        if devCmd.state==devCmd.Failed:
            # note if devCmd.state==Cancelled, this code doesn't run
            # only init may set device command to cancelled
            # so this is the desired behavior.
            # only auto-init if the devCmd failed, and was not explicitly canceled
            # by an init
            msgStr="%s._devCmdFailInit(devCmd=%s).  DevCmd failed, sending init"%(self,devCmd)
            log.warn(msgStr)
            self.init(getStatus=False)

    def _printTrackAdvTime(self, runStatObj):
        """!Callback for self.runningStats; print info
        """
        stats = runStatObj.getStats()
        msgStr = "%sTrackAdvTime=%i, %.2f, %.2f, %.2f, %.2f" % (
            self.name.title(), stats["num"], stats["min"], stats["max"], stats["median"], stats["stdDev"]
        )
        self.writeToUsers("i", msgStr=msgStr)