position.py

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· Python 代码 · 共 195 行

# Desc: position interface handler
# Author: Andrew Howard
# Date: 19 Sep 2004
# CVS: $Id: position.py,v 1.7 2006/07/07 17:57:37 natepak Exp $

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

from wxgazebo.panel import gzPanel
from gazebo import *


# Convert radians to degrees
def rtod(a):
    return a / math.pi * 180


# IDs
ID_STOP = 0x100


class gzPosition(gzPanel):
    """Mediate position interface."""

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

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

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

        # TESTING
        self.iface.data.cmd_enable_motors = 1

        # Position/rotation feedback
        self.poseText = wxStaticText(self, -1, '%040.0f' % 0)
        self.velText = wxStaticText(self, -1, '%040.0f' % 0)
        self.cmdText = wxStaticText(self, -1, '%040.0f' % 0)

        # Scale buttons
        self.posScaleButton = wxToggleButton(self, -1, 'linear x10')
        self.rotScaleButton = wxToggleButton(self, -1, 'angular x2')

        # Stop button
        self.stopButton = wxButton(self, ID_STOP, 'Stop')
        EVT_BUTTON(self, ID_STOP, self.OnStop)

        # Scale factors for sliders
        self.posScale = lambda : +2.0 / 1000 * (1 + 9 * self.posScaleButton.GetValue())
        self.rotScale = lambda : +math.pi / 2 / 1000 * (1 + 1 * self.rotScaleButton.GetValue())

        self.velPosF = lambda x : x * self.posScale()
        self.velPosFi = lambda x : int(x  / self.posScale())

        self.velRotF = lambda x : x * self.rotScale()
        self.velRotFi = lambda x : int(x / self.rotScale())

        # Linear speed sliders
        self.velPosSliders = (wxSlider(self, -1,
                                       self.velPosFi(self.iface.data.cmd_vel_pos[0]),
                                       -1000, 1000, wxDefaultPosition, (200, -1)),
                              wxSlider(self, -1,
                                       self.velPosFi(self.iface.data.cmd_vel_pos[1]),
                                       -1000, 1000, wxDefaultPosition, (200, -1)),
                              wxSlider(self, -1,
                                       self.velPosFi(self.iface.data.cmd_vel_pos[2]),
                                       -1000, 1000, wxDefaultPosition, (200, -1))
                              )
        
        # Yaw speed slider
        self.velRotSliders = (wxSlider(self, -1,
                                       self.velRotFi(self.iface.data.cmd_vel_rot[0]),
                                       -1000, 1000, wxDefaultPosition, (200, -1)),
                              wxSlider(self, -1,
                                       self.velRotFi(self.iface.data.cmd_vel_rot[1]),
                                       -1000, 1000, wxDefaultPosition, (200, -1)),
                              wxSlider(self, -1,
                                       self.velRotFi(self.iface.data.cmd_vel_rot[2]),
                                       -1000, 1000, wxDefaultPosition, (200, -1)))

        # Layout stuff in window
        sizer = wxFlexGridSizer(4, 2, 2, 4)

        sizer.Add(wxStaticText(self, -1, 'pose'))
        sizer.Add(self.poseText)

        sizer.Add(wxStaticText(self, -1, 'vel'))
        sizer.Add(self.velText)
        
        sizer.Add(wxStaticText(self, -1, 'cmd vel'))
        sizer.Add(self.cmdText)
        
        sizer.Add(wxStaticText(self, -1, 'cmd lin vel x'))
        sizer.Add(self.velPosSliders[0], 0, wxEXPAND)
        
        sizer.Add(wxStaticText(self, -1, 'cmd lin vel y'))
        sizer.Add(self.velPosSliders[1], 0, wxEXPAND)
        
        sizer.Add(wxStaticText(self, -1, 'cmd lin vel z'))
        sizer.Add(self.velPosSliders[2], 0, wxEXPAND)

        sizer.Add(wxStaticText(self, -1, 'cmd ang vel x'))
        sizer.Add(self.velRotSliders[0], 0, wxEXPAND)

        sizer.Add(wxStaticText(self, -1, 'cmd ang vel y'))
        sizer.Add(self.velRotSliders[1], 0, wxEXPAND)

        sizer.Add(wxStaticText(self, -1, 'cmd ang vel z'))
        sizer.Add(self.velRotSliders[2], 0, wxEXPAND)

        nsizer = wxBoxSizer(wxHORIZONTAL)
        nsizer.Add(self.posScaleButton, 1, wxEXPAND)
        nsizer.Add(self.rotScaleButton, 1, wxEXPAND)

        sizer.Add((0, 0))
        sizer.Add((nsizer), 1, wxEXPAND)

        sizer.Add((0, 0))
        sizer.Add((self.stopButton), 0, wxEXPAND)

        # 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

            pos = '%.3f %.3f %.3f' % (data.pos[0], data.pos[1], data.pos[2])
            rot = '%.1f %.1f %.1f' % \
                  (rtod(data.rot[0]), rtod(data.rot[1]), rtod(data.rot[2]))
            self.poseText.SetLabel('%s : %s' % (pos, rot))

            pos = '%.3f %.3f %.3f' % (data.vel_pos[0], data.vel_pos[1], data.vel_pos[2])
            rot = '%.1f %.1f %.1f' % \
                  (rtod(data.vel_rot[0]), rtod(data.vel_rot[1]), rtod(data.vel_rot[2]))
            self.velText.SetLabel('%s : %s' % (pos, rot))

        # Update velocity commands
        data.cmd_vel_pos = (self.velPosF(self.velPosSliders[0].GetValue()),
                            self.velPosF(self.velPosSliders[1].GetValue()),
                            self.velPosF(self.velPosSliders[2].GetValue()))
        data.cmd_vel_rot = (self.velRotF(self.velRotSliders[0].GetValue()),
                            self.velRotF(self.velRotSliders[1].GetValue()),
                            self.velRotF(self.velRotSliders[2].GetValue()))

        # Update displayed velocity commands
        pos = '%.3f %.3f %.3f' % (data.cmd_vel_pos[0], data.cmd_vel_pos[1], data.cmd_vel_pos[2])
        rot = '%.1f %.1f %.1f' % \
              (rtod(data.cmd_vel_rot[0]), rtod(data.cmd_vel_rot[1]), rtod(data.cmd_vel_rot[2]))
        self.cmdText.SetLabel('%s : %s' % (pos, rot))

        self.iface.unlock()

        return


    def OnStop(self, event):

        for slider in self.velPosSliders:
            slider.SetValue(self.velPosFi(0.0))

        for slider in self.velRotSliders:
            slider.SetValue(self.velRotFi(0.0))
            
        return