bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Migrate qt (#128) 

* Update JointStatePublisher to qt5

* Implement joint states

* Move comment

* Update joint_state_publisher

* Fix randomization problem
This commit is contained in:
Jackie Kay 2016-04-12 14:59:29 -07:00
parent bd8490c6c7
commit 8c6cf9841c
2 changed files with 83 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
joint_state_publisher/joint_state_publisher/joint_state_publisher
View File

@ -2,15 +2,28 @@
import rospy import rospy
import random import random
import wx
import wx.lib.newevent 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 QWidget
import xml.dom.minidom import xml.dom.minidom
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
from math import pi from math import pi
from threading import Thread from threading import Thread
from sys import argv
RANGE = 10000 RANGE = 10000
def get_param(name, value=None): def get_param(name, value=None):
private = "~%s" % name private = "~%s" % name
if rospy.has_param(private): if rospy.has_param(private):
@ -20,12 +33,13 @@ def get_param(name, value=None):
else: else:
return value return value
class JointStatePublisher(): class JointStatePublisher():
def __init__(self): def __init__(self):
description = get_param('robot_description') description = get_param('robot_description')
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0] robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
self.free_joints = {} self.free_joints = {}
self.joint_list = [] # for maintaining the original order of the joints self.joint_list = [] # for maintaining the original order of the joints
self.dependent_joints = get_param("dependent_joints", {}) self.dependent_joints = get_param("dependent_joints", {})
use_mimic = get_param('use_mimic_tags', True) use_mimic = get_param('use_mimic_tags', True)
use_small = get_param('use_smallest_joint_limits', True) use_small = get_param('use_smallest_joint_limits', True)
@ -59,7 +73,7 @@ class JointStatePublisher():
continue continue
safety_tags = child.getElementsByTagName('safety_controller') safety_tags = child.getElementsByTagName('safety_controller')
if use_small and len(safety_tags)==1: if use_small and len(safety_tags) == 1:
tag = safety_tags[0] tag = safety_tags[0]
if tag.hasAttribute('soft_lower_limit'): if tag.hasAttribute('soft_lower_limit'):
minval = max(minval, float(tag.getAttribute('soft_lower_limit'))) minval = max(minval, float(tag.getAttribute('soft_lower_limit')))
@ -67,7 +81,7 @@ class JointStatePublisher():
maxval = min(maxval, float(tag.getAttribute('soft_upper_limit'))) maxval = min(maxval, float(tag.getAttribute('soft_upper_limit')))
mimic_tags = child.getElementsByTagName('mimic') mimic_tags = child.getElementsByTagName('mimic')
if use_mimic and len(mimic_tags)==1: if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0] tag = mimic_tags[0]
entry = {'parent': tag.getAttribute('joint')} entry = {'parent': tag.getAttribute('joint')}
if tag.hasAttribute('multiplier'): if tag.hasAttribute('multiplier'):
@ -88,7 +102,7 @@ class JointStatePublisher():
else: else:
zeroval = 0 zeroval = 0
joint = {'min':minval, 'max':maxval, 'zero':zeroval} joint = {'min': minval, 'max': maxval, 'zero': zeroval}
if pub_def_positions: if pub_def_positions:
joint['position'] = zeroval joint['position'] = zeroval
if pub_def_vels: if pub_def_vels:
@ -100,13 +114,12 @@ class JointStatePublisher():
joint['continuous'] = True joint['continuous'] = True
self.free_joints[name] = joint self.free_joints[name] = joint
use_gui = get_param("use_gui", False) use_gui = get_param("use_gui", False)
if use_gui: if use_gui:
self.app = wx.App() self.app = QApplication(argv)
self.gui = JointStatePublisherGui("Joint State Publisher", self) self.gui = JointStatePublisherGui("Joint State Publisher", self)
self.gui.Show() self.gui.show()
else: else:
self.gui = None self.gui = None
@ -145,12 +158,11 @@ class JointStatePublisher():
joint['effort'] = effort joint['effort'] = effort
if self.gui is not None: if self.gui is not None:
# post an event here instead of directly calling the update_sliders method, to switch to the wx thread # signal instead of directly calling the update_sliders method, to switch to the QThread
wx.PostEvent(self.gui.GetEventHandler(), self.gui.UpdateSlidersEvent()) self.gui.sliderUpdateTrigger.emit()
def loop(self): def loop(self):
hz = get_param("rate", 10) # 10hz hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz) r = rospy.Rate(hz)
delta = get_param("delta", 0.0) delta = get_param("delta", 0.0)
@ -167,7 +179,7 @@ class JointStatePublisher():
has_position = len(self.dependent_joints.items()) > 0 has_position = len(self.dependent_joints.items()) > 0
has_velocity = False has_velocity = False
has_effort = False has_effort = False
for (name,joint) in self.free_joints.items(): for name, joint in self.free_joints.items():
if not has_position and 'position' in joint: if not has_position and 'position' in joint:
has_position = True has_position = True
if not has_velocity and 'velocity' in joint: if not has_velocity and 'velocity' in joint:
@ -183,7 +195,6 @@ class JointStatePublisher():
if has_effort: if has_effort:
msg.effort = num_joints * [0.0] msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list): for i, name in enumerate(self.joint_list):
msg.name.append(str(name)) msg.name.append(str(name))
joint = None joint = None
@ -228,14 +239,16 @@ class JointStatePublisher():
joint['position'] = joint['min'] joint['position'] = joint['min']
joint['forward'] = not forward joint['forward'] = not forward
class JointStatePublisherGui(wx.Frame):
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp): def __init__(self, title, jsp):
wx.Frame.__init__(self, None, -1, title, (-1, -1)); super(JointStatePublisherGui, self).__init__()
self.jsp = jsp self.jsp = jsp
self.joint_map = {} self.joint_map = {}
panel = wx.Panel(self, wx.ID_ANY); box_layout = QVBoxLayout(self)
box = wx.BoxSizer(wx.VERTICAL) font = QFont("Helvetica", 9, QFont.Bold)
font = wx.Font(9, wx.SWISS, wx.NORMAL, wx.BOLD)
### Sliders ### ### Sliders ###
for name in self.jsp.joint_list: for name in self.jsp.joint_list:
@ -246,46 +259,51 @@ class JointStatePublisherGui(wx.Frame):
if joint['min'] == joint['max']: if joint['min'] == joint['max']:
continue continue
row = wx.GridSizer(1,2) joint_layout = QVBoxLayout()
label = wx.StaticText(panel, -1, name) row_layout = QHBoxLayout()
label.SetFont(font)
row.Add(label, 1, wx.ALIGN_CENTER_VERTICAL)
display = wx.TextCtrl (panel, value=str(0), label = QLabel(name)
style=wx.TE_READONLY | wx.ALIGN_RIGHT) 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)
row.Add(display, flag= wx.ALIGN_RIGHT| wx.ALIGN_CENTER_VERTICAL) joint_layout.addLayout(row_layout)
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 = QSlider(Qt.Horizontal)
'slider':slider, 'joint':joint}
self.UpdateSlidersEvent, self.EVT_UPDATESLIDERS = wx.lib.newevent.NewEvent() slider.setFont(font)
self.Bind(self.EVT_UPDATESLIDERS, self.updateSliders) slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
box_layout.addLayout(joint_layout)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.sliderMoved.connect(self.sliderUpdate)
# 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)
### Buttons ### ### Buttons ###
self.randbutton = wx.Button(panel, 1, 'Randomize') self.randbutton = QPushButton('Randomize', self)
self.ctrbutton = wx.Button(panel, 2, 'Center') self.randbutton.clicked.connect(self.randomize_event)
self.Bind(wx.EVT_SLIDER, self.sliderUpdate) box_layout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
wx.EVT_BUTTON(self, 1, self.randomize_event) self.ctrbutton.clicked.connect(self.center_event)
wx.EVT_BUTTON(self, 2, self.center_event) box_layout.addWidget(self.ctrbutton)
box.Add(self.randbutton, 0, wx.EXPAND)
box.Add(self.ctrbutton, 1, wx.EXPAND)
panel.SetSizer(box)
self.center()
box.Fit(self)
self.update_values()
def update_values(self): def update_values(self):
for (name,joint_info) in self.joint_map.items(): for name, joint_info in self.joint_map.items():
purevalue = joint_info['slidervalue'] purevalue = joint_info['slidervalue']
joint = joint_info['joint'] joint = joint_info['joint']
value = self.sliderToValue(purevalue, joint) value = self.sliderToValue(purevalue, joint)
@ -296,19 +314,20 @@ class JointStatePublisherGui(wx.Frame):
self.update_sliders() self.update_sliders()
def update_sliders(self): def update_sliders(self):
for (name,joint_info) in self.joint_map.items(): for name, joint_info in self.joint_map.items():
joint = joint_info['joint'] joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['position'], joint_info['slidervalue'] = self.valueToSlider(joint['position'],
joint) joint)
joint_info['slider'].SetValue(joint_info['slidervalue']) joint_info['slider'].setValue(joint_info['slidervalue'])
joint_info['display'].SetValue("%.2f"%joint['position']) joint_info['display'].setText("%.2f" % joint['position'])
def center_event(self, event): def center_event(self, event):
self.center() self.center()
def center(self): def center(self):
rospy.loginfo("Centering") rospy.loginfo("Centering")
for (name,joint_info) in self.joint_map.items(): for name, joint_info in self.joint_map.items():
joint = joint_info['joint'] joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint) joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint)
self.update_values() self.update_values()
@ -318,15 +337,14 @@ class JointStatePublisherGui(wx.Frame):
def randomize(self): def randomize(self):
rospy.loginfo("Randomizing") rospy.loginfo("Randomizing")
for (name,joint_info) in self.joint_map.items(): for name, joint_info in self.joint_map.items():
joint = joint_info['joint'] joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(random.uniform(joint['min'], joint['max']), joint) joint_info['slidervalue'] = self.valueToSlider(random.uniform(joint['min'], joint['max']), joint)
self.update_values() self.update_values()
def sliderUpdate(self, event): def sliderUpdate(self, event):
for (name,joint_info) in self.joint_map.items(): for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].GetValue() joint_info['slidervalue'] = joint_info['slider'].value()
self.update_values() self.update_values()
def valueToSlider(self, value, joint): def valueToSlider(self, value, joint):
@ -346,6 +364,7 @@ if __name__ == '__main__':
jsp.loop() jsp.loop()
else: else:
Thread(target=jsp.loop).start() Thread(target=jsp.loop).start()
jsp.app.MainLoop() jsp.app.exec_()
except rospy.ROSInterruptException: pass except rospy.ROSInterruptException:
pass

4
joint_state_publisher/package.xml
View File

@ -14,11 +14,11 @@
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>wxpython</build_depend> <build_depend>python_qt_binding</build_depend>
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<run_depend>rospy</run_depend> <run_depend>rospy</run_depend>
<run_depend>wxpython</run_depend> <run_depend>python_qt_binding</run_depend>
<run_depend>sensor_msgs</run_depend> <run_depend>sensor_msgs</run_depend>
</package> </package>