sim.py

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

# Desc: Handle simulator interface
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: sim.py,v 1.13 2004/11/16 23:14:50 inspectorg Exp $

import os
import string
import sys
from wxPython.wx import *

from gazebo import *
from wxgazebo.ids import *
from wxgazebo.panel import gzPanel



class gzSim(gzPanel):
    """Mediate global simulator data."""

    def __init__(self, parent, client, sim, ifaces):

        gzPanel.__init__(self, parent, 'Gazebo : simulation control', (300, 400))

        self.client = client
        self.iface = sim

        # Create menu
        menuBar = wxMenuBar()
        self.controlMenu = wxMenu()
        self.controlMenu.Append(ID_PAUSE, 'Pause', 'Pause the simulation', True)
        self.controlMenu.Append(ID_RESET, 'Reset', 'Reset to initial poses', False)
        EVT_MENU(self, ID_RESET, self.OnReset)
        self.controlMenu.Append(ID_SAVE, 'Save', 'Save the current state', False)
        EVT_MENU(self, ID_SAVE, self.OnSave)
        menuBar.Append(self.controlMenu, 'Controls')
        self.SetMenuBar(menuBar)

        # Initialize menu state
        data = self.iface.data
        self.controlMenu.Check(ID_PAUSE, data.pause)

        # Simulator state
        self.stateLabel = wxStaticText(self, -1, '')

        # Checklist containing available models and interfaces
        self.checklist = wxCheckListBox(self, -1)
        EVT_CHECKLISTBOX(self.checklist, -1, self.OnCheck)

        # Load the interface list
        self.LoadList(ifaces)

        # Layout stuff
        self.vsizer = wxBoxSizer(wxVERTICAL)
        self.vsizer.Add(self.checklist, 1, wxEXPAND)
        self.vsizer.Add(self.stateLabel, 0, wxEXPAND)
        self.SetAutoLayout(True)
        self.SetSizer(self.vsizer)
        self.Layout()

        self.Show(True)
        return


    def LoadList(self, ifaces):
        """Load the check list with model/interface data."""

        # Group interfaces by model
        nodes = {}
        for h in ifaces:
            node = nodes.get(h.model_id, None)
            if node == None:
                node = []
                nodes[h.model_id] = node
            node.append(h)

        # Fill list recursively
        self.FillList(nodes, 0, 0)

        # Open any guicam panels
        for i in range(self.checklist.GetCount()):
            h = self.checklist.GetClientData(i)
            if h.iface == 'guicam':
                self.checklist.Check(i)
                h.panel = h.panel_class(self, self.client, h.gzid)
                EVT_CLOSE(h.panel, self.OnClosePanel)                

        return


    def FillList(self, nodes, parent_id, depth):
        """Fill list recursively"""

        for id in nodes.keys():
            k = nodes[id][0]            
            if k.parent_model_id != parent_id:
                continue

            # Add all the interfaces for this model
            for h in nodes[id]:
                label = '%*s %s : %s : %s' % \
                        (depth * 2, '', h.model_type, h.iface, h.gzid)
                self.checklist.Append(label, h)

            # Add any children of this model
            self.FillList(nodes, k.model_id, depth + 1)

        return


    def OnCheck(self, event):
        """Process list check events."""

        # Some panels will hang the GUI if started while paused
        if self.controlMenu.IsChecked(ID_PAUSE):
            return

        # Event does seem to return client data correctly,
        # so we will do it the hard way
        for i in range(self.checklist.GetCount()):
            h = self.checklist.GetClientData(i)
            on = self.checklist.IsChecked(i)

            # If there is no panel for this, but should be, open one
            if on and not h.panel:
                if h.panel_class:
                    h.panel = h.panel_class(self, self.client, h.gzid)
                    EVT_CLOSE(h.panel, self.OnClosePanel)

            # If there is a panel for this, but shouldnt be, close it
            elif not on and h.panel:
                h.panel.Close()

        return


    def OnClosePanel(self, event):
        """Process close events from child panels."""
        
        for i in range(self.checklist.GetCount()):
            h = self.checklist.GetClientData(i)
            if h.panel == event.GetEventObject():                
                self.checklist.Check(i, False)
                h.panel.Destroy()
                h.panel = None

        return


    def OnUpdate(self):
        """Update panel."""
        
        # Update child panels
        for i in range(self.checklist.GetCount()):
            h = self.checklist.GetClientData(i)
            on = self.checklist.IsChecked(i)
            if not h.panel:
                continue
            h.panel.OnUpdate()

        self.iface.lock(1)
        data = self.iface.data
        data.pause = self.controlMenu.IsChecked(ID_PAUSE)

        if data.pause:
            state = 'paused'
        else:
            state = 'running'

        text = 'time %.3f %.3f %.3f [%.3f] %s' % \
               (data.real_time, data.sim_time, data.pause_time,
                (data.sim_time + data.pause_time) / (data.real_time + 1e-6),
                state)
        self.stateLabel.SetLabel(text)
        
        self.iface.unlock()
        return


    def OnReset(self, event):
        """Process menu events"""

        self.iface.lock(1)
        self.iface.data.reset = 1
        self.iface.unlock()
        return


    def OnSave(self, event):
        """Process menu events"""

        self.iface.lock(1)
        self.iface.data.save = 1
        self.iface.unlock()
        return