Joint Controller renamed and reworked as Joint_state_publisher

joint_controller/joint_controller

@@ -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
-

joint_state_publisher/joint_state_publisher

@@ -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
+

joint_state_publisher/manifest.xml

@@ -1,11 +1,12 @@
 <package>
-  <description brief="A node for inputting joint angles directly">
+  <description brief="A node for publishing joint angles">
-	A node for inputting joint angles directly
+	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" />