# Desc: PTZ interface handler# Author: Andrew Howard# Date: 19 Sep 2004# CVS: $Id: ptz.py,v 1.4 2004/11/17 19:01:14 inspectorg Exp $import sysimport mathimport timefrom wxPython.wx import *from wxgazebo.panel import gzPanelfrom gazebo import *# Convert radians to degreesdef rtod(a):    return a / math.pi * 180# IDsID_RESET = 0x100class gzPtz(gzPanel):    """Mediate PTZ interface."""    def __init__(self, parent, client, gzid):        gzPanel.__init__(self, parent, 'ptz [%s]' % gzid, (0, 0))        # Open interface        self.iface = gz_ptz()        if self.iface.open(client, gzid) != 0:            raise gz_error_str()        self.updateTime = 0        # PTZ feedback        self.ptzText = wxStaticText(self, -1, '%020.0f' % 0)        # Pan slider        self.panF = lambda x : -x * math.pi / 180        self.panFi = lambda x : int(-x * 180 / math.pi)        self.panSlider = wxSlider(self, -1, self.panFi(self.iface.data.cmd_pan),                                  -180, 180, wxDefaultPosition, (100, -1))        # Tilt slider        self.tiltF = lambda x : -x * math.pi / 180        self.tiltFi = lambda x : int(-x * 180 / math.pi)        self.tiltSlider = wxSlider(self, -1, self.tiltFi(self.iface.data.cmd_tilt),                                   -180, 180, wxDefaultPosition, (100, -1))        # Zoom slider        self.zoomF = lambda x : math.pi / (1 + x * 9 / 1000.0)        self.zoomFi = lambda x : int((math.pi / x - 1) / (9 / 1000.0))        self.zoomSlider = wxSlider(self, -1, self.zoomFi(self.iface.data.cmd_zoom),                                   0, 1000, wxDefaultPosition, (100, -1))                # Reset button        self.resetButton = wxButton(self, ID_RESET, 'Reset')        EVT_BUTTON(self, ID_RESET, self.OnReset)        # Layout stuff in window        sizer = wxFlexGridSizer(4, 2, 2, 4)        sizer.Add(wxStaticText(self, -1, 'ptz'))        sizer.Add(self.ptzText)        sizer.Add(wxStaticText(self, -1, 'pan'))        sizer.Add(self.panSlider, 0, wxEXPAND)        sizer.Add(wxStaticText(self, -1, 'tilt'))        sizer.Add(self.tiltSlider, 0, wxEXPAND)        sizer.Add(wxStaticText(self, -1, 'zoom'))        sizer.Add(self.zoomSlider, 0, wxEXPAND)        nsizer = wxBoxSizer(wxVERTICAL)        nsizer.Add(sizer, 1, wxALIGN_CENTER)        nsizer.Add(self.resetButton, 0, wxEXPAND)        # Layout stuff in window        sizer = wxBoxSizer(wxVERTICAL)        sizer.Add(self.panSlider, 1, wxALIGN_CENTER)        sizer.Add(self.tiltSlider, 1, wxALIGN_CENTER)        sizer.Add(self.zoomSlider, 1, wxALIGN_CENTER)        # All done; set frame size        self.SetSizer(nsizer)                self.SetAutoLayout(True)        self.Fit()        nsizer.SetSizeHints(self)        # We're ready, so show ourselves        self.Show(True)        return    def __del__(self):        # Close the interface        self.iface.close()        return    def OnUpdate(self):        """Process periodic updates."""                self.iface.lock(1)        data = self.iface.data        # See if we have new data        if data.time != self.updateTime:            self.updateTime = data.time            ptz = '%.1f %.1f %.1f' % \                  (rtod(data.pan), rtod(data.tilt), rtod(data.zoom))            self.ptzText.SetLabel('%s' % (ptz))            # Update PTZ commands            data.cmd_pan = self.panF(self.panSlider.GetValue())            data.cmd_tilt = self.tiltF(self.tiltSlider.GetValue())            data.cmd_zoom = self.zoomF(self.zoomSlider.GetValue())                    self.iface.unlock()        return    def OnReset(self, event):        self.panSlider.SetValue(self.panFi(0.0))        self.tiltSlider.SetValue(self.tiltFi(0.0))        self.zoomSlider.SetValue(self.zoomFi(math.pi))        return