From 36e7861b3ad023d2cb6d191b0d64bd9762b34bb0 Mon Sep 17 00:00:00 2001
From: David Lu!! <davidvlu@gmail.com>
Date: Thu, 21 Feb 2013 16:42:20 -0800
Subject: [PATCH] Add Mimic Tag Support

---
 .../joint_state_publisher                      | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/visualization/joint_state_publisher/joint_state_publisher b/visualization/joint_state_publisher/joint_state_publisher
index ea3623b..35f8eff 100755
--- a/visualization/joint_state_publisher/joint_state_publisher
+++ b/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']: