guicam.py

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

# Desc: Camera interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: guicam.py,v 1.20 2005/06/26 18:25:05 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


class gzGuicam(gzPanel):
    """Mediate gui camera data."""

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

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

        # Open interface
        self.iface = gz_guicam()
        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.sim_time + self.iface.data.pause_time
        self.iface.unlock()

        # Create menu
        menuBar = wxMenuBar()
        self.controlMenu = wxMenu()

        self.controlMenu.Append(ID_ROLL_LOCK, 'Roll lock', '', True)
        self.controlMenu.Append(ID_SAVE_FRAMES, 'Save frames', '', True)

        self.controlMenu.Append(ID_DISPLAY_AXES, 'Display axes', '', True)
        self.controlMenu.Append(ID_DISPLAY_COM, 'Display com', '', True)                
        self.controlMenu.Append(ID_DISPLAY_SKINS, 'Display skins', '', True)
        self.controlMenu.Append(ID_DISPLAY_RAYS, 'Display rays', '', True)
        self.controlMenu.Append(ID_DISPLAY_FRUSTRUMS, 'Display frustrums', '', True)
        self.controlMenu.Append(ID_DISPLAY_MATERIALS, 'Display materials', '', True)
        self.controlMenu.Append(ID_DISPLAY_TEXTURES, 'Display textures', '', True)
        
        self.controlMenu.Append(ID_POLYGON_MODE, 'Filled polygons', '', True)
        self.controlMenu.Append(ID_SHADE_MODEL, 'Smooth shading', '', True)
        
        menuBar.Append(self.controlMenu, 'Controls')
        self.SetMenuBar(menuBar)

        # Initialize menu state
        data = self.iface.data
        self.controlMenu.Check(ID_ROLL_LOCK, data.roll_lock)
        self.controlMenu.Check(ID_SAVE_FRAMES, data.save_frames)
        self.controlMenu.Check(ID_DISPLAY_AXES, data.display_axes)
        self.controlMenu.Check(ID_DISPLAY_COM, data.display_com)
        self.controlMenu.Check(ID_DISPLAY_SKINS, data.display_skins)
        self.controlMenu.Check(ID_DISPLAY_RAYS, data.display_rays)
        self.controlMenu.Check(ID_DISPLAY_FRUSTRUMS, data.display_frustrums)
        self.controlMenu.Check(ID_DISPLAY_MATERIALS, data.display_materials)
        self.controlMenu.Check(ID_DISPLAY_TEXTURES, data.display_textures)
        self.controlMenu.Check(ID_POLYGON_MODE, data.polygon_fill)
        self.controlMenu.Check(ID_SHADE_MODEL, data.shade_smooth)

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

        # Process mouse events
        EVT_LEFT_DOWN(self.imagePanel, self.OnMouseDown)
        EVT_LEFT_UP(self.imagePanel, self.OnMouseUp)
        EVT_RIGHT_DOWN(self.imagePanel, self.OnMouseDown)
        EVT_RIGHT_UP(self.imagePanel, self.OnMouseUp)
        EVT_MIDDLE_DOWN(self.imagePanel, self.OnMouseDown)
        EVT_MIDDLE_UP(self.imagePanel, self.OnMouseUp)
        EVT_MOTION(self.imagePanel, self.OnMouseMotion)

        # Widget 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)

        # Current mouse controls
        self.mouse_state = 0
        self.mouse_mode = 0
        self.mouse_start = None
        self.mouse_stop = None

        # 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.sim_time + data.pause_time != self.updateTime:
            self.updateTime = data.sim_time + data.pause_time

            text = 'time %14.3f %14.3f \nw %d h %d\n' % \
                   (data.sim_time, data.pause_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)

        # Spot tracking using the mouse
        if self.mouse_start != None:
            data.cmd_spot_state = self.mouse_state
            data.cmd_spot_mode = self.mouse_mode
            data.cmd_spot_a = self.mouse_start
            data.cmd_spot_b = self.mouse_stop

        data.roll_lock = self.controlMenu.IsChecked(ID_ROLL_LOCK)
        data.save_frames = self.controlMenu.IsChecked(ID_SAVE_FRAMES)
        data.display_axes = self.controlMenu.IsChecked(ID_DISPLAY_AXES)
        data.display_com = self.controlMenu.IsChecked(ID_DISPLAY_COM)
        data.display_skins = self.controlMenu.IsChecked(ID_DISPLAY_SKINS)
        data.display_rays = self.controlMenu.IsChecked(ID_DISPLAY_RAYS)
        data.display_frustrums = self.controlMenu.IsChecked(ID_DISPLAY_FRUSTRUMS)
        data.display_materials = self.controlMenu.IsChecked(ID_DISPLAY_MATERIALS)
        data.display_textures = self.controlMenu.IsChecked(ID_DISPLAY_TEXTURES)
        data.polygon_fill = self.controlMenu.IsChecked(ID_POLYGON_MODE)
        data.shade_smooth = self.controlMenu.IsChecked(ID_SHADE_MODEL)

        self.iface.unlock()

        return
    
        
    def OnMouseDown(self, event):
        """Process mouse events."""

        # Set mouse state and mode
        self.mouse_state = 1

        if event.RightIsDown():
            self.mouse_mode = 0
        elif event.MiddleIsDown():
            self.mouse_mode = 1
        else:
            self.mouse_mode = 2

        # Record initial location
        self.mouse_start = event.GetPosition()
        self.mouse_stop = self.mouse_start
        return


    def OnMouseUp(self, event):
        """Process mouse events."""

        self.mouse_state = 0
        return


    def OnMouseMotion(self, event):
        """Process mouse events."""

        if not self.mouse_state:
            return
        self.mouse_stop = event.GetPosition()
        return