133 lines
3.8 KiB
Python
Executable File
133 lines
3.8 KiB
Python
Executable File
#!/usr/bin/python
|
|
|
|
import roslib; roslib.load_manifest('joint_controller')
|
|
import rospy
|
|
import wx
|
|
import xml.dom.minidom
|
|
from sensor_msgs.msg import JointState
|
|
|
|
RANGE = 1000
|
|
|
|
class JointController(wx.Frame):
|
|
def __init__(self, parent, id, title):
|
|
self.dependent_joints = rospy.get_param("dependent_joints", {})
|
|
(self.free_joints, self.fixed_joints, ordered_joints) = self.getFreeJoints()
|
|
|
|
wx.Frame.__init__(self, parent, id, title, (-1, -1));
|
|
panel = wx.Panel(self, wx.ID_ANY);
|
|
box = wx.BoxSizer(wx.VERTICAL)
|
|
font = wx.Font(9, wx.SWISS, wx.NORMAL, wx.BOLD)
|
|
|
|
### Sliders ###
|
|
for name in ordered_joints:
|
|
joint = self.free_joints[name]
|
|
row = wx.GridSizer(1,2)
|
|
label = wx.StaticText(panel, -1, name)
|
|
label.SetFont(font)
|
|
row.Add(label, 1, wx.ALIGN_CENTER_VERTICAL)
|
|
|
|
display = wx.TextCtrl (panel, value=str(0), style=wx.TE_READONLY | wx.ALIGN_RIGHT)
|
|
|
|
row.Add(display, flag= wx.ALIGN_RIGHT| wx.ALIGN_CENTER_VERTICAL)
|
|
box.Add(row, 1, wx.EXPAND)
|
|
slide = wx.Slider(panel, -1, RANGE/2, 0, RANGE, style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL)
|
|
slide.SetFont(font)
|
|
box.Add(slide, 1, wx.EXPAND)
|
|
|
|
joint['display'] = display
|
|
joint['slider'] = slide
|
|
|
|
### Buttons ###
|
|
self.ctrbutton = wx.Button(panel, 1, 'Center')
|
|
self.Bind(wx.EVT_SLIDER, self.sliderUpdate)
|
|
|
|
wx.EVT_BUTTON(self, 1, self.center)
|
|
|
|
box.Add(self.ctrbutton, 0, wx.EXPAND)
|
|
|
|
panel.SetSizer(box)
|
|
self.Center()
|
|
|
|
self.pub = rospy.Publisher('joint_states', JointState)
|
|
|
|
box.Fit(self)
|
|
self.publish()
|
|
|
|
def getFreeJoints(self):
|
|
description = rospy.get_param("robot_description")
|
|
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
|
|
joints = {}
|
|
fixed_joints = {}
|
|
ordered_joints = []
|
|
|
|
for child in robot.childNodes:
|
|
if child.nodeType is child.TEXT_NODE:
|
|
continue
|
|
if child.localName == 'joint':
|
|
jtype = child.getAttribute('type')
|
|
if jtype == 'fixed':
|
|
continue
|
|
name = child.getAttribute('name')
|
|
if jtype == 'continuous':
|
|
minval = -3.1415
|
|
maxval = 3.1415
|
|
else:
|
|
limit = child.getElementsByTagName('limit')[0]
|
|
minval = float(limit.getAttribute('lower'))
|
|
maxval = float(limit.getAttribute('upper'))
|
|
if minval == maxval:
|
|
fixed_joints[name] = minval
|
|
continue
|
|
if name in self.dependent_joints:
|
|
continue
|
|
zeroval = (int)(-minval * RANGE / (maxval - minval))
|
|
joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'slidervalue':zeroval, 'value':0 }
|
|
joints[name] = joint
|
|
ordered_joints.append(name)
|
|
return (joints, fixed_joints, ordered_joints)
|
|
|
|
def center(self, event):
|
|
rospy.loginfo("Centering")
|
|
for (name,joint) in self.free_joints.items():
|
|
joint['slidervalue'] = joint['zero']
|
|
self.publish()
|
|
|
|
def sliderUpdate(self, event):
|
|
for (name,joint) in self.free_joints.items():
|
|
joint['slidervalue'] = joint['slider'].GetValue()
|
|
self.publish()
|
|
|
|
def publish(self):
|
|
msg = JointState()
|
|
msg.header.stamp = rospy.Time.now()
|
|
|
|
for (name,joint) in self.free_joints.items():
|
|
msg.name.append(str(name))
|
|
purevalue = joint['slidervalue']
|
|
pctvalue = purevalue / float( RANGE)
|
|
value = joint['min'] + (joint['max']-joint['min']) * pctvalue
|
|
joint['value'] = value
|
|
msg.position.append(value)
|
|
joint['slider'].SetValue(purevalue)
|
|
joint['display'].SetValue("%.2f"%value)
|
|
for (name,value) in self.fixed_joints.items():
|
|
msg.name.append(str(name))
|
|
msg.position.append(value)
|
|
for (name,param) in self.dependent_joints.items():
|
|
msg.name.append(str(name))
|
|
parent = param['parent']
|
|
baseval = self.free_joints[parent]['value']
|
|
value = baseval * param.get('factor', 1)
|
|
msg.position.append(value)
|
|
self.pub.publish(msg)
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
rospy.init_node('joint_controller')
|
|
app = wx.App()
|
|
gui = JointController(None, -1, "Joint Controller")
|
|
gui.Show()
|
|
app.MainLoop()
|
|
except rospy.ROSInterruptException: pass
|
|
|