diff --git a/visualization/joint_state_publisher/joint_state_publisher b/visualization/joint_state_publisher/joint_state_publisher index ae5aeb2..ea3623b 100755 --- a/visualization/joint_state_publisher/joint_state_publisher +++ b/visualization/joint_state_publisher/joint_state_publisher @@ -27,6 +27,10 @@ class JointStatePublisher(): self.joint_list = [] # for maintaining the original order of the joints self.dependent_joints = get_param("dependent_joints", {}) + pub_def_positions = get_param("publish_default_positions", True) + pub_def_vels = get_param("publish_default_velocities", False) + pub_def_efforts = get_param("publish_default_efforts", False) + # Find all non-fixed joints for child in robot.childNodes: if child.nodeType is child.TEXT_NODE: @@ -51,17 +55,25 @@ class JointStatePublisher(): else: zeroval = 0 - joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval } + joint = {'min':minval, 'max':maxval, 'zero':zeroval} + if pub_def_positions: + joint['position'] = zeroval + if pub_def_vels: + joint['velocity'] = 0.0 + if pub_def_efforts: + joint['effort'] = 0.0 + + if jtype == 'continuous': + joint['continuous'] = True self.free_joints[name] = joint self.joint_list.append(name) use_gui = get_param("use_gui", False) if use_gui: - app = wx.App() + self.app = wx.App() self.gui = JointStatePublisherGui("Joint State Publisher", self) self.gui.Show() - Thread(target=app.MainLoop).start() else: self.gui = None @@ -75,10 +87,30 @@ class JointStatePublisher(): def source_cb(self, msg): for i in range(len(msg.name)): name = msg.name[i] - position = msg.position[i] - if name in self.free_joints: - joint = self.free_joints[name] - joint['value'] = position + if name not in self.free_joints: + continue + + if msg.position: + position = msg.position[i] + else: + position = None + if msg.velocity: + velocity = msg.velocity[i] + else: + velocity = None + if msg.effort: + effort = msg.effort[i] + else: + effort = None + + joint = self.free_joints[name] + if position is not None: + joint['position'] = position + if velocity is not None: + joint['velocity'] = velocity + if effort is not None: + joint['effort'] = effort + if self.gui is not None: self.gui.update_sliders() @@ -87,29 +119,81 @@ class JointStatePublisher(): hz = get_param("rate", 10) # 10hz r = rospy.Rate(hz) + delta = get_param("delta", 0.0) + # Publish Joint States while not rospy.is_shutdown(): msg = JointState() msg.header.stamp = rospy.Time.now() - # Add Free Joints + if delta > 0: + self.update(delta) + + # Initialize msg.position, msg.velocity, and msg.effort. + has_position = len(self.dependent_joints.items()) > 0 + has_velocity = False + has_effort = False 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) + if not has_position and 'position' in joint: + has_position = True + if not has_velocity and 'velocity' in joint: + has_velocity = True + if not has_effort and 'effort' in joint: + has_effort = True + num_joints = (len(self.free_joints.items()) + + len(self.dependent_joints.items())) + if has_position: + msg.position = num_joints * [0.0] + if has_velocity: + msg.velocity = num_joints * [0.0] + if has_effort: + msg.effort = num_joints * [0.0] + + for i, name in enumerate(self.joint_list): msg.name.append(str(name)) - msg.position.append(value) + joint = None + + # Add Free Joint + if name in self.free_joints: + joint = self.free_joints[name] + factor = 1 + offset = 0 + # Add Dependent Joint + elif name in self.dependent_joints: + param = self.dependent_joints[name] + parent = param['parent'] + joint = self.free_joints[parent] + factor = param.get('factor', 1) + offset = param.get('offset', 0) + + if has_position and 'position' in joint: + msg.position[i] = joint['position'] * factor + offset + if has_velocity and 'velocity' in joint: + msg.velocity[i] = joint['velocity'] * factor + if has_effort and 'effort' in joint: + msg.effort[i] = joint['effort'] self.pub.publish(msg) - r.sleep() + def update(self, delta): + for name, joint in self.free_joints.iteritems(): + forward = joint.get('forward', True) + if forward: + joint['position'] += delta + if joint['position'] > joint['max']: + if joint.get('continuous', False): + joint['position'] = joint['min'] + else: + joint['position'] = joint['max'] + joint['forward'] = not forward + else: + joint['position'] -= delta + if joint['position'] < joint['min']: + joint['position'] = joint['min'] + joint['forward'] = not forward + class JointStatePublisherGui(wx.Frame): def __init__(self, title, jsp): wx.Frame.__init__(self, None, -1, title, (-1, -1)); @@ -163,15 +247,16 @@ class JointStatePublisherGui(wx.Frame): purevalue = joint_info['slidervalue'] joint = joint_info['joint'] value = self.sliderToValue(purevalue, joint) - joint['value'] = value + joint['position'] = value self.update_sliders() def update_sliders(self): for (name,joint_info) in self.joint_map.items(): joint = joint_info['joint'] - joint_info['slidervalue'] = self.valueToSlider(joint['value'], joint) + joint_info['slidervalue'] = self.valueToSlider(joint['position'], + joint) joint_info['slider'].SetValue(joint_info['slidervalue']) - joint_info['display'].SetValue("%.2f"%joint['value']) + joint_info['display'].SetValue("%.2f"%joint['position']) def center_event(self, event): self.center() @@ -200,7 +285,12 @@ if __name__ == '__main__': try: rospy.init_node('joint_state_publisher') jsp = JointStatePublisher() - jsp.loop() + + if jsp.gui is None: + jsp.loop() + else: + Thread(target=jsp.loop).start() + jsp.app.MainLoop() except rospy.ROSInterruptException: pass