Add Mimic Tag Support

visualization/joint_state_publisher/joint_state_publisher

@@ -26,6 +26,7 @@ class JointStatePublisher():
         self.free_joints = {}
         self.joint_list = [] # for maintaining the original order of the 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_vels = get_param("publish_default_velocities", False)
@@ -40,6 +41,7 @@ class JointStatePublisher():
                 if jtype == 'fixed':
                     continue
                 name = child.getAttribute('name')
+                self.joint_list.append(name)
                 if jtype == 'continuous':
                     minval = -pi
                     maxval = pi
@@ -47,6 +49,18 @@ class JointStatePublisher():
                     limit = child.getElementsByTagName('limit')[0]
                     minval = float(limit.getAttribute('lower'))
                     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:
                     continue
@@ -66,7 +80,7 @@ class JointStatePublisher():
                 if jtype == 'continuous':
                     joint['continuous'] = True
                 self.free_joints[name] = joint
-                self.joint_list.append(name)
+                
 
         use_gui = get_param("use_gui", False)
 
@@ -205,6 +219,8 @@ class JointStatePublisherGui(wx.Frame):
         
         ### Sliders ###
         for name in self.jsp.joint_list:
+            if name not in self.jsp.free_joints:
+                continue
             joint = self.jsp.free_joints[name]
 
             if joint['min'] == joint['max']: