laser.py

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

# Desc: Camera interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: laser.py,v 1.3 2004/11/01 21:17:23 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 gzLaser(gzPanel):
    """Mediate gui camera data."""

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

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

        # Open interface
        self.iface = gz_laser()
        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.laserPoints = 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 max range %.3f ang res %.2f' % \
                   (data.time, data.max_range, data.res_angle * 180 / math.pi)
            self.textLabel.SetLabel(text)
            
            # Construct laser point list (slow)
            #self.laserPoints = []
            #for i in range(data.range_count):
            #    b = i * data.res_angle + data.min_angle
            #    r = data.ranges[i] / self.plotScale
            #    p = (r * math.cos(b), r * math.sin(b))
            #    self.laserPoints += [p]

            # Construct laser point list (fast)
            self.laserPoints = utils.ranges_to_points(data.min_angle,
                                                      data.res_angle,
                                                      self.plotScale,
                                                      data.range_count,
                                                      data.ranges)

            # 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(0, wy / 2)
        dc.SetAxisOrientation(True, True)

        if self.laserPoints:
            dc.SetPen(wxBLACK_PEN)
            dc.DrawLines(self.laserPoints)
        
        return