Skip to content

Commit

Permalink
Replace standalone qt with dynamic reconfigure, no more use_gui param…
Browse files Browse the repository at this point in the history 
… (maybe should get it and then output a warning message if it exists?). TODO add tests ros#12
  • Loading branch information
@lucasw
lucasw committed Aug 5, 2018
1 parent dc9205b commit 99c1973
Show file tree
Hide file tree
Showing 2 changed files with 69 additions and 200 deletions.
267 changes: 68 additions & 199 deletions joint_state_publisher/joint_state_publisher/joint_state_publisher
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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", {})
Expand All @@ -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 = []
Expand All @@ -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:
Expand All @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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
2 changes: 1 addition & 1 deletion joint_state_publisher/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<buildtool_depend>catkin</buildtool_depend>

<exec_depend>rospy</exec_depend>
<exec_depend>python_qt_binding</exec_depend>
<exec_depend>ddynamic_reconfigure_python</exec_depend>
<exec_depend>sensor_msgs</exec_depend>

<test_depend>rostest</test_depend>
Expand Down

0 comments on commit 99c1973

Please sign in to comment.