diff --git a/README.md b/README.md index cce53a0b..ef6d2698 100644 --- a/README.md +++ b/README.md @@ -12,5 +12,7 @@ More information is available at [`ros/robot_model#195`](https://github.com/ros/ * `collada_urdf` and `collada_parser` * [`ros/collada_urdf`](https://github.com/ros/collada_urdf) +* `joint_state_publisher` + * [`ros/joint_state_publisher`](https://github.com/ros/joint_state_publisher) * `kdl_parser` and `kdl_parser_py` * [`ros/kdl_parser`](https://github.com/ros/kdl_parser) diff --git a/joint_state_publisher/CHANGELOG.rst b/joint_state_publisher/CHANGELOG.rst deleted file mode 100644 index 3743f6db..00000000 --- a/joint_state_publisher/CHANGELOG.rst +++ /dev/null @@ -1,110 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package joint_state_publisher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.12.11 (2017-06-27) --------------------- - -1.12.10 (2017-06-24) --------------------- - -1.12.9 (2017-04-26) -------------------- - -1.12.8 (2017-03-27) -------------------- -* [joint_state_publisher] Handle time moving backwards - Without this patch, joint_state_publisher dies whenever the ROS time moves backwards (e.g., when running `rosbag play --clock --loop`). -* Switch a couple more packages over to Chris and Shane. -* Fix rostest dependency. -* Add recursive mimic joint (`#177 `_) - * Add recursive mimic joint -* Contributors: Alessandro Tondo, Chris Lalancette, Martin Günther, Mike Purvis - -1.12.7 (2017-01-26) -------------------- -* Fixed a crash which happened when there were ``0`` free joints, opens empty window (`#178 `_) -* Contributors: Bence Magyar - -1.12.6 (2017-01-04) -------------------- -* Migrated slots in joint state publisher gui to Qt5 (`#147 `_) -* Now uses GridLayout to support large numbers of joints and small screens (`#150 `_) -* Contributors: Bence Magyar, Michał Barciś - -1.12.5 (2016-10-27) -------------------- -* Fix initial position of sliders in joint_state_publisher GUI (`#148 `_) - Caused by a regression in 8c6cf9841cb, the slider positions are not initialized correctly - from the provided zero positions at startup. - This commit fixes the issue, by adding the call to center() again that got lost. -* Contributors: Timm Linder - -1.12.4 (2016-08-23) -------------------- - -1.12.3 (2016-06-10) -------------------- -* Fix circular logic in joint state publisher events (`#140 `_) -* Use signal and sys.exit to fix shutdown in joint_state_publisher (`#139 `_) -* joint_state_publisher: Change slider update method (`#135 `_) -* Contributors: Jackie Kay, vincentrou - -1.12.2 (2016-04-12) -------------------- -* Migrate qt (`#128 `_) - * Migrate JointStatePublisher from wxPython to qt5 -* Contributors: Jackie Kay - -1.12.1 (2016-04-10) -------------------- - -1.11.8 (2015-09-11) -------------------- - -1.11.7 (2015-04-22) -------------------- -* Added a randomize button for the joints. -* Contributors: Aaron Blasdel - -1.11.6 (2014-11-30) -------------------- -* Added floating joints to joint types ignored by publisher -* warn when joints have no limits -* add queue_size for publisher -* Contributors: Jihoon Lee, Michael Ferguson, Shaun Edwards - -1.11.5 (2014-07-24) -------------------- - -1.11.4 (2014-07-07) -------------------- -* Update package.xml - Updating author and maintainer email for consistency -* Contributors: David Lu!! - -1.11.3 (2014-06-24) -------------------- - -1.11.2 (2014-03-22) -------------------- - -1.11.1 (2014-03-20) -------------------- - -1.11.0 (2014-02-21) -------------------- -* Use #!/usr/bin/env python for systems with multiple Python versions. -* Contributors: Benjamin Chretien - -1.10.18 (2013-12-04) --------------------- - -1.10.16 (2013-11-18) --------------------- - -1.10.15 (2013-08-17) --------------------- - -* joint_state_publisher: do not install script to global bin - Also clean up no longer required setup.py diff --git a/joint_state_publisher/CMakeLists.txt b/joint_state_publisher/CMakeLists.txt deleted file mode 100644 index 553e5e8e..00000000 --- a/joint_state_publisher/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(joint_state_publisher) - -find_package(catkin REQUIRED) - -catkin_package() - -install(PROGRAMS joint_state_publisher/joint_state_publisher DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - -if(CATKIN_ENABLE_TESTING) - find_package(rostest) - add_rostest(test/test_mimic_chain.launch) - add_rostest(test/test_mimic_cycle.launch) - add_rostest(test/test_zero_joints.launch) -endif() diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher/joint_state_publisher deleted file mode 100755 index 44565452..00000000 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher +++ /dev/null @@ -1,442 +0,0 @@ -#!/usr/bin/env python - -import rospy -import random - -from python_qt_binding.QtCore import pyqtSlot -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtCore import Signal -from python_qt_binding.QtGui import QFont -from python_qt_binding.QtWidgets import QApplication -from python_qt_binding.QtWidgets import QHBoxLayout -from python_qt_binding.QtWidgets import QLabel -from python_qt_binding.QtWidgets import QLineEdit -from python_qt_binding.QtWidgets import QPushButton -from python_qt_binding.QtWidgets import QSlider -from python_qt_binding.QtWidgets import QVBoxLayout -from python_qt_binding.QtWidgets import QGridLayout -from python_qt_binding.QtWidgets import QSpinBox -from python_qt_binding.QtWidgets import QWidget - -import xml.dom.minidom -from sensor_msgs.msg import JointState -from math import pi -from threading import Thread -import sys -import signal -import math - -RANGE = 10000 - - -def get_param(name, value=None): - private = "~%s" % name - if rospy.has_param(private): - return rospy.get_param(private) - elif rospy.has_param(name): - return rospy.get_param(name) - else: - return value - - -class JointStatePublisher(): - def __init__(self): - description = 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 = get_param("dependent_joints", {}) - use_mimic = get_param('use_mimic_tags', True) - use_small = get_param('use_smallest_joint_limits', True) - - self.zeros = get_param("zeros") - - 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: - continue - if child.localName == 'joint': - jtype = child.getAttribute('type') - if jtype == 'fixed' or jtype == 'floating': - continue - name = child.getAttribute('name') - self.joint_list.append(name) - if jtype == 'continuous': - minval = -pi - maxval = pi - else: - try: - limit = child.getElementsByTagName('limit')[0] - minval = float(limit.getAttribute('lower')) - maxval = float(limit.getAttribute('upper')) - except: - rospy.logwarn("%s is not fixed, nor continuous, but limits are not specified!" % name) - continue - - safety_tags = child.getElementsByTagName('safety_controller') - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute('soft_lower_limit'): - minval = max(minval, float(tag.getAttribute('soft_lower_limit'))) - if tag.hasAttribute('soft_upper_limit'): - maxval = min(maxval, float(tag.getAttribute('soft_upper_limit'))) - - 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 - - if self.zeros and name in self.zeros: - zeroval = self.zeros[name] - elif minval > 0 or maxval < 0: - zeroval = (maxval + minval)/2 - else: - zeroval = 0 - - 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 - - use_gui = get_param("use_gui", False) - - if use_gui: - num_rows = get_param("num_rows", 0) - self.app = QApplication(sys.argv) - self.gui = JointStatePublisherGui("Joint State Publisher", self, num_rows) - self.gui.show() - else: - self.gui = None - - source_list = get_param("source_list", []) - self.sources = [] - for source in source_list: - self.sources.append(rospy.Subscriber(source, JointState, self.source_cb)) - - self.pub = rospy.Publisher('joint_states', JointState, queue_size=5) - - def source_cb(self, msg): - for i in range(len(msg.name)): - name = msg.name[i] - 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: - # signal instead of directly calling the update_sliders method, to switch to the QThread - self.gui.sliderUpdateTrigger.emit() - - def loop(self): - 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() - - 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(): - 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)) - 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'] - factor = param.get('factor', 1) - offset = param.get('offset', 0) - # Handle recursive mimic chain - recursive_mimic_chain_joints = [name] - while parent in self.dependent_joints: - if parent in recursive_mimic_chain_joints: - error_message = "Found an infinite recursive mimic chain" - rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent) - sys.exit(-1) - recursive_mimic_chain_joints.append(parent) - param = self.dependent_joints[parent] - parent = param['parent'] - offset += factor * param.get('offset', 0) - factor *= param.get('factor', 1) - joint = self.free_joints[parent] - - 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'] - - if msg.name or msg.position or msg.velocity or msg.effort: - # Only publish non-empty messages - self.pub.publish(msg) - try: - r.sleep() - except rospy.exceptions.ROSTimeMovedBackwardsException: - pass - - 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(QWidget): - sliderUpdateTrigger = Signal() - - def __init__(self, title, jsp, num_rows=0): - super(JointStatePublisherGui, self).__init__() - self.jsp = jsp - self.joint_map = {} - self.vlayout = QVBoxLayout(self) - self.gridlayout = QGridLayout() - font = QFont("Helvetica", 9, QFont.Bold) - - ### Generate sliders ### - 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']: - continue - - joint_layout = QVBoxLayout() - row_layout = QHBoxLayout() - - label = QLabel(name) - label.setFont(font) - row_layout.addWidget(label) - display = QLineEdit("0.00") - display.setAlignment(Qt.AlignRight) - display.setFont(font) - display.setReadOnly(True) - row_layout.addWidget(display) - - joint_layout.addLayout(row_layout) - - slider = QSlider(Qt.Horizontal) - - slider.setFont(font) - slider.setRange(0, RANGE) - slider.setValue(RANGE/2) - - joint_layout.addWidget(slider) - - self.joint_map[name] = {'slidervalue': 0, 'display': display, - 'slider': slider, 'joint': joint} - # Connect to the signal provided by QSignal - slider.valueChanged.connect(self.onValueChanged) - sliders.append(joint_layout) - - # Determine number of rows to be used in grid - self.num_rows = num_rows - # if desired num of rows wasn't set, default behaviour is a vertical layout - if self.num_rows == 0: - self.num_rows = len(sliders) # equals VBoxLayout - # Generate positions in grid and place sliders there - self.positions = self.generate_grid_positions(len(sliders), self.num_rows) - for item, pos in zip(sliders, self.positions): - self.gridlayout.addLayout(item, *pos) - - # Set zero positions read from parameters - self.center() - - # Synchronize slider and displayed value - self.sliderUpdate(None) - - # Set up a signal for updating the sliders based on external joint info - self.sliderUpdateTrigger.connect(self.updateSliders) - - self.vlayout.addLayout(self.gridlayout) - - # Buttons for randomizing and centering sliders and - # Spinbox for on-the-fly selecting number of rows - self.randbutton = QPushButton('Randomize', self) - self.randbutton.clicked.connect(self.randomize_event) - self.vlayout.addWidget(self.randbutton) - self.ctrbutton = QPushButton('Center', self) - self.ctrbutton.clicked.connect(self.center_event) - self.vlayout.addWidget(self.ctrbutton) - self.maxrowsupdown = QSpinBox() - self.maxrowsupdown.setMinimum(1) - self.maxrowsupdown.setMaximum(len(sliders)) - self.maxrowsupdown.setValue(self.num_rows) - self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window - self.maxrowsupdown.valueChanged.connect(self.reorggrid_event) - self.vlayout.addWidget(self.maxrowsupdown) - - @pyqtSlot(int) - def onValueChanged(self, event): - # A slider value was changed, but we need to change the joint_info metadata. - for name, joint_info in self.joint_map.items(): - joint_info['slidervalue'] = joint_info['slider'].value() - joint = joint_info['joint'] - joint['position'] = self.sliderToValue(joint_info['slidervalue'], joint) - joint_info['display'].setText("%.2f" % joint['position']) - - @pyqtSlot() - def updateSliders(self): - 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['position'], - joint) - joint_info['slider'].setValue(joint_info['slidervalue']) - - 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['slider'].setValue(self.valueToSlider(joint['zero'], joint)) - - def reorggrid_event(self, event): - self.reorganize_grid(event) - - def reorganize_grid(self, number_of_rows): - self.num_rows = number_of_rows - - # Remove items from layout (won't destroy them!) - items = [] - for pos in self.positions: - item = self.gridlayout.itemAtPosition(*pos) - items.append(item) - self.gridlayout.removeItem(item) - - # Generate new positions for sliders and place them in their new spots - self.positions = self.generate_grid_positions(len(items), self.num_rows) - for item, pos in zip(items, self.positions): - self.gridlayout.addLayout(item, *pos) - - def generate_grid_positions(self, num_items, num_rows): - if num_rows==0: - return [] - positions = [(y, x) for x in range(int((math.ceil(float(num_items) / num_rows)))) for y in range(num_rows)] - positions = positions[:num_items] - return positions - - def randomize_event(self, event): - self.randomize() - - def randomize(self): - rospy.loginfo("Randomizing") - for name, joint_info in self.joint_map.items(): - joint = joint_info['joint'] - joint_info['slider'].setValue( - self.valueToSlider(random.uniform(joint['min'], joint['max']), joint)) - - def sliderUpdate(self, event): - for name, joint_info in self.joint_map.items(): - joint_info['slidervalue'] = joint_info['slider'].value() - self.update_sliders() - - 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() - - if jsp.gui is None: - jsp.loop() - else: - Thread(target=jsp.loop).start() - signal.signal(signal.SIGINT, signal.SIG_DFL) - sys.exit(jsp.app.exec_()) - - except rospy.ROSInterruptException: - pass diff --git a/joint_state_publisher/package.xml b/joint_state_publisher/package.xml deleted file mode 100644 index 6e3bb1e3..00000000 --- a/joint_state_publisher/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - joint_state_publisher - 1.12.11 - - This package contains a tool for setting and publishing joint state values for a given URDF. - - David V. Lu!! - Jackie Kay - Chris Lalancette - Shane Loretz - - BSD - http://www.ros.org/wiki/joint_state_publisher - - catkin - - rospy - python_qt_binding - sensor_msgs - - rostest - diff --git a/joint_state_publisher/screenshot.png b/joint_state_publisher/screenshot.png deleted file mode 100644 index cb8a6079..00000000 Binary files a/joint_state_publisher/screenshot.png and /dev/null differ diff --git a/joint_state_publisher/test/mimic_chain.urdf b/joint_state_publisher/test/mimic_chain.urdf deleted file mode 100644 index 9cf0c8ba..00000000 --- a/joint_state_publisher/test/mimic_chain.urdf +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/joint_state_publisher/test/mimic_cycle.urdf b/joint_state_publisher/test/mimic_cycle.urdf deleted file mode 100644 index 233a7c56..00000000 --- a/joint_state_publisher/test/mimic_cycle.urdf +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/joint_state_publisher/test/test_mimic_chain.launch b/joint_state_publisher/test/test_mimic_chain.launch deleted file mode 100644 index 385edd0b..00000000 --- a/joint_state_publisher/test/test_mimic_chain.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/joint_state_publisher/test/test_mimic_cycle.launch b/joint_state_publisher/test/test_mimic_cycle.launch deleted file mode 100644 index cfec4c35..00000000 --- a/joint_state_publisher/test/test_mimic_cycle.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/joint_state_publisher/test/test_zero_joints.launch b/joint_state_publisher/test/test_zero_joints.launch deleted file mode 100644 index 481ab829..00000000 --- a/joint_state_publisher/test/test_zero_joints.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/joint_state_publisher/test/zero_joint_robot.urdf b/joint_state_publisher/test/zero_joint_robot.urdf deleted file mode 100644 index 1e94aaa4..00000000 --- a/joint_state_publisher/test/zero_joint_robot.urdf +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - -