diff --git a/joint_controller/joint_controller b/joint_controller/joint_controller deleted file mode 100755 index 0673965..0000000 --- a/joint_controller/joint_controller +++ /dev/null @@ -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 - diff --git a/joint_controller/CMakeLists.txt b/joint_state_publisher/CMakeLists.txt similarity index 100% rename from joint_controller/CMakeLists.txt rename to joint_state_publisher/CMakeLists.txt diff --git a/joint_controller/Makefile b/joint_state_publisher/Makefile similarity index 100% rename from joint_controller/Makefile rename to joint_state_publisher/Makefile diff --git a/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher new file mode 100755 index 0000000..4ffabba --- /dev/null +++ b/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 + diff --git a/joint_controller/manifest.xml b/joint_state_publisher/manifest.xml similarity index 53% rename from joint_controller/manifest.xml rename to joint_state_publisher/manifest.xml index 97d6fbd..5447c5d 100644 --- a/joint_controller/manifest.xml +++ b/joint_state_publisher/manifest.xml @@ -1,11 +1,12 @@ - - A node for inputting joint angles directly + + A node for publishing joint angles, either through a gui, or with + default values. David Lu!! BSD - http://ros.org/wiki/joint_controller + http://ros.org/wiki/joint_state_publisher diff --git a/joint_controller/screenshot.png b/joint_state_publisher/screenshot.png similarity index 100% rename from joint_controller/screenshot.png rename to joint_state_publisher/screenshot.png