controllerGUI.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """ 
00004   A simple Controller GUI to drive robots and pose heads.
00005   Copyright (c) 2008-2011 Michael E. Ferguson.  All right reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without
00008   modification, are permitted provided that the following conditions are met:
00009       * Redistributions of source code must retain the above copyright
00010         notice, this list of conditions and the following disclaimer.
00011       * Redistributions in binary form must reproduce the above copyright
00012         notice, this list of conditions and the following disclaimer in the
00013         documentation and/or other materials provided with the distribution.
00014       * Neither the name of Vanadium Labs LLC nor the names of its 
00015         contributors may be used to endorse or promote products derived 
00016         from this software without specific prior written permission.
00017   
00018   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019   ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020   WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021   DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024   OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027   ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029 
00030 import roslib; roslib.load_manifest('arbotix_python')
00031 import rospy
00032 import wx
00033 
00034 from math import radians
00035 
00036 from geometry_msgs.msg import Twist
00037 from sensor_msgs.msg import JointState
00038 from std_msgs.msg import Float64
00039 from arbotix_msgs.srv import Relax
00040 from arbotix_python.servos import *
00041 
00042 width = 325
00043 
00044 class servoSlider():
00045     def __init__(self, parent, min_angle, max_angle, name, i):
00046         self.name = name
00047         if name.find("_joint") > 0: # remove _joint for display name
00048             name = name[0:-6]
00049         self.position = wx.Slider(parent, -1, 0, int(min_angle*100), int(max_angle*100), wx.DefaultPosition, (150, -1), wx.SL_HORIZONTAL)
00050         self.enabled = wx.CheckBox(parent, i, name+":")
00051         self.enabled.SetValue(False)
00052         self.position.Disable()
00053         self.synched = 0
00054 
00055     def setPosition(self, angle):
00056         self.position.SetValue(int(angle*100))
00057 
00058     def getPosition(self):
00059         return self.position.GetValue()/100.0
00060 
00061 class controllerGUI(wx.Frame):
00062     TIMER_ID = 100
00063 
00064     def __init__(self, parent, debug = False):  
00065         wx.Frame.__init__(self, parent, -1, "ArbotiX Controller GUI", style = wx.DEFAULT_FRAME_STYLE & ~ (wx.RESIZE_BORDER | wx.MAXIMIZE_BOX))
00066         sizer = wx.GridBagSizer(5,5)
00067 
00068         # Move Base
00069         drive = wx.StaticBox(self, -1, 'Move Base')
00070         drive.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
00071         driveBox = wx.StaticBoxSizer(drive,orient=wx.VERTICAL) 
00072         self.movebase = wx.Panel(self,size=(width,width-20))
00073         self.movebase.SetBackgroundColour('WHITE')
00074         self.movebase.Bind(wx.EVT_MOTION, self.onMove)  
00075         wx.StaticLine(self.movebase, -1, (width/2, 0), (1,width), style=wx.LI_VERTICAL)
00076         wx.StaticLine(self.movebase, -1, (0, width/2), (width,1))
00077         driveBox.Add(self.movebase)        
00078         sizer.Add(driveBox,(0,0),wx.GBSpan(1,1),wx.EXPAND|wx.TOP|wx.BOTTOM|wx.LEFT,5)
00079         self.forward = 0
00080         self.turn = 0
00081         self.X = 0
00082         self.Y = 0
00083         self.cmd_vel = rospy.Publisher('cmd_vel', Twist)
00084 
00085         # Move Servos
00086         servo = wx.StaticBox(self, -1, 'Move Servos')
00087         servo.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
00088         servoBox = wx.StaticBoxSizer(servo,orient=wx.VERTICAL) 
00089         servoSizer = wx.GridBagSizer(5,5)
00090 
00091         joint_defaults = getServosFromURDF()
00092         
00093         i = 0
00094         dynamixels = rospy.get_param("/arbotix/dynamixels", dict())
00095         
00096 
00097         self.servos = list()
00098         self.publishers = list()
00099         self.relaxers = list()
00100 
00101         self.synched = list()
00102 
00103         
00104         # create sliders and publishers
00105         for name in sorted(dynamixels.keys()):
00106             # pull angles
00107             min_angle, max_angle = getServoLimits(name, joint_defaults)
00108             # create publisher
00109             self.publishers.append(rospy.Publisher(name+'/command', Float64))
00110             self.relaxers.append(rospy.ServiceProxy(name+'/relax', Relax))
00111             # create slider
00112             s = servoSlider(self, min_angle, max_angle, name, i)
00113             servoSizer.Add(s.enabled,(i,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)   
00114             servoSizer.Add(s.position,(i,1), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
00115 
00116             #check for synch
00117             synch = rospy.get_param("/arbotix/dynamixels/"+name+"/synched", -1)
00118             s.synched = int(synch)
00119 
00120             self.servos.append(s)
00121             i += 1
00122             
00123         # add everything
00124         servoBox.Add(servoSizer) 
00125         sizer.Add(servoBox, (0,1), wx.GBSpan(1,1), wx.EXPAND|wx.TOP|wx.BOTTOM|wx.RIGHT,5)
00126         self.Bind(wx.EVT_CHECKBOX, self.enableSliders)
00127         # now we can subscribe
00128         rospy.Subscriber('joint_states', JointState, self.stateCb)
00129 
00130         # timer for output
00131         self.timer = wx.Timer(self, self.TIMER_ID)
00132         self.timer.Start(50)
00133         wx.EVT_CLOSE(self, self.onClose)
00134         wx.EVT_TIMER(self, self.TIMER_ID, self.onTimer)
00135 
00136         # bind the panel to the paint event
00137         wx.EVT_PAINT(self, self.onPaint)
00138         self.dirty = 1
00139         self.onPaint()
00140 
00141         self.SetSizerAndFit(sizer)
00142         self.Show(True)
00143 
00144     def onClose(self, event):
00145         self.timer.Stop()
00146         self.Destroy()
00147 
00148     def enableSliders(self, event):
00149         servo = event.GetId()
00150         if event.IsChecked(): 
00151             self.servos[servo].position.Enable()
00152         else:
00153             self.servos[servo].position.Disable()
00154             self.relaxers[servo]()
00155 
00156     def stateCb(self, msg):        
00157         for servo in self.servos:
00158             if not servo.enabled.IsChecked():
00159                 try:
00160                     idx = msg.name.index(servo.name)
00161                     servo.setPosition(msg.position[idx])
00162                 except: 
00163                     pass
00164 
00165     def onPaint(self, event=None):
00166         # this is the wx drawing surface/canvas
00167         dc = wx.PaintDC(self.movebase)
00168         dc.Clear()
00169         # draw crosshairs
00170         dc.SetPen(wx.Pen("black",1))
00171         dc.DrawLine(width/2, 0, width/2, width)
00172         dc.DrawLine(0, width/2, width, width/2)
00173         dc.SetPen(wx.Pen("red",2))
00174         dc.SetBrush(wx.Brush('red', wx.SOLID))
00175         dc.SetPen(wx.Pen("black",2))
00176         dc.DrawCircle((width/2) + self.X*(width/2), (width/2) - self.Y*(width/2), 5)  
00177 
00178     def onMove(self, event=None):
00179         if event.LeftIsDown():        
00180             pt = event.GetPosition()
00181             if pt[0] > 0 and pt[0] < width and pt[1] > 0 and pt[1] < width:
00182                 self.forward = ((width/2)-pt[1])/2
00183                 self.turn = (pt[0]-(width/2))/2 
00184                 self.X = (pt[0]-(width/2.0))/(width/2.0)
00185                 self.Y = ((width/2.0)-pt[1])/(width/2.0)        
00186         else:
00187             self.forward = 0; self.Y = 0
00188             self.turn = 0; self.X = 0
00189         self.onPaint()          
00190         
00191 
00192     def onTimer(self, event=None):
00193         # send joint update
00194 
00195         #store info the servo pairs
00196         shoulder_lift = 0
00197         elbow_flex = 0
00198 
00199         #set synched servo values
00200         for s in self.servos:
00201                 if s.synched != -1:
00202                         s.setPosition(self.servos[s.synched].getPosition()*-1)
00203 
00204         for s,p in zip(self.servos,self.publishers):
00205             if s.enabled.IsChecked():
00206                 d = Float64()
00207                 d.data = s.getPosition()
00208 
00209                 p.publish(d)
00210                 
00211         # send base updates
00212         t = Twist()
00213         t.linear.x = self.forward/150.0; t.linear.y = 0; t.linear.z = 0
00214         if self.forward > 0:
00215             t.angular.x = 0; t.angular.y = 0; t.angular.z = -self.turn/50.0
00216         else:
00217             t.angular.x = 0; t.angular.y = 0; t.angular.z = self.turn/50.0
00218         self.cmd_vel.publish(t)
00219 
00220 if __name__ == '__main__':
00221     # initialize GUI
00222     rospy.init_node('controllerGUI')
00223     app = wx.PySimpleApp()
00224     frame = controllerGUI(None, True)
00225     app.MainLoop()
00226 


corobot_arm
Author(s): Morgan Cormier/mcormier@coroware.com
autogenerated on Tue Jan 7 2014 11:39:14