gps.py

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

# Desc: GPS interface handler
# Author: Andrew Howard
# Date: 27 Oct 2004
# CVS: $Id: gps.py,v 1.1 2004/10/29 00:57:01 inspectorg Exp $

import sys
import math
import time
from wxPython.wx import *

from wxgazebo.panel import gzPanel
from gazebo import *


class gzGps(gzPanel):
    """Mediate GPS interface."""

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

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

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

        # GPS feedback
        self.gpsText = wxStaticText(self, -1, '%060.0f' % 0)

        # Layout stuff in window
        sizer = wxBoxSizer(wxVERTICAL)
        sizer.Add(wxStaticText(self, -1, 'gps'))
        sizer.Add(self.gpsText)

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

        # 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.time != self.updateTime:
            self.updateTime = data.time
            
            gps = 'lonlat %.6f %.6f alt %.2f utm %.2f %.2f' % \
                  (data.longitude, data.latitude, data.altitude, data.utm_e, data.utm_n)
            self.gpsText.SetLabel('%s' % (gps))
            
        self.iface.unlock()
        return