bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Joint Controller renamed and reworked as Joint_state_publisher

This commit is contained in:
probablydavid 2010-10-04 19:29:51 +00:00
parent 3d7b63d180
commit 0135a03811
6 changed files with 177 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
joint_controller/joint_controller
View File

@ -1,132 +0,0 @@
#!/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

0
joint_controller/CMakeLists.txt → joint_state_publisher/CMakeLists.txt
View File

0
joint_controller/Makefile → joint_state_publisher/Makefile
View File

173
joint_state_publisher/joint_state_publisher Executable file
View File

@ -0,0 +1,173 @@
#!/usr/bin/python
import roslib; roslib.load_manifest('joint_state_publisher')
import rospy
import wx
import xml.dom.minidom
from sensor_msgs.msg import JointState
from math import pi
from threading import Thread
RANGE = 10000
class JointStatePublisher():
def __init__(self):
description = rospy.get_param("robot_description")
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
self.free_joints = {}
self.joint_list = [] # for maintaining the original order of the joints
self.dependent_joints = rospy.get_param("dependent_joints", {})
# Find all non-fixed 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 = -pi
maxval = pi
else:
limit = child.getElementsByTagName('limit')[0]
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
if name in self.dependent_joints:
continue
if minval > 0 or maxval < 0:
zeroval = (maxval + minval)/2
else:
zeroval = 0
joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
self.free_joints[name] = joint
self.joint_list.append(name)
self.pub = rospy.Publisher('joint_states', JointState)
def loop(self):
hz = rospy.get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
# Add Free Joints
for (name,joint) in self.free_joints.items():
msg.name.append(str(name))
msg.position.append(joint['value'])
# Add Dependent Joints
for (name,param) in self.dependent_joints.items():
parent = param['parent']
baseval = self.free_joints[parent]['value']
value = baseval * param.get('factor', 1)
msg.name.append(str(name))
msg.position.append(value)
self.pub.publish(msg)
r.sleep()
class JointStatePublisherGui(wx.Frame):
def __init__(self, title, jsp):
wx.Frame.__init__(self, None, -1, title, (-1, -1));
self.jsp = jsp
self.joint_map = {}
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 self.jsp.joint_list:
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
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)
slider = wx.Slider(panel, -1, RANGE/2, 0, RANGE,
style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL)
slider.SetFont(font)
box.Add(slider, 1, wx.EXPAND)
self.joint_map[name] = {'slidervalue':0, 'display':display,
'slider':slider, 'joint':joint}
### Buttons ###
self.ctrbutton = wx.Button(panel, 1, 'Center')
self.Bind(wx.EVT_SLIDER, self.sliderUpdate)
wx.EVT_BUTTON(self, 1, self.center_event)
box.Add(self.ctrbutton, 0, wx.EXPAND)
panel.SetSizer(box)
self.center()
box.Fit(self)
self.update_values()
def update_values(self):
for (name,joint_info) in self.joint_map.items():
purevalue = joint_info['slidervalue']
joint = joint_info['joint']
value = self.sliderToValue(purevalue, joint)
joint['value'] = value
joint_info['slider'].SetValue(purevalue)
joint_info['display'].SetValue("%.2f"%value)
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for (name,joint_info) in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint)
self.update_values()
def sliderUpdate(self, event):
for (name,joint_info) in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].GetValue()
self.update_values()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max']-joint['min']) * pctvalue
if __name__ == '__main__':
try:
rospy.init_node('joint_state_publisher')
jsp = JointStatePublisher()
use_gui = rospy.get_param("use_gui", False)
if use_gui:
app = wx.App()
gui = JointStatePublisherGui("Joint State Publisher", jsp)
gui.Show()
Thread(target=app.MainLoop).start()
jsp.loop()
except rospy.ROSInterruptException: pass

7
joint_controller/manifest.xml → joint_state_publisher/manifest.xml
View File

@ -1,11 +1,12 @@
<package>
<description brief="A node for inputting joint angles directly">
A node for inputting joint angles directly
<description brief="A node for publishing joint angles">
A node for publishing joint angles, either through a gui, or with
default values.
</description>
<author>David Lu!!</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/joint_controller</url>
<url>http://ros.org/wiki/joint_state_publisher</url>
<depend package="rospy" />
<depend package="sensor_msgs" />
<rosdep name="wxpython" />

0
joint_controller/screenshot.png → joint_state_publisher/screenshot.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 11 KiB

After

Width:  |  Height:  |  Size: 11 KiB