stereo.py

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

# Desc: Camera interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: stereo.py,v 1.8 2005/09/30 20:50:08 natepak Exp $

import sys
import time
from wxPython.wx import *

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



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

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

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

        self.gzid = gzid

        # Open interface
        self.iface = gz_stereo()
        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

        # Frame capture
        self.frameCount = 0

        # Create menu
        menuBar = wxMenuBar()
        self.controlMenu = wxMenu()
        self.controlMenu.AppendCheckItem(ID_DISPLAY_LEFT_DISPARITY, 'Left disparity')
        self.controlMenu.AppendCheckItem(ID_DISPLAY_RIGHT_DISPARITY, 'Right disparity')
        menuBar.Append(self.controlMenu, 'Controls')
        self.SetMenuBar(menuBar)

        self.controlMenu.Check(ID_DISPLAY_LEFT_DISPARITY, True)
        self.controlMenu.Check(ID_DISPLAY_RIGHT_DISPARITY, False)

        # Create image panels
        self.imagePanels = [None, None]
        self.imagePanels[0] = gzImagePanel(self, (0, 0), imageSize)
        self.imagePanels[1] = gzImagePanel(self, (0, 0), imageSize)

        # Lay out images in a seperate panel
        isizer = wxBoxSizer(wxHORIZONTAL) 
        isizer.Add(self.imagePanels[0], 0, wxALIGN_CENTER)
        isizer.Add(self.imagePanels[1], 0, wxALIGN_CENTER)

        # Widget for displaying textual info
        self.textLabel = wxStaticText(self, -1, '%40s' % '')

        # Lay out controls in their panel
        sizer = wxBoxSizer(wxVERTICAL)
        sizer.Add(isizer, 0, wxALIGN_CENTER)
        sizer.Add(self.textLabel, 0, 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.updateTime:
            self.updateTime = data.time

            text = 'time %14.3f w %d h %d\n' % \
                   (data.time, data.width, data.height)
            self.textLabel.SetLabel(text)
            
            # Left panel
            if self.controlMenu.IsChecked(ID_DISPLAY_LEFT_DISPARITY):
              image = utils.disparity_to_rgb(data.left_disparity)
              self.imagePanels[0].SetData(image[0:data.left_disparity_size*3])
            else:
              self.imagePanels[0].SetData(data.left_image[0:data.width * data.height * 3])

            # Right panel
            if self.controlMenu.IsChecked(ID_DISPLAY_RIGHT_DISPARITY):
                image = utils.disparity_to_rgb(data.right_disparity)
                self.imagePanels[1].SetData(image[0:right_disparity_size*3])
            else:
              self.imagePanels[1].SetData(data.right_image[0:data.width * data.height * 3])

            # Refresh the window
            self.imagePanels[0].Refresh(False)
            self.imagePanels[1].Refresh(False)

        self.iface.unlock()
        return


    def OnCapture(self, event):
        """Capture button."""

        filename = '%s.%s.%s.pnm' % ('stereo-left-image', self.gzid, self.frameCount)
        self.imagePanels[0].SaveFile(filename, wxBITMAP_TYPE_PNM)

        filename = '%s.%s.%s.pnm' % ('stereo-right-image', self.gzid, self.frameCount)
        self.imagePanels[1].SaveFile(filename, wxBITMAP_TYPE_PNM)

        filename = '%s.%s.%s.pnm' % ('stereo-left-depth', self.gzid, self.frameCount)
        self.imagePanels[2].SaveFile(filename, wxBITMAP_TYPE_PNM)

        self.SetStatusText('saved [%s]' % filename)

        self.frameCount += 1
        return