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

Add Mimic Tag Support

This commit is contained in:
David Lu!! 2013-02-21 16:42:20 -08:00
parent 49c352909b
commit 36e7861b3a
1 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
visualization/joint_state_publisher/joint_state_publisher
View File

@ -26,6 +26,7 @@ class JointStatePublisher():
self.free_joints = {} self.free_joints = {}
self.joint_list = [] # for maintaining the original order of the joints self.joint_list = [] # for maintaining the original order of the joints
self.dependent_joints = get_param("dependent_joints", {}) self.dependent_joints = get_param("dependent_joints", {})
use_mimic = get_param('use_mimic_tags', True)
pub_def_positions = get_param("publish_default_positions", True) pub_def_positions = get_param("publish_default_positions", True)
pub_def_vels = get_param("publish_default_velocities", False) pub_def_vels = get_param("publish_default_velocities", False)
@ -40,6 +41,7 @@ class JointStatePublisher():
if jtype == 'fixed': if jtype == 'fixed':
continue continue
name = child.getAttribute('name') name = child.getAttribute('name')
self.joint_list.append(name)
if jtype == 'continuous': if jtype == 'continuous':
minval = -pi minval = -pi
maxval = pi maxval = pi
@ -48,6 +50,18 @@ class JointStatePublisher():
minval = float(limit.getAttribute('lower')) minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper')) maxval = float(limit.getAttribute('upper'))
mimic_tags = child.getElementsByTagName('mimic')
if use_mimic and len(mimic_tags)==1:
tag = mimic_tags[0]
entry = {'parent': tag.getAttribute('joint')}
if tag.hasAttribute('multiplier'):
entry['factor'] = float(tag.getAttribute('multiplier'))
if tag.hasAttribute('offset'):
entry['offset'] = float(tag.getAttribute('offset'))
self.dependent_joints[name] = entry
continue
if name in self.dependent_joints: if name in self.dependent_joints:
continue continue
if minval > 0 or maxval < 0: if minval > 0 or maxval < 0:
@ -66,7 +80,7 @@ class JointStatePublisher():
if jtype == 'continuous': if jtype == 'continuous':
joint['continuous'] = True joint['continuous'] = True
self.free_joints[name] = joint self.free_joints[name] = joint
self.joint_list.append(name)
use_gui = get_param("use_gui", False) use_gui = get_param("use_gui", False)
@ -205,6 +219,8 @@ class JointStatePublisherGui(wx.Frame):
### Sliders ### ### Sliders ###
for name in self.jsp.joint_list: for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name] joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']: if joint['min'] == joint['max']: