main.py

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

# Desc: Program entry point
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: main.py,v 1.17 2004/11/17 19:01:12 inspectorg Exp $

import sys
import os
import signal
import time

try:
    from wxPython.wx import *
except ImportError:
    print 'wxPython (http://www.wxpython.org) not found; please install it'
    print 'or else fix your PYTHONPATH'
    sys.exit(-1)
    
try:    
    from gazebo import *
except ImportError:
    print 'Python bindings for libgazebo not found; please re-install Gazebo'
    print 'with bindings enabled, or else fix you PYTHONPATH'
    sys.exit(-1)
    
from panels import *
from wxgazebo.panels import *


# Map from descriptive interface types to panel classes
panelmap = \
{
    'camera' : gzCamera,
    'gps' : gzGps,
    'guicam' : gzGuicam,
    'laser' : gzLaser,
    'position' : gzPosition,
    'power' : gzPower,
    'ptz' : gzPtz,
    'sim' : None,
    'sonar' : gzSonar,
    'stereo' : gzStereo
}


class Server:
    """Manage interactions with the server."""

    def __init__(self):

        self.pid = None
        return


    def start(self, argv):
        """Start the server."""

        print 'starting server'
        self.pid = os.fork()
        
        # If we are in the child process, start the server        
        if self.pid == 0:
            os.execvp('gazebo', argv)  # TESTING
            pass

        return 0


    def stop(self):
        """Stop the server."""

        try:
            print 'stopping server'
            os.kill(self.pid, signal.SIGINT)
            os.waitpid(self.pid, 0)
        except OSError, errno:
            return False
        
        return


    def check(self):
        """Check to see if the server is still running."""

        try:
            (pid, status) = os.waitpid(self.pid, os.WNOHANG)
        except OSError, errno:
            return False

        return True



class InterfaceData:
    """Data pertaining to a particular interface"""
    pass


def LoadInterfaces(client):
    """Load a list of available interfaces."""

    ifaces = []

    # Get the list of interfaces from the directory
    filenames = os.listdir(client.filename)
    for file in filenames:
        (type, gzid) = string.split(file, '.')
        if type == 'sim':
            continue

        # Open the interface so we can get its properties
        iface = eval('gz_%s()' % type)
        if iface.open(client, gzid) != 0:
            raise gz_error_str()

        h = InterfaceData()
        h.model_type = iface.data.head.model_type
        h.model_id = iface.data.head.model_id
        h.parent_model_id = iface.data.head.parent_model_id
        h.iface = type
        h.gzid = gzid
        h.panel_class = panelmap.get(type, None)
        h.panel = None

        # HACK; prevents broken HUD interface from causing
        # problems
        if h.model_id != 0:
            ifaces.append(h)

        # Close interface
        iface.close()

    return ifaces


class App(wxPySimpleApp):
    """Application class."""

    def __init__(self, server):

        wxApp.__init__(self)

        self.server = server
        self.wins = []

        self.timer = wxTimer(self)
        EVT_TIMER(self, -1, self.OnTimer)
        return


    def MainLoop(self):
        """Start the main loop."""

        self.timer.Start(20)
        wxApp.MainLoop(self)
        
        return
    

    def OnTimer(self, event):
        """Process timer events"""

        # Check that the server is still there
        if not self.server.check():
            self.ExitMainLoop()

        # Update all of the open windows
        for win in self.wins:
            try:
                win.OnUpdate()
            except wxPyDeadObjectError:
                self.wins.remove(win)

        return




def main(argv):
    """Program entry point."""

    server_id = 0
    client_id = GZ_CLIENT_ID_WXGAZEBO

    # Quietly suck out some flags (we cant use getop; it will error on
    # the flags used soley by the server)
    for i in range(len(argv) - 1):
        if argv[i] == '-s':
            server_id = int(argv[i + 1])

    gz_error_init(False, 0)
    
    server = Server()

    try:

        # Start the server
        if server.start(argv) != 0:
            return

        # Create a client
        client = gz_client()
        if not client:
            raise gz_error_str()

        # Wait for the server to come up
        print 'waiting for server'
        while True:
            if not server.check():
                print 'server died'
                return
            r = client.query(server_id)
            if r < 0:
                raise gz_error_str()
            elif r == 0:
                break            
            time.sleep(0.1)

        # Connect to the server
        print 'connecting to server'
        if client.connect_wait(server_id, client_id) != 0:
            raise gz_error_str()

        # Open the simulator interface
        print 'opening simulator interface'
        sim = gz_sim()
        while True:
            if not server.check():
                print 'server died'
                server.stop()
                return
            if sim.open(client, 'default') == 0:
                break
            time.sleep(0.1)

        print 'waiting for data'
        while sim.data.sim_time + sim.data.pause_time == 0:
            time.sleep(0.1)

        # Load interface list
        ifaces = LoadInterfaces(client)

        # Create the application with default simulator panel
        app = App(server)
        app.wins.append(gzSim(None, client, sim, ifaces))

        # Let's go
        print 'running'
        app.MainLoop()

    finally:

        # Make sure the server gets stopped
        server.stop()

    return



if __name__ == '__main__':

    main(sys.argv)