sonar.py

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

# Desc: Camera interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: sonar.py,v 1.1 2004/11/04 07:30:25 inspectorg Exp $

import math
import sys
import time

from wxPython.wx import *    
from gazebo import *

from wxgazebo.panel import gzPanel
from wxgazebo import utils

# Menu ids
# TODO


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

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

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

        # Open interface
        self.iface = gz_sonar()
        if self.iface.open(client, gzid) != 0:
            raise gz_error_str()
        
        # Create menu
        menuBar = wxMenuBar()
        self.controlMenu = wxMenu()

        # TODO self.controlMenu.Append(ID_ROLL_LOCK, 'Roll lock', '', True)        
        menuBar.Append(self.controlMenu, 'Controls')
        self.SetMenuBar(menuBar)

        # Initialize menu state
        # TODO self.controlMenu.Check(ID_ROLL_LOCK, data.roll_lock)

        # Create plot panel
        self.plotPanel = wxWindow(self, -1, (0, 0), (200, 200))
        EVT_PAINT(self.plotPanel, self.OnPlotPaint)

        # Plot scale (m/pixel)
        self.plotScale = 0.1

        # Widget for displaying textual info
        self.textLabel = wxStaticText(self, -1, '%020f' % 0)

        # Layout stuff in window
        sizer = wxBoxSizer(wxVERTICAL)
        sizer.Add(self.plotPanel, 1, wxEXPAND)
        sizer.Add(self.textLabel, 0, wxEXPAND)

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

        # Misc data
        self.updateTime = 0.0
        self.sonarPoints = 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.time != self.updateTime:
            self.updateTime = data.time

            # Update labels
            text = 'time %14.3f' % (data.time)
            self.textLabel.SetLabel(text)
            
            # Construct sonar point list (slow)
            self.sonarPoints = []
            for i in range(data.sonar_count):
                o = (data.sonar_pos[i][0], data.sonar_pos[i][1])
                b = data.sonar_rot[i][2]
                r = data.sonar_ranges[i]
                p = (o[0] + r * math.cos(b), o[1] + r * math.sin(b))

                o = map(lambda x : x / self.plotScale, o)
                p = map(lambda x : x / self.plotScale, p)

                self.sonarPoints += [(o, p)]

            # Refresh the plot
            self.plotPanel.Refresh(False)

        self.iface.unlock()
        return


    def OnPlotPaint(self, event):
        """Handle paint events"""

        dc = wxPaintDC(self.plotPanel)

        # Change to centered CS with +y running upwards
        (wx, wy) = dc.GetSizeTuple()
        dc.DrawRectangle(0, 0, wx, wy)                
        dc.SetDeviceOrigin(wx / 2, wy / 2)
        dc.SetAxisOrientation(True, True)

        if self.sonarPoints == None:
            return

        dc.SetPen(wxBLACK_PEN)
        for (a, b) in self.sonarPoints:
            dc.DrawLines((a, b))            
        
        return