tccActor.py

Go to the documentation of this file.

from __future__ import division, absolute_import

import os
import re
import sys
import traceback

import numpy

from RO.Comm.TwistedTimer import Timer
from RO.StringUtil import strFromException, quoteStr
from coordConv import PVT
from twistedActor import BaseActor, UserCmd, CommandError, LinkCommands, log
import tcc.base
import tcc.axis
from tcc import __version__
from .tccCmdParser import TCCCmdParser
from tcc.mir import computeOrient
from tcc.msg import formatAxisCmdStateAndErr, formatDisabledProc, formatPVTList, UDPSender, getBinaryForUDP, \
    showAxeLim, showTune
from tcc.cmd import showScaleFactor

__all__ = ["TCCActor"]

numpy.seterr(divide="ignore") # don't warn about divide by 0

DefaultUserPort = 2200
DefaultUDPPort = 2201
EarthInterval = 3600 # time between earth updates (sec)

# set of axis "error" codes that do not merit a warning
AxisNoWarnErrSet = set((tcc.base.AxisErr_HaltRequested, tcc.base.AxisErr_NotAvailable, tcc.base.AxisErr_OK))
class TCCActor(BaseActor):
    """!Actor for controlling a telescope
    """
    def __init__(self,
        axisDict,
        mirrorDict,
        userPort = DefaultUserPort,
        udpPort = DefaultUDPPort,
        name = "TCCActor",
        posRefCatPath = "posRefCatalog.dat",
        connectRot = False,
    ):
        """Construct a TCCActor

        @param[in] axisDict  a dict of axis name: axis device
        @param[in] mirrorDict  a dict of mirror name: mirror device
        @param[in] userPort  port on which to listen for users
        @param[in] udpPort  port on which to broadcast UDP packets.
        @param[in] maxUsers  the maximum allowed number of users; if 0 then there is no limit
        @param[in] name  actor name; used for logging
        @param[in] posRefCatPath  path to position reference catalog, relative to $TCC_DATA_DIR
        @param[in] connectRot  bool. If True, automatically connect to rot1 on startup, else leave
                                      the slot empty. SDSS TCC should set to True. 3.5m set to False
        """
        self.dataDir = tcc.base.getDataDir()
        self.udpPort = udpPort
        # create a null tune block before calling BaseActor.__init__, and do NOT set it from a data file yet,
        # because BaseActor sets self.maxUsers, which sets self.tune.maxUsers
        self.tune = tcc.base.Tune()
        BaseActor.__init__(self, userPort=userPort, maxUsers=1, name=name, version=__version__)

        log.info("%r starting up" % (self,))

        for dev in mirrorDict.itervalues():
            if dev.name not in ("prim", "sec", "tert"):
                raise RuntimeError("invalid mirror name: %r" % (dev.name,))
            dev.writeToUsers = self.writeToUsers
        self.mirrorDict = mirrorDict
        mirDevList = [mirrorDict.get(mirName, None) for mirName in tcc.mir.MirrorDeviceSet.SlotList]
        self.mirDevSet = tcc.mir.MirrorDeviceSet(
            actor = self,
            devList  = mirDevList,
        )

        for dev in axisDict.itervalues():
            if not re.match(r"az|alt|rot\d+", dev.name):
                raise RuntimeError("invalid axis name: %r" % (dev.name,))
            dev.writeToUsers = self.writeToUsers

        # a dict of all axis devices
        # keys are "az", "alt", "rot1", "rot2", etc. (for those rotators that exist)
        # values are AxisDevices
        self.axisDict = axisDict
        axisDevList=[axisDict["az"], axisDict["alt"], None]
        if connectRot:
            axisDevList[-1] = axisDict["rot1"]
        self.axisDevSet = tcc.axis.AxisDeviceSet(
            actor=self,
            devList=axisDevList,
        )

        # self.dev = DeviceCollection(self.mirrorDict.values() + self.axisDict.values())
        self.cmdParser = TCCCmdParser()
        self.axeLim = tcc.base.AxeLim()
        self.axeLim.loadPath(os.path.join(self.dataDir, "axelim.dat"))
        self.earth = tcc.base.Earth()
        self.inst = tcc.base.Inst()
        if connectRot:
            self.inst.rotID = 1
        self.obj = tcc.base.Obj()
        self.obj.site.wavelen = 4500
        self.obj.gsSite.wavelen = 4500
        self.savedObj = None
        self.telMod = tcc.base.TelMod(os.path.join(self.dataDir, "telmod.dat"))
        self.tune.loadPath(os.path.join(self.dataDir, "tune.dat"))
        self.weath = tcc.base.Weath()
        self.slewCmd = UserCmd()
        self.slewCmd.setState(self.slewCmd.Done)

        self.collimateTimer = Timer(0, self.updateCollimation)
        self.collimateSlewEndTimer = Timer()
        self.earthTimer = Timer(0, self.updateEarth)
        self.slewDoneTimer = Timer()
        self.statusTimer = Timer()
        self.trackTimer = Timer()

        fullPosRefCatPath = os.path.join(self.dataDir, posRefCatPath)
        log.info("Loading position reference catalog %r" % (fullPosRefCatPath,))
        self.posRefCatalog = tcc.axis.PosRefCatalog(fullPosRefCatPath)
        log.info("Loaded %s position reference stars" % (len(self.posRefCatalog),))

        def startAxisStatus(userCmd):
            """Queue axis status once userCmd succeeds"""
            if userCmd.didFail:
                log.warn("Axis connection failed; not queueing status")
            elif userCmd.isDone:
                statusDelay = self.tune.statusInterval[2]
                log.info("Axes connected; queueing status for %s seconds from now" % (statusDelay,))
                self.statusTimer.start(statusDelay, self.showStatus)
        axisConnCmd = self.axisDevSet.connect()
        axisConnCmd.addCallback(startAxisStatus, callNow=False)

        self.mirDevSet.connect()
        self.updateEarth()
        tcc.axis.computeObj(
            obj = self.obj,
            doWrap=False,
            doRestart=[False]*tcc.base.NAxes,
            reportLim=True, # ???is this argument good for anything???
            earth = self.earth,
            inst = self.inst,
            telMod = self.telMod,
            axeLim = self.axeLim,
            tai = tcc.base.tai(),
        )

        self.udpSender = UDPSender(self.udpPort)
        self.udpSender.startListening()
        self.brdTelPosTimer = Timer(0, self.brdTelPos)

    @property
    def maxUsers(self):
        # link self.maxUsers to self.tune.maxUsers
        return self.tune.maxUsers

    @maxUsers.setter
    def maxUsers(self, maxUsers):
        # link self.maxUsers to self.tune.maxUsers
        maxUsers = int(maxUsers)
        if maxUsers < 1:
            raise ValueError("maxUsers = %r; must be > 0" % (maxUsers,))
        self.tune.maxUsers = maxUsers

    def _cancelTimers(self):
        """Cancel all timers
        """
        self.udpSender.stopListening()
        self.collimateTimer.cancel()
        self.collimateSlewEndTimer.cancel()
        self.earthTimer.cancel()
        self.slewDoneTimer.cancel()
        self.statusTimer.cancel()
        self.trackTimer.cancel()
        self.brdTelPosTimer.cancel()

    def parseAndDispatchCmd(self, cmd):
        """Dispatch the user command

        @param[in] cmd  user command (a twistedActor.UserCmd)
        """
        if not cmd.cmdBody:
            # echo to show alive
            self.writeToOneUser(":", "", cmd=cmd)
            return

        try:
            cmd.parsedCmd = self.cmdParser.parseLine(cmd.cmdBody)
        except Exception as e:
            cmd.setState(cmd.Failed, "Could not parse %r: %s" % (cmd.cmdBody, strFromException(e)))
            return

        #cmd.parsedCmd.printData()
        if cmd.parsedCmd.callFunc:
            cmd.setState(cmd.Running)
            try:
                cmd.parsedCmd.callFunc(self, cmd)
            except CommandError as e:
                cmd.setState("failed", textMsg=strFromException(e))
                return
            except Exception as e:
                sys.stderr.write("command %r failed\n" % (cmd.cmdStr,))
                sys.stderr.write("function %s raised %s\n" % (cmd.parsedCmd.callFunc, strFromException(e)))
                traceback.print_exc(file=sys.stderr)
                textMsg = strFromException(e)
                hubMsg = "Exception=%s" % (e.__class__.__name__,)
                cmd.setState("failed", textMsg=textMsg, hubMsg=hubMsg)
        else:
            raise RuntimeError("Command %r not yet implemented" % (cmd.parsedCmd.cmdVerb,))

    def showUserInfo(self, cmd):
        """Show user information including your userID.

        This overridden version also outputs YourUserNum, to match the old TCC.
        """
        self.writeToOneUser("i", "YourUserNum=%s" % (cmd.userID,), cmd=cmd)
        BaseActor.showUserInfo(self, cmd)
        showAxeLim(self, cmd)
        showTune(self, cmd)
        showScaleFactor(self, cmd, setDone=False)

    def showVersion(self, cmd, onlyOneUser=False):
        """!Show actor version

        This overridden version uses uppercase for the version string
        """
        msgStr = "Version=%s" % (quoteStr(self.version),)
        if onlyOneUser:
            self.writeToOneUser("i", msgStr, cmd=cmd)
        else:
            self.writeToUsers("i", msgStr, cmd=cmd)

    def queueStatus(self, delaySec=None):
        """Queue a new status request

        @param[in] delaySec  delay (sec); if None then auto-computed
        """
        if delaySec is None:
            if self.obj.isSlewing():
                delaySec = self.tune.statusInterval[1]
            elif self.obj.isMoving() or any(actMt.isfinite() and abs(actMt.vel) > 0.001 for actMt in self.obj.actMount):
                delaySec = self.tune.statusInterval[0]
            else:
                delaySec = self.tune.statusInterval[2]
        log.info("%s.queueStatus; delaySec=%s" % (self, delaySec))
        self.statusTimer.start(delaySec, self.showStatus)

    def showStatus(self, cmd=None):
        """Show status

        @param[in,out] cmd  user command; warning; if not None then set to Done;
            if None then some status is omitted if unchanged
        """
        # print "showStatus"
        if not self.tune.doStatus:
            msgCode, msgStr = formatDisabledProc(self.tune)
            self.writeToUsers(msgCode, msgStr)
            return

        def showRestOfStatus(statusCmd, cmd=cmd):
            """Show the rest of the status after axis status
            """
            obj = self.obj
            currTAI = tcc.base.tai()
            msgList = (
                "TCCPos=%0.6f, %0.6f, %0.6f" % tuple(pvt.getPos(currTAI) for pvt in obj.targetMount),
                # AxePos must use that case to make the enclosure controller happy
                "AxePos=%0.6f, %0.6f, %0.6f" % tuple(pvt.getPos(currTAI) for pvt in obj.actMount),
                "ObjInstAng=%s" % (formatPVTList([obj.objUserInstAng]),),
                "SpiderInstAng=%s" % (formatPVTList([obj.spiderInstAng]),),
            )
            self.writeToUsers(msgCode="i", msgStr="; ".join(msgList), cmd=cmd)

            msgCode, axisCmdStateStr = formatAxisCmdStateAndErr(obj, showAll=True)
            self.writeToUsers(msgCode=msgCode, msgStr=axisCmdStateStr, cmd=cmd)

            self.axisDevSet.showConnState(userCmd=cmd)
            self.mirDevSet.showConnState(userCmd=cmd)
            # verify that the tune block wants status
            if self.tune.doStatus:
                self.queueStatus()

            if cmd:
                if statusCmd.didFail:
                    cmd.setState(cmd.Failed, textMsg=statusCmd.textMsg, hubMsg=statusCmd.hubMsg)
                else:
                    cmd.setState(cmd.Done)

        statusCmd = self.axisDevSet.status()
        statusCmd.addCallback(showRestOfStatus)

    def updateCollimation(self, cmd=None):
        """Update collimation based on info in obj, inst, weath blocks, for all mirrors present

        @param[in] cmd  command (twistedActor.BaseCmd) associated with this request;
            state will be updated upon completion; None if no command is associated
        """
        try:
            # print "updateCollimation"
            if not self.tune.doCollimate:
                msgCode, msgStr = formatDisabledProc(self.tune)
                self.writeToUsers(msgCode, msgStr)
                return

            alt = self.obj.targetMount[1].getPos(tcc.base.tai())
            if not numpy.isfinite(alt):
                # altitude unknown; don't bother
                if cmd:
                    self.writeToUsers("w", 'Text="Cannot set collimation: target altitude unknown"', cmd=cmd)
                    cmd.setState(cmd.Done)
                return

            # compute mirror commands (if this fails for any mirror, give up,
            # since mirrors should be moved together)
            mirNameCmdStrList = [] # list of mirror name: mirror command string
            for mirName in self.mirrorDict.keys():
                orient = computeOrient(mirName, alt, self.inst, self.weath)
                cmdStr = 'move ' + ','.join(['%.4f' % (o,) for o in orient])
                mirNameCmdStrList.append((mirName, cmdStr))

            # execute mirror commands
            subCmdList = []
            for mirName, mirCmdStr in mirNameCmdStrList:
                mirCmd = self.mirrorDict[mirName].startCmd(mirCmdStr, timeLim=60)
                subCmdList.append(mirCmd)

            if cmd:
                if subCmdList:
                    # link the user command to the mirror move commands
                    LinkCommands(mainCmd = cmd, subCmdList = subCmdList)
                else:
                    cmd.setState(cmd.Done)
            else:
                # no userCmd supplied, add callbacks to sub commands
                # to inform users if they fail
                for cmd in subCmdList:
                    cmd.addCallback(self._writeCollimationFailureToUsers, callNow=True)
        finally:
            if self.tune.doCollimate:
                self.collimateTimer.start(self.tune.collimateInterval, self.updateCollimation)
            else:
                self.collimateTimer.cancel()

    def _writeCollimationFailureToUsers(self, cmd):
        if cmd.didFail:
            self.writeToUsers("w", "Text=\"Collimation Failure: %s\""%cmd.textMsg)

    def updateEarth(self):
        """Update earth orientation predictions

        Load data for the current TAI from the latest data file into self.earth and schedule the next update.
        """
        try:
            currTAI = tcc.base.tai()
            with file(os.path.join(self.dataDir, "earthpred.dat")) as f:
                self.earth.loadPredictions(f, forTAI=currTAI)
        finally:
            self.earthTimer.start(sec=EarthInterval, callFunc=self.updateEarth)

    def updateTracking(self, cmd=None):
        """Compute next tracking position and send to axis controllers (slew or track).

        @param[in] cmd  command (twistedActor.BaseCmd) associated with this request;
            state will be updated upon completion; None if no command is associated
        """
        try:
            # print "updateTracking(cmd=%s): axisCmdState=%s" % (cmd, self.obj.axisCmdState)
            self.trackTimer.cancel()
            if not self.obj.isMoving():
                return

            startTAI = tcc.base.tai()
            advTime = self.obj.updateTime - startTAI

            startMsgStr = "updateTracking started at TAI=%0.2f: advTime=%0.2f seconds" % (startTAI, advTime)
            if advTime < 0.1: # perhaps add a value to the Tune block?
                log.warn(startMsgStr)
            else:
                log.info(startMsgStr)

            if advTime < self.tune.trackAdvTime * 1.5:
                # issue a tracking update
                oldTargetMount = [PVT(targetMount) for targetMount in self.obj.targetMount]
                oldErrCode = self.obj.axisErrCode[:]
                oldHaltedList = [errCode != 0 for errCode in oldErrCode]
                oldSigBad = numpy.any(numpy.logical_and(oldHaltedList, self.obj.axisIsSignificant))

                tcc.axis.computeObj(
                    obj = self.obj,
                    doWrap=False,
                    doRestart=[False]*tcc.base.NAxes,
                    reportLim=False, # ???is this argument good for anything???
                    earth = self.earth,
                    inst = self.inst,
                    telMod = self.telMod,
                    axeLim = self.axeLim,
                    tai = self.obj.updateTime + self.tune.trackInterval,
                )

                # check position, velocity and acceleration over the path segment
                for i in range(3):
                    if not self.obj.targetMount[i].isfinite():
                        # axis is not moving; don't bother
                        continue

                    if not oldTargetMount[i].isfinite():
                        # bug: tracking must have a known start and end PVT
                        self.log.error("updateTracking bug: new targetMount computed when old unknown: axis=%s; obj.targetMount[i]=%s; obj.axisErrCode[i]=%s" \
                            % (i, self.obj.targetMount[i], self.obj.axisErrCode[i]))
                        self.obj.targetMount[i].invalidate(self.obj.updateTime)
                        if self.obj.axisErrCode[i] == tcc.base.AxisErr_OK:
                            self.obj.axisErrCode[i] == tcc.base.AxisErr_TCCBug
                        continue

                    if self.obj.axisErrCode[i] == tcc.base.AxisErr_OK:
                        self.obj.axisErrCode[i] = tcc.mov.checkSegment(
                            pA = oldTargetMount[i].pos,
                            vA = oldTargetMount[i].vel,
                            pB = self.obj.targetMount[i].pos,
                            vB = self.obj.targetMount[i].vel,
                            dt = self.obj.targetMount[i].t - oldTargetMount[i].t,
                            doPos = True,
                            doVel = True,
                            doAccel = True,
                            doJerk = False,
                            axisLim = self.axeLim[i],
                        )

                    # if axis has a problem then halt it
                    if self.obj.axisErrCode[i] != tcc.base.AxisErr_OK:
                        self.obj.targetMount[i].invalidate(self.obj.updateTime)

                newErrCode = self.obj.axisErrCode[:]
                newHaltedList = [errCode != tcc.base.AxisErr_OK for errCode in newErrCode]
                newSigBad = numpy.any(numpy.logical_and(newHaltedList, self.obj.axisIsSignificant))

                # send MOVE PVT command (or MOVE if done halting) to axis devices
                self.axisDevSet.movePVT(self.obj.targetMount, userCmd=cmd)

                # report any state change
                msgCode, msgStr = formatAxisCmdStateAndErr(self.obj)
                if msgStr:
                    self.writeToUsers(msgCode=msgCode, msgStr=msgStr, cmd=cmd)

                newSigBad = numpy.any(numpy.logical_and(newHaltedList, self.obj.axisIsSignificant))
                if newSigBad and not oldSigBad:
                    raise RuntimeError("one or more significant axes halted")
            else:
                log.info("updateTracking called early enough that no tracking updated needed yet")

            if self.obj.isMoving():
                endTAI = tcc.base.tai()
                nextWakeTAI = self.obj.updateTime - self.tune.trackAdvTime
                sleepSec = nextWakeTAI - endTAI
                self.trackTimer.start(sec=sleepSec, callFunc=self.updateTracking)

                execSec = endTAI - startTAI
                endMsgStr = "updateTracking ends at TAI=%0.2f; execSec=%0.2f; sleepSec=%0.2f; nextWakeTAI=%0.2f" % \
                    (endTAI, execSec, sleepSec, nextWakeTAI)
                if sleepSec < 0.1:
                    log.warn(endMsgStr)
                else:
                    log.info(endMsgStr)
            else:
                endMsgStr = "updateTracking ends at TAI=%0.2f; execSec=%0.2f; no wakeup scheduled" % \
                    (endTAI, execSec)
                log.info(endMsgStr)
        except Exception as e:
            traceback.print_exc(file=sys.stderr)

            # halt axes abruptly
            if not self.slewCmd.isDone:
                self.slewCmd.setState(self.slewCmd.Failed, "Slew failed: " + strFromException(e))

            self.axisDevSet.stop()

    def brdTelPos(self):
        """Broadcast telescope position in a UDP packet
        """
        try:
            # print "brdTelPos"
            if not self.tune.doBrdTelPos:
                msgCode, msgStr = formatDisabledProc(self.tune)
                self.writeToUsers(msgCode, msgStr)
                return

            binaryData = getBinaryForUDP(inst=self.inst, obj=self.obj)
            self.udpSender.sendPacket(binaryData)
        finally:
            self.brdTelPosTimer.start(1., self.brdTelPos)