00001
00002
00003 import roslib
00004 roslib.load_manifest('joint_state_gui')
00005 from sensor_msgs.msg import JointState
00006 import rospy
00007
00008 from math import pi
00009
00010 import wx
00011
00012 class SliderPanel(wx.Panel):
00013 """ creates a panel with sliders """
00014 def __init__(self, parent, id, slider_names):
00015 wx.Panel.__init__(self, parent, id)
00016
00017 self.SetBackgroundColour("white")
00018 self.sizer = wx.FlexGridSizer(2, 2, 0, 0)
00019 self.sizer.AddGrowableCol(1, 1)
00020 self.SetSizer(self.sizer)
00021
00022 self.Bind(wx.EVT_SLIDER, self._slider_update)
00023
00024 self.joint_names = slider_names
00025 self.joint_angles = [0.0]*len(self.joint_names)
00026
00027
00028 for i in range(len(self.joint_names)):
00029 l = wx.StaticText(self, id=-1, label=self.joint_names[i])
00030 s = wx.Slider(self, id=i,
00031 style = wx.SL_HORIZONTAL | wx.SL_AUTOTICKS | wx.SL_LABELS)
00032 s.SetRange(-180, 180)
00033 s.SetSizeHints(360, 40)
00034
00035 self.sizer.Add(l, 1, wx.ALIGN_BOTTOM)
00036 self.sizer.Add(s, 1, wx.EXPAND)
00037
00038 def set_slider_cb(self, callback):
00039 self.slider_callback = callback
00040
00041 def _slider_update(self, event):
00042 self.joint_angles[event.Id] = event.Int
00043 self.slider_callback(self.joint_angles, self.joint_names)
00044
00045
00046 class RosComm:
00047 def __init__(self):
00048 """ initialize ROS """
00049
00050 self.joint_names = None
00051
00052 rospy.init_node('joint_gui')
00053 self.name = rospy.names.get_name()
00054
00055 if rospy.client.has_param('~axes'):
00056 self.joint_names = rospy.client.get_param('~axes')
00057
00058 self._pub = rospy.Publisher('/joint_states', JointState)
00059
00060 self._shutdown_timer = wx.Timer()
00061 self._shutdown_timer.Bind(wx.EVT_TIMER, self._on_shutdown_timer)
00062 self._shutdown_timer.Start(100)
00063
00064 def _on_shutdown_timer(self, event):
00065 """shut down the program when the node closes"""
00066 if rospy.is_shutdown():
00067 wx.Exit()
00068
00069 def send_angles(self, joint_angles, joint_names):
00070 """ publish joint states """
00071 jss=JointState()
00072 for i in range(len(joint_names)):
00073 jss.name.append(joint_names[i])
00074 jss.position.append(joint_angles[i]*pi/180.0)
00075 jss.velocity.append(0.0)
00076 jss.header.stamp=rospy.Time.now()
00077
00078 self._pub.publish(jss)
00079
00080
00081
00082
00083 if __name__ == "__main__":
00084
00085 app = wx.PySimpleApp()
00086
00087 ros = RosComm()
00088
00089 if ros.joint_names == None:
00090 rospy.logerr("parameter ~axes not set")
00091 exit()
00092
00093 frame = wx.Frame(None, -1, ros.name, size = (400, 310))
00094 panel = SliderPanel(frame,-1,ros.joint_names)
00095 panel.set_slider_cb(ros.send_angles)
00096
00097 frame.ClientSize = panel.BestSize
00098
00099 frame.Show(True)
00100 app.MainLoop()
00101
