diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher/joint_state_publisher index ecd1f1d..2318459 100755 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher @@ -3,32 +3,15 @@ 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 QScrollArea -from python_qt_binding.QtWidgets import QSpinBox -from python_qt_binding.QtWidgets import QWidget - import xml.dom.minidom +from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure from sensor_msgs.msg import JointState from math import pi -from threading import Thread +from threading import Lock import sys import signal import math -RANGE = 10000 - def get_param(name, value=None): private = "~%s" % name @@ -134,6 +117,8 @@ class JointStatePublisher(): def __init__(self): description = get_param('robot_description') + self.mutex = Lock() + self.free_joints = {} self.joint_list = [] # for maintaining the original order of the joints self.dependent_joints = get_param("dependent_joints", {}) @@ -152,15 +137,7 @@ class JointStatePublisher(): else: self.init_urdf(robot) - 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 + self.init_ddr() source_list = get_param("source_list", []) self.sources = [] @@ -170,6 +147,8 @@ class JointStatePublisher(): self.pub = rospy.Publisher('joint_states', JointState, queue_size=5) def source_cb(self, msg): + self.mutex.acquire() + self.new_config = {} for i in range(len(msg.name)): name = msg.name[i] if name not in self.free_joints: @@ -191,14 +170,12 @@ class JointStatePublisher(): joint = self.free_joints[name] if position is not None: joint['position'] = position + self.new_config[name] = 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() + self.mutex.release() def loop(self): hz = get_param("rate", 10) # 10hz @@ -273,6 +250,13 @@ class JointStatePublisher(): if msg.name or msg.position or msg.velocity or msg.effort: # Only publish non-empty messages self.pub.publish(msg) + + self.mutex.acquire() + if self.new_config: + self.ddynrec.dyn_rec_srv.update_configuration(self.new_config) + self.new_config = None + self.mutex.release() + try: r.sleep() except rospy.exceptions.ROSTimeMovedBackwardsException: @@ -296,189 +280,74 @@ class JointStatePublisher(): 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.scrollable = QWidget() - self.gridlayout = QGridLayout() - self.scroll = QScrollArea() - self.scroll.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOn) - self.scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff) - self.scroll.setWidgetResizable(True) + def init_ddr(self): + self.ddynrec = DDynamicReconfigure("joint_state") - font = QFont("Helvetica", 9, QFont.Bold) - - ### Generate sliders ### - sliders = [] - for name in self.jsp.joint_list: - if name not in self.jsp.free_joints: + for name in self.joint_list: + if name not in self.free_joints: continue - joint = self.jsp.free_joints[name] + joint = self.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) + self.ddynrec.add_variable(name, "", float(joint['zero']), + float(joint['min']), float(joint['max'])) + # there cannot be a joint named 'center' or 'randomize' + self.ddynrec.add_variable("center", "Set all joints to zero positions", False) # 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.scrollable.setLayout(self.gridlayout) - self.scroll.setWidget(self.scrollable) - self.vlayout.addWidget(self.scroll) - - # 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) - self.setLayout(self.vlayout) - - @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)) + # self.center() - def sliderUpdate(self, event): - for name, joint_info in self.joint_map.items(): - joint_info['slidervalue'] = joint_info['slider'].value() - self.update_sliders() + self.ddynrec.add_variable("randomize", "Set all joints to random positions", False) - def valueToSlider(self, value, joint): - return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min']) + self.config = None + self.mutex.acquire() + self.new_config = None + self.mutex.release() + self.ddynrec.start(self.dyn_rec_callback) - def sliderToValue(self, slider, joint): - pctvalue = slider / float(RANGE) - return joint['min'] + (joint['max']-joint['min']) * pctvalue + def dyn_rec_callback(self, config, level): + for name in self.free_joints.keys(): + if name not in config.keys(): + rospy.logerr(name + " not in config: " + str(config.keys())) + continue + self.free_joints[name]['position'] = config[name] + if config.center: + config = self.center(config) + config.center = False + if config.randomize: + config = self.randomize(config) + config.randomize = False + + self.config = config + return config + + def update_config(self): + config = {} + for name in self.free_joints.keys(): + config[name] = self.free_joints[name]['position'] + self.ddynrec.dyn_rec_srv.update_configuration(config) + + def center(self, config): + rospy.loginfo("Centering") + for name in self.free_joints.keys(): + if name in config.keys(): + config[name] = self.free_joints[name]['zero'] + return config + def randomize(self, config): + rospy.loginfo("Randomizing") + for name in self.free_joints.keys(): + if name in config.keys(): + config[name] = random.uniform(self.free_joints[name]['min'], + self.free_joints[name]['max']) + return config 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_()) - + jsp.loop() except rospy.ROSInterruptException: pass diff --git a/joint_state_publisher/package.xml b/joint_state_publisher/package.xml index dd19d71..2e1708f 100644 --- a/joint_state_publisher/package.xml +++ b/joint_state_publisher/package.xml @@ -17,7 +17,7 @@ catkin rospy - python_qt_binding + ddynamic_reconfigure_python sensor_msgs rostest