ptz.py

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

# 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 sys
import math
import time
from wxPython.wx import *

from wxgazebo.panel import gzPanel
from gazebo import *


# Convert radians to degrees
def rtod(a):
    return a / math.pi * 180


# IDs
ID_RESET = 0x100


class 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