camera.py

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

# Desc: Camera interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: camera.py,v 1.7 2005/08/11 19:42:47 natepak Exp $

import sys
import time
from wxPython.wx import *

from gazebo import *
from wxgazebo.panel import gzPanel
from wxgazebo.image import gzImagePanel


class gzCamera(gzPanel):
    """Mediate camera data."""

    def __init__(self, parent, client, gzid):

        gzPanel.__init__(self, parent, 'Camera [%s]' % gzid, (0, 0))

        self.update_time = -1e6

        # Open interface
        self.iface = gz_camera()
        if self.iface.open(client, gzid) != 0:
            raise gz_error_str()

        # Get the image size
        self.iface.lock(1)
        imageSize = (self.iface.data.width, self.iface.data.height)
        self.updateTime = self.iface.data.time
        self.iface.unlock()

        # Create image panel
        self.imagePanel = gzImagePanel(self, (0, 0), imageSize)

        # Wdiget for displaying textual info
        self.textLabel = wxStaticText(self, -1, '')

        # Layout stuff in window
        sizer = wxBoxSizer(wxVERTICAL)
        sizer.Add(self.imagePanel, 0, wxALIGN_CENTER)
        sizer.Add(self.textLabel, 1, wxALIGN_CENTER)

        # All done; set frame size
        self.SetSizer(sizer)        
        self.SetAutoLayout(True)
        self.Fit()
        sizer.SetSizeHints(self)

        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.update_time:
            self.update_time = data.time

            text = 'time %14.3f\nw %d h %d\n' % \
                   (data.time, data.width, data.height)
            self.textLabel.SetLabel(text)

            # Set the image data
            self.imagePanel.SetData(data.image[0:data.width * data.height * 3])

            # Refresh the window
            self.imagePanel.Refresh(False)

        self.iface.unlock()

        return