def __init__(self,
options,
title='Exclusive Options',
selected_index=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
button_id = 0
for option in self._options:
button_id += 1
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False)
or (button_id - 1) == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
widget = QWidget()
widget.setLayout(QVBoxLayout())
widget.layout().addWidget(radio_button)
if 'description' in option:
widget.layout().addWidget(QLabel(option['description']))
self._button_group.addButton(radio_button, button_id)
self.layout().addWidget(widget)
def restore_settings(self, settings):
serial_number = settings.value('parent', None)
#print 'DockWidget.restore_settings()', 'parent', serial_number, 'settings group', settings._group
if serial_number is not None:
serial_number = int(serial_number)
if self._parent_container_serial_number() != serial_number and self._container_manager is not None:
floating = self.isFloating()
pos = self.pos()
new_container = self._container_manager.get_container(serial_number)
if new_container is not None:
new_parent = new_container.main_window
else:
new_parent = self._container_manager.get_root_main_window()
area = self.parent().dockWidgetArea(self)
new_parent.addDockWidget(area, self)
if floating:
self.setFloating(floating)
self.move(pos)
title_bar = self.titleBarWidget()
title_bar.restore_settings(settings)
if title_bar.hide_title_bar and not self.features() & DockWidget.DockWidgetFloatable and \
not self.features() & DockWidget.DockWidgetMovable:
self._title_bar_backup = title_bar
title_widget = QWidget(self)
layout = QHBoxLayout(title_widget)
layout.setContentsMargins(0, 0, 0, 0)
title_widget.setLayout(layout)
self.setTitleBarWidget(title_widget)
class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + \
(' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
def restore_settings(self, settings):
serial_number = settings.value('parent', None)
#print 'DockWidget.restore_settings()', 'parent', serial_number, 'settings group', settings._group
if serial_number is not None:
serial_number = int(serial_number)
if self._parent_container_serial_number(
) != serial_number and self._container_manager is not None:
floating = self.isFloating()
pos = self.pos()
new_container = self._container_manager.get_container(
serial_number)
if new_container is not None:
new_parent = new_container.main_window
else:
new_parent = self._container_manager.get_root_main_window()
area = self.parent().dockWidgetArea(self)
new_parent.addDockWidget(area, self)
if floating:
self.setFloating(floating)
self.move(pos)
title_bar = self.titleBarWidget()
title_bar.restore_settings(settings)
if title_bar.hide_title_bar and not self.features() & DockWidget.DockWidgetFloatable and \
not self.features() & DockWidget.DockWidgetMovable:
self._title_bar_backup = title_bar
title_widget = QWidget(self)
layout = QHBoxLayout(title_widget)
layout.setContentsMargins(0, 0, 0, 0)
title_widget.setLayout(layout)
self.setTitleBarWidget(title_widget)
class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
class NodeSelection(QWidget):
def __init__(self, parent):
super(NodeSelection, self).__init__()
self.parent_widget = parent
self.selected_nodes = []
self.setWindowTitle("Select the nodes you want to record")
self.resize(500, 700)
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Done", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.node_list = rosnode.get_node_names()
self.node_list.sort()
for node in self.node_list:
self.addCheckBox(node)
self.main_widget.setLayout(self.selection_vlayout)
self.show()
def addCheckBox(self, node):
item = QCheckBox(node, self)
item.stateChanged.connect(lambda x: self.updateNode(x, node))
self.selection_vlayout.addWidget(item)
def updateNode(self, state, node):
if state == Qt.Checked:
self.selected_nodes.append(node)
else:
self.selected_nodes.remove(node)
if len(self.selected_nodes) > 0:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
def onButtonClicked(self):
master = rosgraph.Master('rqt_bag_recorder')
state = master.getSystemState()
subs = [
t for t, l in state[1] if len([
node_name
for node_name in self.selected_nodes if node_name in l
]) > 0
]
for topic in subs:
self.parent_widget.changeTopicCheckState(topic, Qt.Checked)
self.parent_widget.updateList(Qt.Checked, topic)
self.close()
class TriangleGUI(Plugin):
def __init__(self, context):
super(TriangleGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TriangleGUI')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
self._toolbar = QToolBar()
self._toolbar.addWidget(QLabel('Triangle'))
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Add a button for killing nodes
self._go_button = QPushButton('Go')
self._go_button.clicked.connect(self._go)
self._layout.addWidget(self._go_button)
self._clear_button = QPushButton('Clear')
self._clear_button.clicked.connect(self._clear)
self._layout.addWidget(self._clear_button)
# self._step_run_button.setStyleSheet('QPushButton {color: black}')
context.add_widget(self._container)
def _go(self):
go = rospy.ServiceProxy('/triangle_screenpoint/go', Empty)
go()
def _clear(self):
clear = rospy.ServiceProxy('/triangle_screenpoint/cancel', Empty)
clear()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class PingGUI(Plugin):
def __init__(self, context):
super(PingGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PingGUI')
self.msg = None
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._label = QLabel("xx ms latency")
p = self._label.palette()
p.setColor(self._label.backgroundRole(), Qt.red)
self._label.setPalette(p)
self._layout.addWidget(self._label)
self.set_bg_color(100, 100, 100)
rospy.Subscriber("/ping/delay", Float64, self.ping_cb)
context.add_widget(self._container)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_gui)
self._update_plot_timer.start(1)
def update_gui(self):
if not self.msg:
return
msg = self.msg
# msec
# 100 -> green, 1000 -> red
# normalize within 1000 ~ 100
orig_latency = msg.data
msg_data = orig_latency
if msg.data > 1000:
msg_data = 1000
elif msg.data < 100:
msg_data = 100
ratio = (msg_data - 100) / (1000 - 100)
color_r = ratio * 255.0
color_g = (1 - ratio) * 255.0
# devnull = open(os.devnull, "w")
# with RedirectStdStreams(stdout=devnull, stderr=devnull):
self.set_bg_color(color_r, color_g, 0)
self._label.setText("%d ms latency" % (orig_latency))
def set_bg_color(self, r, g, b):
self._label.setStyleSheet("QLabel { display: block; background-color: rgba(%d, %d, %d, 255); text-align: center; font-size: 30px;}" % (r, g, b))
def ping_cb(self, msg):
self.msg = msg
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def process(self, action):
"""
:param action: action to execute, ''QAction''
:raises: when it doesn't recognice the action passed in, ''Exception''
"""
if action == self._reset_timeline:
self.timeline._timeline_frame.reset_timeline()
elif action == self._play_all:
self.timeline.toggle_play_all()
elif action == self._publish_all:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.start_publishing(topic):
break
elif action == self._publish_none:
for topic in self.timeline._timeline_frame.topics:
if not self.timeline.stop_publishing(topic):
break
elif action == self._thumbnail_show_action:
self.timeline._timeline_frame.set_renderers_active(True)
elif action == self._thumbnail_hide_action:
self.timeline._timeline_frame.set_renderers_active(False)
elif action in self._thumbnail_actions:
if self.timeline._timeline_frame.is_renderer_active(action.text()):
self.timeline._timeline_frame.set_renderer_active(
action.text(), False)
else:
self.timeline._timeline_frame.set_renderer_active(
action.text(), True)
elif action in self._topic_actions + self._type_actions:
popup_name = action.parentWidget().title() + '__' + action.text()
if popup_name not in self.timeline.popups:
frame = QWidget()
layout = QVBoxLayout()
frame.setLayout(layout)
frame.resize(640, 480)
viewer_type = action.data()
frame.setObjectName(popup_name)
view = viewer_type(self.timeline, frame)
self.timeline.popups.add(popup_name)
self.timeline.get_context().add_widget(frame)
self.timeline.add_view(action.parentWidget().title(), view,
frame)
frame.show()
elif action in self._publish_actions:
if self.timeline.is_publishing(action.text()):
self.timeline.stop_publishing(action.text())
else:
self.timeline.start_publishing(action.text())
else:
raise Exception('Unknown action in TimelinePopupMenu.process')
class GhostRobotControlPlugin(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(GhostRobotControlPlugin, self).__init__(context)
self.setObjectName('GhostRobotControlPlugin')
self.joint_states_to_ghost_pub = rospy.Publisher('/joint_states_to_ghost', JointState, queue_size=10)
self.ghost_joint_states_sub = rospy.Subscriber('/ghost/joint_states', JointState, self.ghost_joint_states_cb)
self.real_joint_states_sub = rospy.Subscriber('/atlas/joint_states', JointState, self.real_joint_states_cb)
self.ghost_joint_states = JointState()
self.real_joint_states = JointState()
self.move_real_robot = False
self.widget = QWidget()
vbox = QVBoxLayout()
self.real_to_ghost_push_button = QPushButton('Set Ghost from real robot')
self.real_to_ghost_push_button.clicked.connect(self.handle_set_real_to_ghost)
vbox.addWidget(self.real_to_ghost_push_button)
self.send_motion_plan_to_real_robot_check_box = QCheckBox('Motion GUI moves real robot')
self.send_motion_plan_to_real_robot_check_box.stateChanged.connect(self.handle_send_motion_plan_to_real_robot_check_box)
vbox.addWidget(self.send_motion_plan_to_real_robot_check_box)
self.widget.setLayout(vbox)
context.add_widget(self.widget)
def ghost_joint_states_cb(self, data):
self.ghost_joint_states = data
def real_joint_states_cb(self, data):
self.real_joint_states = data
def handle_set_real_to_ghost(self):
self.joint_states_to_ghost_pub.publish(self.real_joint_states)
@Slot(bool)
def handle_send_motion_plan_to_real_robot_check_box(self, checked):
self.move_real_robot = checked
class MotionEditorPlugin(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(MotionEditorPlugin, self).__init__(context)
self.setObjectName('MotionEditorPlugin')
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param('/motion_editor/robot_config')
except KeyError:
rospy.logwarn(
'Could not find robot config param in /motion_editor/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
motion_publishers = {}
for target in config_loader.targets:
motion_publishers[target.name] = MotionPublisher(
config_loader.robot_config, target.joint_state_topic,
target.publisher_prefix)
input_output_selector = InputOutputSelectorWidget(motion_publishers)
self._motion_editor = MotionEditorWidget(input_output_selector,
config_loader.robot_config)
position_editor = PositionEditorWidget(input_output_selector,
config_loader.robot_config)
position_editor.position_list_updated.connect(
self._motion_editor.update_positions_lists)
position_editor.position_list_updated.emit(
position_editor._position_data)
self._motion_editor.position_renamed.connect(
position_editor.on_position_renamed)
layout = QVBoxLayout()
layout.addWidget(input_output_selector)
layout.addWidget(position_editor)
layout.addWidget(self._motion_editor)
self._widget = QWidget()
self._widget.setLayout(layout)
context.add_widget(self._widget)
def save_settings(self, plugin_settings, instance_settings):
self._motion_editor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._motion_editor.restore_settings(plugin_settings,
instance_settings)
class TriangleGUI(Plugin):
def __init__(self, context):
super(TriangleGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TriangleGUI')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
self._toolbar = QToolBar()
self._toolbar.addWidget(QLabel('Triangle'))
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Add a button for killing nodes
self._go_button = QPushButton('Go')
self._go_button.clicked.connect(self._go)
self._layout.addWidget(self._go_button)
self._clear_button = QPushButton('Clear')
self._clear_button.clicked.connect(self._clear)
self._layout.addWidget(self._clear_button)
# self._step_run_button.setStyleSheet('QPushButton {color: black}')
context.add_widget(self._container)
def _go(self):
go = rospy.ServiceProxy('/triangle_screenpoint/go', Empty)
go()
def _clear(self):
clear = rospy.ServiceProxy('/triangle_screenpoint/cancel', Empty)
clear()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def __init__(self, context):
super(BiasCalibrationDialog, self).__init__(context)
self.setObjectName('BiasCalibrationDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
controller_widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointBias(self, roslib.packages.get_pkg_dir('vigir_rqt_bias_calibration') + '/launch/joints.txt',hbox)
controller_widget.setLayout(hbox)
self.marker = Marker()
self.marker.header.frame_id = "/world"
self.marker.type = self.marker.CUBE
self.marker.action = self.marker.ADD
self.marker.scale.x = 14.0
self.marker.scale.y = 14.0
self.marker.scale.z = 0.02
self.marker.color.a = 0.25
self.marker.color.r = 1.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
#self.marker.points = [[0.0, 0.0, 0.0], [7.0, -4.0, 0.0], [7.0, 4.0, 0.0]]
self.marker.pose.orientation.w = 1.0
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 0.0
self.bias_pub = rospy.Publisher('/flor/controller/atlas_biases', JointState, queue_size=10)
self.flor_marker_pub = rospy.Publisher('/flor_plane_marker', Marker, queue_size=10)
self.feet_state_sub = rospy.Subscriber('/atlas/atlas_sim_interface_state', AtlasSimInterfaceState, self.stateCallbackFnc)
vbox.addWidget(controller_widget)
self.save_button = QPushButton("Save Biases")
self.save_button.pressed.connect(self.on_savePressed)
vbox.addWidget(self.save_button)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def __init__(self, parent, fileName, top_widget_layout):
self.controllers = []
self.parent = parent
self.loadFile(fileName)
print "Initialize controllers..."
for controller in self.controllers:
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
vbox = QVBoxLayout()
label = QLabel()
label.setText(controller.label)
vbox.addWidget(label);
print controller.name
for joint in controller.joints:
label = QLabel()
label.setText(joint.name)
vbox.addWidget(label);
#Add input for setting the biases
widget = QWidget()
hbox = QHBoxLayout()
hbox.addWidget(joint.sensor_bias_spinbox)
hbox.addWidget(joint.control_bias_spinbox)
hbox.addWidget(joint.gearing_bias_spinbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
label = QLabel()
label.setText(" Sensor Control Gearing")
vbox.addWidget(label);
vbox.addStretch()
frame.setLayout(vbox)
top_widget_layout.addWidget(frame)
print "Done loading controllers"
class EusGUI(Plugin):
def __init__(self, context):
super(EusGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('EusGUI')
self.msg = None
# Create a container widget and give it a layout
self._toolbar = QToolBar()
self._toolbar.addWidget(QLabel('EusGUI'))
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
self._prev_button = QPushButton('PREV')
self._prev_button.clicked.connect(self._prev_cb)
self._layout.addWidget(self._prev_button)
self._refresh_button = QPushButton('DO IT AGAIN')
self._refresh_button.clicked.connect(self._refresh_cb)
self._layout.addWidget(self._refresh_button)
self._next_button = QPushButton('NEXT')
self._next_button.clicked.connect(self._next_cb)
self._layout.addWidget(self._next_button)
context.add_widget(self._container)
def _prev_cb(self):
func = rospy.ServiceProxy('prev', Empty)
func()
def _next_cb(self):
func = rospy.ServiceProxy('next', Empty)
func()
def _refresh_cb(self):
func = rospy.ServiceProxy('refresh', Empty)
func()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class MotionEditorPlugin(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(MotionEditorPlugin, self).__init__(context)
self.setObjectName('MotionEditorPlugin')
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param('/motion_editor/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /motion_editor/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
motion_publishers = {}
for target in config_loader.targets:
motion_publishers[target.name] = MotionPublisher(config_loader.robot_config, target.joint_state_topic, target.publisher_prefix)
input_output_selector = InputOutputSelectorWidget(motion_publishers)
self._motion_editor = MotionEditorWidget(input_output_selector, config_loader.robot_config)
position_editor = PositionEditorWidget(input_output_selector, config_loader.robot_config)
position_editor.position_list_updated.connect(self._motion_editor.update_positions_lists)
position_editor.position_list_updated.emit(position_editor._position_data)
self._motion_editor.position_renamed.connect(position_editor.on_position_renamed)
layout = QVBoxLayout()
layout.addWidget(input_output_selector)
layout.addWidget(position_editor)
layout.addWidget(self._motion_editor)
self._widget = QWidget()
self._widget.setLayout(layout)
context.add_widget(self._widget)
def save_settings(self, plugin_settings, instance_settings):
self._motion_editor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._motion_editor.restore_settings(plugin_settings, instance_settings)
class TabGroup(GroupWidget):
def __init__(self, parent, updater, config, nodename):
super(TabGroup, self).__init__(updater, config, nodename)
self.parent = parent
if not self.parent.tab_bar:
self.parent.tab_bar = QTabWidget()
self.wid = QWidget()
self.wid.setLayout(self.grid)
parent.tab_bar.addTab(self.wid, self.param_name)
def display(self, grid):
if not self.parent.tab_bar_shown:
grid.addRow(self.parent.tab_bar)
self.parent.tab_bar_shown = True
def close(self):
super(TabGroup, self).close()
self.parent.tab_bar = None
self.parent.tab_bar_shown = False
class WoZPlugin(Plugin):
def __init__(self, context):
super(WoZPlugin, self).__init__(context)
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addWidget(self.create_button('print nice', self.printCallback))
self._widget.setObjectName('TrashbotGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
def create_button(self, name, callback):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(callback)
btn.setAutoRepeat(True)
return btn
def printCallback(self):
print 'nice'
def __init__(self, context, node=None):
"""
This class is intended to be called by rqt plugin framework class.
Currently (12/12/2012) the whole widget is splitted into 2 panes:
one on left allows you to choose the node(s) you work on. Right side
pane lets you work with the parameters associated with the node(s) you
select on the left.
(12/27/2012) Despite the pkg name is changed to rqt_reconfigure to
reflect the available functionality, file & class names remain
'param', expecting all the parameters will become handle-able.
"""
super(ParamWidget, self).__init__()
self.setObjectName(self._TITLE_PLUGIN)
self.setWindowTitle(self._TITLE_PLUGIN)
rp = rospkg.RosPack()
#TODO: .ui file needs to replace the GUI components declaration
# below. For unknown reason, referring to another .ui files
# from a .ui that is used in this class failed. So for now,
# I decided not use .ui in this class.
# If someone can tackle this I'd appreciate.
_hlayout_top = QHBoxLayout(self)
self._splitter = QSplitter(self)
_hlayout_top.addWidget(self._splitter)
_vlayout_nodesel_widget = QWidget()
_vlayout_nodesel_side = QVBoxLayout()
_hlayout_filter_widget = QWidget(self)
_hlayout_filter = QHBoxLayout()
self._text_filter = TextFilter()
self.filter_lineedit = TextFilterWidget(self._text_filter, rp)
self.filterkey_label = QLabel("&Filter key:")
self.filterkey_label.setBuddy(self.filter_lineedit)
_hlayout_filter.addWidget(self.filterkey_label)
_hlayout_filter.addWidget(self.filter_lineedit)
_hlayout_filter_widget.setLayout(_hlayout_filter)
self._nodesel_widget = NodeSelectorWidget(self, rp, self.sig_sysmsg)
_vlayout_nodesel_side.addWidget(_hlayout_filter_widget)
_vlayout_nodesel_side.addWidget(self._nodesel_widget)
_vlayout_nodesel_side.setSpacing(1)
_vlayout_nodesel_widget.setLayout(_vlayout_nodesel_side)
reconf_widget = ParameditWidget(rp)
self._splitter.insertWidget(0, _vlayout_nodesel_widget)
self._splitter.insertWidget(1, reconf_widget)
# 1st column, _vlayout_nodesel_widget, to minimize width.
# 2nd col to keep the possible max width.
self._splitter.setStretchFactor(0, 0)
self._splitter.setStretchFactor(1, 1)
# Signal from paramedit widget to node selector widget.
reconf_widget.sig_node_disabled_selected.connect(
self._nodesel_widget.node_deselected)
# Pass name of node to editor widget
self._nodesel_widget.sig_node_selected.connect(
reconf_widget.show_reconf)
if not node:
title = self._TITLE_PLUGIN
else:
title = self._TITLE_PLUGIN + ' %s' % node
self.setObjectName(title)
#Connect filter signal-slots.
self._text_filter.filter_changed_signal.connect(
self._filter_key_changed)
class Milestone1GUI(Plugin):
RECEIVE_FROM_HUMAN_R_POS = [
0.00952670015493673, 0.3270780665526253, 0.03185028324084582,
-1.3968658009779173, -3.058799671876592, -1.1083678332942686,
-1.6314425515258866
]
READ_FIDUCIAL_R_POS = [
0.41004856860653505, 0.29772362823537935, -0.019944325676627628,
-1.8394298656773618, -0.44139252862458106, -0.09922194050427624,
-4.761735317011306
]
NAVIGATE_R_POS = [
-0.3594077470836499, 0.971353000916152, -1.9647276598906076,
-1.431900313132731, -1.1839177367219755, -0.09817772642188527,
-1.622044624784374
]
PLACE_ON_SHELF_R_POS = [
-0.15015144487461773, 0.4539704512093072, -0.10846018983280459,
-0.9819529421527269, -3.0207362886631235, -0.4990689162195487,
-1.6026396464199553
]
LOWER_ON_SHELF_R_POS = [
-0.2159792990560645, 0.6221451583409631, -0.1886376030177479,
-0.959223940465513, 9.690004126983537, -0.2866303982732683,
111.39703078836274
]
RELEASE_BOOK_R_POS = [
-0.15545746838546493, 0.44145040258984786, -0.13267376861465774,
-0.972108533778647, -3.028545645401449, -0.38572817936010495,
0.008017066746929924
]
TUCKED_UNDER_L_POS = [
0.05688828299939419, 1.2955758539090194, 1.7180547710154033,
-1.4511548177467404, -0.28718096455759035, -0.0938208188421713,
-10.980395466225648
]
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(Milestone1GUI, self).__init__(context)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
#init torso
self.torso = Torso()
#init gripper
self.l_gripper = Gripper('l')
self.r_gripper = Gripper('r')
#init navigation
self.navigation = Navigation()
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addItem(QtGui.QSpacerItem(15, 2))
box_1.addWidget(
self.create_button('Prepare To Take', self.prepare_to_take))
box_1.addItem(QtGui.QSpacerItem(445, 2))
box_2.addItem(QtGui.QSpacerItem(15, 2))
box_2.addWidget(
self.create_button('Take From Human', self.take_from_human))
box_2.addItem(QtGui.QSpacerItem(445, 2))
box_3.addItem(QtGui.QSpacerItem(15, 2))
box_3.addWidget(
self.create_button('Prepare To Navigate',
self.prepare_to_navigate))
box_3.addItem(QtGui.QSpacerItem(445, 2))
# Button to move to shelf
box_5.addItem(QtGui.QSpacerItem(15, 2))
box_5.addWidget(
self.create_button('Navigate To Shelf', self.navigate_to_shelf))
box_5.addItem(QtGui.QSpacerItem(445, 2))
box_4.addItem(QtGui.QSpacerItem(15, 2))
box_4.addWidget(
self.create_button('Place On Shelf', self.place_on_shelf))
box_4.addItem(QtGui.QSpacerItem(445, 2))
box_6.addItem(QtGui.QSpacerItem(15, 2))
box_6.addWidget(
self.create_button('Give Information', self.give_information))
box_6.addItem(QtGui.QSpacerItem(445, 2))
box_7.addItem(QtGui.QSpacerItem(15, 2))
box_7.addWidget(
self.create_button('Navigate To Person', self.navigate_to_person))
box_7.addItem(QtGui.QSpacerItem(445, 2))
self.book_textbox = QtGui.QLineEdit()
self.book_textbox.setFixedWidth(100)
box_8.addItem(QtGui.QSpacerItem(15, 2))
box_8.addWidget(self.book_textbox)
box_8.addWidget(
self.create_button('Pick Up Book', self.pick_up_from_shelf_button))
box_8.addItem(QtGui.QSpacerItem(445, 2))
box_9.addItem(QtGui.QSpacerItem(15, 2))
box_9.addWidget(self.create_button('Localize', self.spin_base))
box_9.addItem(QtGui.QSpacerItem(445, 2))
box_10.addItem(QtGui.QSpacerItem(15, 2))
box_10.addWidget(
self.create_button('Non-nav Pick Up', self.pick_up_from_shelf))
box_10.addItem(QtGui.QSpacerItem(445, 2))
button_box.addItem(QtGui.QSpacerItem(20, 120))
button_box.addLayout(box_1)
button_box.addLayout(box_2)
button_box.addLayout(box_3)
button_box.addLayout(box_5)
button_box.addLayout(box_4)
button_box.addLayout(box_6)
button_box.addLayout(box_7)
button_box.addLayout(box_8)
button_box.addLayout(box_9)
button_box.addLayout(box_10)
button_box.addItem(QtGui.QSpacerItem(20, 240))
large_box.addLayout(button_box)
self.marker_perception = ReadMarkers()
self.speech_recognition = SpeechRecognition(self)
self.bookDB = BookDB()
self.book_map = self.bookDB.getAllBookCodes()
self._widget.setObjectName('Milestone1GUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../librarian_gui_background.jpg"))
def get_function(self, text):
functions = {
"bring-me-a-book": self.pick_up_from_shelf_routine,
"put-this-book-back": self.prepare_to_take,
"give-me-information": self.give_information_routine,
"here-you-go": self.put_this_book_back_routine,
}
if text not in functions:
print("Could not find key %s" % text)
return None
return functions[text]
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def prepare_to_take(self):
# Open gripper and move arms to take book
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('l', 5.0)
self.move_arm('r', 5.0) # Increase these numbers for slower movement
self.l_gripper.close_gripper()
self.r_gripper.open_gripper(True)
self._sound_client.say("Please give me a book")
def take_from_human(self):
self.marker_perception.is_listening = True
# Close gripper and move arms to see book
self.r_gripper.close_gripper(True)
self._sound_client.say("Thank you")
time.sleep(1)
self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS
self.move_arm('r', 5.0,
True) # Increase these numbers for slower movement
self.look_at_r_gripper()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(self.marker_perception.get_marker_id()))
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def prepare_to_navigate(self):
self.marker_perception.is_listening = False
# Tuck arms
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('r', 5.0) # Increase these numbers for slower movement
#spin 360 * rotate_count degress clock wise
def spin_base(self, rotate_count):
# Adjust height and tuck arms before localization
self._sound_client.say("Please wait while I orient myself.")
self.torso.move(.15)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('l', 5.0, True)
self.move_arm('r', 5.0, True)
self.l_gripper.close_gripper()
self.r_gripper.close_gripper()
if not rotate_count:
rotate_count = 2
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.5)
start_time = rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(
15.0 * rotate_count):
base_publisher.publish(twist_msg)
def navigate_to_shelf(self, marker_id=None):
# Move forward, place book on the shelf, and move back
if marker_id is None or marker_id is False:
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(marker_id))
if marker_id is False:
rospy.loginfo("wuuuuuuuut")
if marker_id is None:
self._sound_client.say("I don't think I am holding a book "
"right now")
rospy.logwarn("Navigate to shelf called when marker id is None")
return
book = self.book_map.get(unicode(marker_id))
if book is None:
self._sound_client.say("The book that I am holding is unknown "
"to me")
rospy.logwarn(
"Navigate to shelf called when marker id is not in database")
return
x = book.getXCoordinate()
y = book.getYCoordinate()
self.navigation.move_to_shelf(x, y)
# Ye Olde Dropping way of putting a book on the shelf. Deprecating.
def drop_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.move_base(True)
self.r_gripper.open_gripper(True)
self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS
self.move_arm('r', 2.0,
True) # Increase these numbers for slower movement
self.move_base(False)
self.marker_perception.reset_marker_id()
def place_on_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.move_base(True)
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 2.0,
True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(False)
self.marker_perception.reset_marker_id()
def give_information(self):
marker_id = self.marker_perception.get_marker_id()
rospy.loginfo("marker id returned by get_marker_id is: " +
str(marker_id))
if marker_id is not None:
book = self.book_map.get(unicode(marker_id))
if book is None:
rospy.logwarn(
"Give information called when marker id is not in database"
)
self._sound_client.say(
"The book that I am holding is unknown to me")
else:
self._sound_client.say(book.getInformation())
else:
rospy.logwarn("Give information called when marker id is None")
self._sound_client.say(
"I don't think I am holding a book right now")
def pick_up_from_shelf_button(self):
self.pick_up_from_shelf_routine(self.book_textbox.text())
def pick_up_from_shelf_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say(
"The book you requested is not present in the database.")
else:
self.torso.move(.15) # move torso .1 meter from base (MAX is .2)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.move_arm('l', 5.0)
self.l_gripper.close_gripper()
# self.marker_perception.set_marker_id(book_id)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf(book_id) # Navigate to book location
self.pick_up_from_shelf() # Pick up from the shelf
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_person(
) # Navigate to designated help desk location
self.give_book() # Give Book to Human
def put_this_book_back_routine(self):
self.take_from_human()
time.sleep(5)
self.prepare_to_navigate()
time.sleep(5)
self.navigate_to_shelf()
self.place_on_shelf()
self.prepare_to_navigate()
# TODO: return to human?
def give_information_routine(self, book_title):
book_id = self.bookDB.getBookIdByTitle(book_title)
if book_id is None:
rospy.logwarn("Book asked for was not present in database")
self._sound_client.say(
"The book you requested is not present in the database.")
return
book = self.book_map.get(unicode(book_id))
self._sound_client.say(book.getInformation())
def pick_up_from_shelf(self):
self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS
self.move_arm('r', 4.0,
True) # Increase these numbers for slower movement
self.r_gripper.open_gripper(True)
self.move_base(True)
self.r_gripper.close_gripper(True)
self.move_base(False)
def navigate_to_person(self):
x = 2.82690143585
y = -0.416650295258
z = 0.252587109056
w = 0.967574158573
self.navigation.move_to_shelf(x, y, z, w)
def give_book(self):
self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS
self.move_arm('r', 4.0, True)
self.r_gripper.open_gripper(True)
# Moves forward to the bookshelf (or backward if isForward is false)
def move_base(self, isForward):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
distance = 0.25
if not isForward:
distance *= -1
twist_msg = Twist()
twist_msg.linear = Vector3(distance, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for x in range(0, 15):
rospy.loginfo("Moving the base")
base_publisher.publish(twist_msg)
time.sleep(0.15)
time.sleep(1.5)
# Moves arms using kinematics
def move_arm(self, side_prefix, time_to_joints=2.0, wait=False):
# forward kinematics
if (side_prefix == 'r'):
if self.saved_r_arm_pose is None:
rospy.logerr('Target pose for right arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
self.move_to_joints(side_prefix, self.saved_r_arm_pose,
time_to_joints, wait)
else: # side_prefix == 'l'
if self.saved_l_arm_pose is None:
rospy.logerr('Target pose for left arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
self.move_to_joints(side_prefix, self.saved_l_arm_pose,
time_to_joints, wait)
pass
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def move_to_joints(self,
side_prefix,
positions,
time_to_joint,
wait=False):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
if side_prefix == 'r':
traj_goal.trajectory.joint_names = self.r_joint_names
action_client = self.r_traj_action_client
else:
traj_goal.trajectory.joint_names = self.l_joint_names
action_client = self.l_traj_action_client
action_client.send_goal(traj_goal)
if wait:
time.sleep(time_to_joint)
class HeadControlDialog(Plugin):
def __init__(self, context):
super(HeadControlDialog, self).__init__(context)
self.setObjectName('HeadControlDialog')
self.head_max_pitch_deg = 65.0
self.head_min_pitch_deg = -60.0
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
self.scan_period_secs = 20.0
self.head_pitch_cmd_deg = 0.0
self.scan_offset = rospy.get_rostime()
self.last_publish_time = self.scan_offset
self._timer = None
self._widget = QWidget()
vbox = QVBoxLayout()
spindle_speed_hbox = QHBoxLayout()
#Add input for setting the spindle speed
spindle_label = QLabel("Spindle Speed [deg/s]")
spindle_speed_hbox.addWidget(spindle_label)
#Add a spinbox for setting the spindle speed
spindle_speed_spinbox = QDoubleSpinBox()
spindle_speed_spinbox.setRange(-360 * 4, 360 * 4)
spindle_speed_spinbox.valueChanged.connect(self.handle_spindle_speed)
self.spindle_speed_pub = rospy.Publisher('/multisense/set_spindle_speed', Float64, queue_size=10)
spindle_speed_hbox.addWidget(spindle_speed_spinbox)
vbox.addLayout(spindle_speed_hbox)
#Add input for directly setting the head pitch
head_pitch_hbox = QHBoxLayout()
head_pitch_label = QLabel("Head Pitch [deg]")
head_pitch_hbox.addWidget(head_pitch_label)
#Add a spinbox for setting the head pitch directly
self.head_pitch_spinbox = QDoubleSpinBox()
self.head_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
self.head_pitch_spinbox.valueChanged.connect(self.handle_head_pitch)
self.head_pitch_pub = rospy.Publisher('/atlas/pos_cmd/neck_ry', Float64, queue_size=10)
head_pitch_hbox.addWidget(self.head_pitch_spinbox)
vbox.addLayout(head_pitch_hbox)
#Publisher for head trajectory
self.head_trajectory_pub = rospy.Publisher('/trajectory_controllers/neck_traj_controller/command', JointTrajectory, queue_size=10)
#Add checkbox for invoking scanning behavior
self.head_scan_chkbox = QCheckBox("Scanning behavior")
self.head_scan_chkbox.stateChanged.connect(self.handle_scan_chg)
vbox.addWidget(self.head_scan_chkbox)
#Add input for setting the minimum head pitch
head_min_pitch_hbox = QHBoxLayout()
head_min_pitch_label = QLabel("Min Head Pitch [deg] (raise head)")
head_min_pitch_hbox.addWidget(head_min_pitch_label)
head_min_pitch_spinbox = QDoubleSpinBox()
head_min_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_min_pitch_spinbox.setValue(self.head_min_pitch_deg)
head_min_pitch_spinbox.valueChanged.connect(self.handle_head_min_pitch)
head_min_pitch_hbox.addWidget(head_min_pitch_spinbox)
vbox.addLayout(head_min_pitch_hbox)
#Add input for setting the maximum head pitch
head_max_pitch_hbox = QHBoxLayout()
head_max_pitch_label = QLabel("Max Head Pitch [deg] (lower head)")
head_max_pitch_hbox.addWidget(head_max_pitch_label)
head_max_pitch_spinbox = QDoubleSpinBox()
head_max_pitch_spinbox.setRange(self.head_min_pitch_deg, self.head_max_pitch_deg)
head_max_pitch_spinbox.setValue(self.head_max_pitch_deg)
head_max_pitch_spinbox.valueChanged.connect(self.handle_head_max_pitch)
head_max_pitch_hbox.addWidget(head_max_pitch_spinbox)
vbox.addLayout(head_max_pitch_hbox)
#Add input for setting the scan period
head_period_hbox = QHBoxLayout()
head_period_label = QLabel("Scanning Period [secs]")
head_period_hbox.addWidget(head_period_label)
head_period_spinbox = QDoubleSpinBox()
head_period_spinbox.setRange(0.01, 60.0)
head_period_spinbox.setValue(self.scan_period_secs)
head_period_spinbox.valueChanged.connect(self.handle_head_period)
head_period_hbox.addWidget(head_period_spinbox)
vbox.addLayout(head_period_hbox)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutdown ..."
if self._timer is not None:
self._timer.shutdown()
rospy.sleep(0.1)
self.spindle_speed_pub.unregister()
self.head_pitch_pub.unregister()
self.head_trajectory_pub.unregister()
print "Done!"
#Slot for setting the spindle speed
def handle_spindle_speed(self, degree_per_sec):
self.spindle_speed_pub.publish(data=math.radians(degree_per_sec))
#Slot for setting the head pitch state
def handle_head_pitch(self, pitch_degree):
self.head_pitch_cmd_deg = pitch_degree
self.head_pitch_pub.publish(data=math.radians(pitch_degree))
#Publish neck trajectory
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ['neck_ry']
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [math.radians(pitch_degree)]
trajectory.points[0].velocities = [0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.head_trajectory_pub.publish(trajectory)
#Slot for setting the head max pitch state
def handle_head_max_pitch(self, value):
self.head_max_pitch_deg = value
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
if (self.head_pitch_range_deg < 0.0001):
print 'invalid pitch limits - fix range!'
self.head_pitch_range_deg = 0.01
#Slot for setting the head min pitch state
def handle_head_min_pitch(self, value):
self.head_min_pitch_deg = value
self.head_pitch_range_deg = self.head_max_pitch_deg - self.head_min_pitch_deg
if (self.head_pitch_range_deg < 0.0001):
print 'invalid pitch limits - fix range!'
self.head_pitch_range_deg = 0.01
def handle_head_period(self, value):
self.scan_period_secs = value
@Slot(bool)
def handle_scan_chg(self, checked):
ros_time = rospy.get_rostime()
if checked:
print 'Start scanning ...'
if ((self.head_pitch_cmd_deg >= self.head_min_pitch_deg) and (self.head_pitch_cmd_deg <= self.head_max_pitch_deg)):
# adjust offset so that scanning command starts from current angle
# to provide bumpless transfer
# cos value corresponding to the current head pitch command
tmp = ((self.head_pitch_cmd_deg - self.head_min_pitch_deg) / self.head_pitch_range_deg - 0.5) * 2.0
# angle to give the same cosine value as current command
tmp_rad = math.acos(tmp)
tmp_time = tmp_rad * self.scan_period_secs / (math.pi * 2.0)
#Use integer math to avoid issues with UTC time having large numbers
self.scan_offset.secs = int(math.floor(tmp_time))
self.scan_offset.nsecs = int(math.floor((tmp_time - self.scan_offset.secs) * 1000000000))
self.scan_offset.secs -= ros_time.secs
self.scan_offset.nsecs -= ros_time.nsecs
if (self.scan_offset.nsecs < 0):
self.scan_offset.secs -= 1
self.scan_offset.nsecs += 1000000000
else:
print 'outside of range - ignore offset '
self.scan_offset = ros_time
self._timer = rospy.Timer(rospy.Duration(0.01), self.time_callback)
else:
print 'Stop scanning!'
if self._timer is not None:
self._timer.shutdown()
#Update the time
def time_callback(self, timer_event):
ros_time = rospy.get_rostime()
# Test and only publish at 100Hz so often to avoid spamming at 1khz
delta_time = ros_time - self.last_publish_time
if (delta_time.secs > 0) or (delta_time.nsecs > 10000000):
self.last_publish_time = ros_time
float_time = float(ros_time.secs + self.scan_offset.secs) + float(ros_time.nsecs+ self.scan_offset.nsecs) * 1e-9
rad = (float_time) * (math.pi * 2.0 / self.scan_period_secs)
cos_rad = math.cos(rad)
frac_range = 0.5 * cos_rad + 0.5
tmp_deg = frac_range * self.head_pitch_range_deg + self.head_min_pitch_deg
# Set value and publish inside spinbox callback
self.head_pitch_spinbox.setValue(tmp_deg)
class cob_teacher_plugin(Plugin):
plugin_chooser = []
current_teacher = []
def __init__(self, context):
super(cob_teacher_plugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('cob_teacher_plugin')
self._widget = QWidget()
self._widget.setObjectName('cob_teacher_plugin')
self.group_layout = QtGui.QVBoxLayout()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
args = self._parse_args(context.argv())
for config_file in args.config_file:
print config_file
self.ym = YamlManager(config_file)
for field in self.ym.getFields():
self.group_layout.addWidget(self.getFieldWidget(field))
placeholder = QtGui.QWidget()
self.group_layout.addWidget(placeholder, 1)
self.save_header = QtGui.QWidget()
self.save_layout = QtGui.QHBoxLayout()
self.save_header.setLayout(self.save_layout)
self.save_button = QtGui.QPushButton("Save")
self.save_button.connect(self.save_button, QtCore.SIGNAL('clicked()'), self.saveValues)
self.close_button = QtGui.QPushButton("Close")
self.connect(self.close_button, QtCore.SIGNAL('clicked()'), QtGui.qApp, QtCore.SLOT('quit()'))
self.save_layout.addWidget(self.save_button)
self.save_layout.addWidget(self.close_button)
self.group_layout.addWidget(self.save_header)
self._widget.setLayout(self.group_layout)
def getFieldWidget(self, field):
group = QtGui.QGroupBox()
# set title
teachers_found = self.findTeachers(field)
if(len(teachers_found) > 1):
group.setTitle("select teacher for: '"+ field + "'")
elif(len(teachers_found) == 0):
not_found_widget = QtGui.QLabel("No Plugin found for "+ self.ym.getType(field))
field_layout.addWidget(not_found_widget)
else:
group.setTitle(teachers_found[0]().getName())
# set layout
field_layout = QtGui.QVBoxLayout()
group.setLayout(field_layout)
# choose teacher
if(len(teachers_found) > 1):
print "Several plugins were found for fieldtype " + self.ym.getType(field)
combo = QtGui.QComboBox()
combo.addItem("")
for teacher in teachers_found:
combo.addItem(teacher().getName())
field_layout.addWidget(combo)
self.plugin_chooser.append({"name": field, "layout": field_layout, "widget": None, "chooser": combo})
self.current_teacher.append({"name": field, "teacher": None})
self.connect(combo, QtCore.SIGNAL('activated(QString)'), self.combo_chosen)
elif(len(teachers_found) == 0):
not_found_widget = QtGui.QLabel("No Plugin found for "+ self.ym.getType(field))
field_layout.addWidget(not_found_widget)
else:
teacher = teachers_found[0]()
self.current_teacher.append({"name": field, "teacher": teacher})
teach_widget = teacher.getRQTWidget(field, self.ym.data[field])
field_layout.addWidget(teach_widget)
return group
def saveValues(self):
for teacher in self.current_teacher:
if(teacher["teacher"] != None):
print "Updating:", teacher["name"]
self.ym.updateField(teacher["name"], teacher["teacher"].getRQTData(teacher["name"]))
print "saving values"
self.ym.writeFile()
def combo_chosen(self, text):
sender = self.sender()
for chooser in self.plugin_chooser:
if(chooser["chooser"] == sender):
print "Teacher for", chooser["name"], "?"
for teacher in availableTeachers:
if(teacher().getName() == text):
print "Chosen: ", text
this_teacher = teacher()
if this_teacher.getName() == "PoseTouchupTeacher":
target_frame = ""
for t in self.current_teacher:
if t["name"] == "target_frame":
target_frame = t["teacher"].getRQTData(t["name"])
teach_widget = this_teacher.getRQTWidget(chooser["name"], self.ym.data[chooser["name"]], target_frame)
else:
teach_widget = this_teacher.getRQTWidget(chooser["name"], self.ym.data[chooser["name"]])
if(teach_widget != None):
#remove currently activated plugin
if(chooser["widget"] != None):
chooser["widget"].setParent(None)
for t in self.current_teacher:
if(t["name"] == chooser["name"]):
t["teacher"] = this_teacher
chooser["widget"] = teach_widget
chooser["layout"].addWidget(teach_widget)
def findTeachers(self, fieldName):
teachers_found = []
for teacher in availableTeachers:
if(teacher().getType() == self.ym.getType(fieldName)):
teachers_found.append(teacher)
return teachers_found
def closeEvent(self, event):
reply = QtGui.QMessageBox.question(self, 'Message',
"Are you sure to quit?", QtGui.QMessageBox.Yes, QtGui.QMessageBox.No)
if reply == QtGui.QMessageBox.Yes:
event.accept()
else:
event.ignore()
def _parse_args(self, argv):
parser = argparse.ArgumentParser(prog='cob_teacher', add_help=False)
cob_teacher_plugin.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
group = parser.add_argument_group('Options for cob_teacher plugin')
group.add_argument('config_file', nargs='*', default=[], help='Configfile to load')
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class PbDGUI(Plugin):
exp_state_sig = Signal(ExperimentState)
def __init__(self, context):
super(PbDGUI, self).__init__(context)
self.setObjectName('PbDGUI')
self._widget = QWidget()
self.speech_cmd_publisher = rospy.Publisher('recognized_command',
Command)
self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand)
rospy.Subscriber('experiment_state', ExperimentState,
self.exp_state_cb)
rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.exp_state_sig.connect(self.update_state)
self.commands = dict()
self.commands[Command.TEST_MICROPHONE] = 'Test microphone'
self.commands[Command.NEW_DEMONSTRATION] = 'New demonstration'
self.commands[Command.START_RECORDING] = 'Start recording'
self.commands[Command.STOP_RECORDING] = 'Stop recording'
self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration'
self.commands[Command.DETECT_SURFACE] = 'Detect surface'
self.commands[Command.TAKE_TOOL] = 'Take tool'
self.commands[Command.RELEASE_TOOL] = 'Release tool'
self.commands[Command.SAVE_ARM_POSE] = 'Save arm pose'
self.currentAction = -1
self.currentStep = -1
allWidgetsBox = QtGui.QVBoxLayout()
actionBox = QGroupBox('Demonstrations', self._widget)
self.actionGrid = QtGui.QGridLayout()
self.actionGrid.setHorizontalSpacing(0)
for i in range(6):
self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i,
QtCore.Qt.AlignCenter)
self.actionGrid.setColumnStretch(i, 0)
self.actionIcons = dict()
actionBoxLayout = QtGui.QHBoxLayout()
actionBoxLayout.addLayout(self.actionGrid)
actionBox.setLayout(actionBoxLayout)
actionButtonGrid = QtGui.QHBoxLayout()
actionButtonGrid.addWidget(
self.create_button(Command.NEW_DEMONSTRATION))
self.stepsBox = QGroupBox('No demonstrations', self._widget)
self.stepsGrid = QtGui.QGridLayout()
self.l_model = QtGui.QStandardItemModel(self)
self.l_view = self._create_table_view(self.l_model,
self.l_row_clicked_cb)
self.r_model = QtGui.QStandardItemModel(self)
self.r_view = self._create_table_view(self.r_model,
self.r_row_clicked_cb)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3)
self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0)
self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2)
self.stepsGrid.addWidget(self.l_view, 1, 0)
self.stepsGrid.addWidget(self.r_view, 1, 2)
stepsBoxLayout = QtGui.QHBoxLayout()
stepsBoxLayout.addLayout(self.stepsGrid)
self.stepsBox.setLayout(stepsBoxLayout)
stepsButtonGrid = QtGui.QHBoxLayout()
stepsButtonGrid.addWidget(
self.create_button(Command.REPLAY_DEMONSTRATION))
motionButtonGrid = QtGui.QHBoxLayout()
motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING))
motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING))
misc_grid = QtGui.QHBoxLayout()
misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE))
misc_grid.addWidget(self.create_button(Command.DETECT_SURFACE))
misc_grid.addStretch(1)
misc_grid2 = QtGui.QHBoxLayout()
misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL))
misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL))
misc_grid2.addStretch(1)
misc_grid3 = QtGui.QHBoxLayout()
misc_grid3.addWidget(self.create_button(Command.SAVE_ARM_POSE))
misc_grid3.addStretch(1)
speechGroupBox = QGroupBox('Robot Speech', self._widget)
speechGroupBox.setObjectName('RobotSpeechGroup')
speechBox = QtGui.QHBoxLayout()
self.speechLabel = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speechLabel.setPalette(palette)
speechBox.addWidget(self.speechLabel)
speechGroupBox.setLayout(speechBox)
allWidgetsBox.addWidget(actionBox)
allWidgetsBox.addLayout(actionButtonGrid)
allWidgetsBox.addWidget(self.stepsBox)
allWidgetsBox.addLayout(motionButtonGrid)
allWidgetsBox.addLayout(stepsButtonGrid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid2)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40))
allWidgetsBox.addLayout(misc_grid3)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addWidget(speechGroupBox)
allWidgetsBox.addStretch(1)
# Fix layout and add main widget to the user interface
QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique'))
vAllBox = QtGui.QVBoxLayout()
vAllBox.addLayout(allWidgetsBox)
vAllBox.addStretch(1)
hAllBox = QtGui.QHBoxLayout()
hAllBox.addLayout(vAllBox)
hAllBox.addStretch(1)
self._widget.setObjectName('PbDGUI')
self._widget.setLayout(hAllBox)
context.add_widget(self._widget)
rospy.loginfo('Will wait for the experiment state service...')
rospy.wait_for_service('get_experiment_state')
exp_state_srv = rospy.ServiceProxy('get_experiment_state',
GetExperimentState)
rospy.loginfo('Got response from the experiment state service...')
response = exp_state_srv()
self.update_state(response.state)
def _create_table_view(self, model, row_click_cb):
proxy = QtGui.QSortFilterProxyModel(self)
proxy.setSourceModel(model)
view = QtGui.QTableView(self._widget)
verticalHeader = view.verticalHeader()
verticalHeader.sectionClicked.connect(row_click_cb)
view.setModel(proxy)
view.setMaximumWidth(250)
view.setSortingEnabled(False)
view.setCornerButtonEnabled(False)
return view
def get_uid(self, arm_index, index):
'''Returns a unique id of the marker'''
return (2 * (index + 1) + arm_index)
def get_arm_and_index(self, uid):
'''Returns a unique id of the marker'''
arm_index = uid % 2
index = (uid - arm_index) / 2
return (arm_index, (index - 1))
def r_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(0, logicalIndex))
def l_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(1, logicalIndex))
def create_button(self, command):
btn = QtGui.QPushButton(self.commands[command], self._widget)
btn.clicked.connect(self.command_cb)
return btn
def update_state(self, state):
qWarning('Received new state')
n_actions = len(self.actionIcons.keys())
if n_actions < state.n_actions:
for i in range(n_actions, state.n_actions):
self.new_action()
if (self.currentAction != (state.i_current_action - 1)):
self.delete_all_steps()
self.action_pressed(state.i_current_action - 1, False)
n_steps = self.n_steps()
if (n_steps < state.n_steps):
for i in range(n_steps, state.n_steps):
self.save_pose(frameType=ord(state.frame_types[i]))
elif (n_steps > state.n_steps):
n_to_remove = n_steps - state.n_steps
self.r_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
self.l_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
## TODO: DEAL with the following arrays!!!
state.r_gripper_states
state.l_gripper_states
state.r_ref_frames
state.l_ref_frames
state.objects
if (self.currentStep != state.i_current_step):
if (self.n_steps() > 0):
self.currentStep = state.i_current_step
arm_index, index = self.get_arm_and_index(self.currentStep)
if (arm_index == 0):
self.r_view.selectRow(index)
else:
self.l_view.selectRow(index)
def get_frame_type(self, fr_type):
if (fr_type > 1):
rospy.logwarn(
"Invalid frame type @ save_pose -> get_frame_type: " +
str(fr_type))
return ["Go to pose", "Maneuver"][fr_type]
def save_pose(self, actionIndex=None, frameType=0):
nColumns = 9
if actionIndex is None:
actionIndex = self.currentAction
stepIndex = self.n_steps(actionIndex)
r_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')
]
l_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem(self.get_frame_type(frameType)),
QtGui.QStandardItem('Absolute')
]
self.r_model.invisibleRootItem().appendRow(r_step)
self.l_model.invisibleRootItem().appendRow(l_step)
self.update_table_view()
self.currentStep = stepIndex
def update_table_view(self):
self.l_view.setColumnWidth(0, 50)
self.l_view.setColumnWidth(1, 100)
self.l_view.setColumnWidth(2, 70)
self.r_view.setColumnWidth(0, 50)
self.r_view.setColumnWidth(1, 100)
self.r_view.setColumnWidth(2, 70)
def n_steps(self, actionIndex=None):
return self.l_model.invisibleRootItem().rowCount()
def delete_all_steps(self, actionIndex=None):
if actionIndex is None:
actionIndex = self.currentAction
n_steps = self.n_steps()
if (n_steps > 0):
self.l_model.invisibleRootItem().removeRows(0, n_steps)
self.r_model.invisibleRootItem().removeRows(0, n_steps)
def n_actions(self):
return len(self.actionIcons.keys())
def new_action(self):
nColumns = 6
actionIndex = self.n_actions()
for key in self.actionIcons.keys():
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed)
self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns),
actionIndex % nColumns)
self.actionIcons[actionIndex] = actIcon
def step_pressed(self, step_index):
gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index)
self.gui_cmd_publisher.publish(gui_cmd)
def action_pressed(self, actionIndex, isPublish=True):
for i in range(len(self.actionIcons.keys())):
key = self.actionIcons.keys()[i]
if key == actionIndex:
self.actionIcons[key].selected = True
else:
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
self.currentAction = actionIndex
self.stepsBox.setTitle('Steps for Action ' +
str(self.currentAction + 1))
if isPublish:
gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION,
(actionIndex + 1))
self.gui_cmd_publisher.publish(gui_cmd)
def command_cb(self):
clickedButtonName = self._widget.sender().text()
for key in self.commands.keys():
if (self.commands[key] == clickedButtonName):
qWarning('Sending speech command: ' + key)
command = Command()
command.command = key
self.speech_cmd_publisher.publish(command)
def robotSoundReceived(self, soundReq):
if (soundReq.command == SoundRequest.SAY):
qWarning('Robot said: ' + soundReq.arg)
self.speechLabel.setText('Robot sound: ' + soundReq.arg)
def exp_state_cb(self, state):
qWarning('Received new experiment state.')
self.exp_state_sig.emit(state)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.speech_cmd_publisher.unregister()
self.gui_cmd_publisher.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class VelocityControl(Plugin):
signal_topic = Signal(JointState)
def __init__(self, context):
super(VelocityControl, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('VelocityControl')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
#ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
#loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('VelocityControlUi')
vLayout = QVBoxLayout(self._widget)
vLayout.setContentsMargins(0, 0, 0, 0)
vLayout.setSpacing(0)
self._widget.setLayout(vLayout)
self._widget.layout().setSpacing(0)
self._widget.setWindowTitle("VelocityControl");
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
self._topic_list = ''
self._topic_commands = {}
self._jointgroups = {}
self.signal_topic.connect( self.signal_callback_list )
rospy.on_shutdown(self.on_ros_shutdown) #handle the ROS shutdown commands
def on_ros_shutdown(self, *args):
from python_qt_binding.QtGui import QApplication
QApplication.exit(0)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.shutdownRosComm()
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
self._topic_commands.clear()
for caller, joint_frame in self._jointgroups.items():
self._topic_commands[caller] = joint_frame.get_command_topic()
instance_settings.set_value('topic_commands', self._topic_commands)
instance_settings.set_value('topic_list', self._topic_list)
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
self.shutdownRosComm()
self._topic_commands = instance_settings.value('topic_commands', {})
self._topic_list = instance_settings.value('topic_list', 'joint_state')
self.fill_widget()
self.reinitRosComm()
def reinitRosComm(self):
# if self._topic_command:
# self._topic_command = rosgraph.names.script_resolve_name('rostopic', self._topic_command)
# if (self._topic_command):
# self._publisher_command = rospy.Publisher(self._topic_command, PowerSwitch, queue_size=1)
if self._topic_list:
self._topic_list = rosgraph.names.script_resolve_name('rostopic', self._topic_list)
if self._topic_list:
self._subscriber_list = rospy.Subscriber(self._topic_list, JointState, self.callback_jointstate)
def clearstuff(self):
pass
# for name,p in self._devices.items():
# self._widget.layout().removeWidget(p)
# p.clearParent()
# self._devices.clear();
def shutdownRosComm(self):
self.clearstuff()
# if self._topic_command:
# self._publisher_command.unregister()
# self._publisher_command = None
if self._topic_list:
self._subscriber_list.unregister()
self._subscriber_list = None
def fill_widget(self):
# for i in range(len(self._topics)):
self._widget.layout().addItem(QSpacerItem(1,1,QSizePolicy.Expanding, QSizePolicy.Expanding))
def callback_jointstate(self, msg):
self.signal_topic.emit(msg)
def signal_callback_list( self, msg ):
# update
caller = msg._connection_header['callerid']
if caller not in self._jointgroups:
stored_topic = None
if caller in self._topic_commands:
stored_topic = self._topic_commands[caller]
group_frame = JointStateGroup(caller, msg, stored_topic=stored_topic)
self._widget.layout().insertWidget(self._widget.layout().count() - 1, group_frame)
self._jointgroups[caller] = group_frame
else:
# update the values
self._jointgroups[caller].update(msg)
def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure it
# Usually used to open a configuration dialog
self.dialog_config = QDialog()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'iop_rqt_velocity_config.ui')
loadUi(ui_file, self.dialog_config)
self.dialog_config.accepted.connect(self.on_dialog_config_accepted)
# fill configuration dialog
ti = TopicInfo()
ti.fill_published_topics(self.dialog_config.comboBox_listTopic, "sensor_msgs/JointState", self._topic_list)
# ti.fill_subscribed_topics(self.dialog_config.comboBox_commandTopic, "std_msgs/Float64MultiArray", self._topic_command)
# stop on cancel pressed
if not self.dialog_config.exec_():
return
def on_dialog_config_accepted(self):
self.shutdownRosComm()
# self._topic_command = self.dialog_config.comboBox_commandTopic.currentText()
self._topic_list = self.dialog_config.comboBox_listTopic.currentText()
self.reinitRosComm()
class SinusoidalTrajectoryDialog(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(SinusoidalTrajectoryDialog, self).__init__(context)
self.setObjectName('SinusoidalTrajectoryDialog')
#self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx,jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
#print jnt.name, " ",self.joint_list[jnt.name]
self.chain=[]
self.chain_file = rospy.get_param("chain_file")
self.chain_name = rospy.get_param("chain_name")
yaml_file = self.chain_file+self.chain_name+"_chain.yaml"
print yaml_file
#define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
print '\n\n'
for ndx, data in enumerate(jointChain):
print ndx," : ", data
self.delay_time = data["delay_time"]
self.amplitude = data["amplitude"]
self.frequency = data["frequency"]
self.frequency_limit = data["frequency_limit"]
self.iterations = data["iterations"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
joints = rospy.get_param(data["chain_param_name"])
for joint in joints:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint) )
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0]*len(self.robot_state.name)
self.robot_state.velocity = [0.0]*len(self.robot_state.name)
self.robot_state.effort = [0.0]*len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =",self.delay_time
print "amplitude =",self.amplitude
print "frequency =",self.frequency
print "iterations =",self.iterations
print "Robot State Structure",self.robot_state
print "Robot Command Structure",self.robot_command
# initialize structure to hold widget handles
self.cur_position_spinbox=[]
self.amplitude_spinbox=[]
self.frequency_spinbox=[]
self.iterations_spinbox=[]
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Values")
zero_ramp.clicked.connect(self.zero_values_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_frequqency = QVBoxLayout()
vbox_frequqency.addWidget(QLabel("Frequency"))
self.frequency_spinbox = QDoubleSpinBox()
self.frequency_spinbox.setDecimals(5)
self.frequency_spinbox.setRange(0, self.frequency_limit)
self.frequency_spinbox.setSingleStep(0.05)
self.frequency_spinbox.valueChanged.connect(self.on_frequency_value)
self.frequency_spinbox.setValue(self.frequency)
vbox_frequqency.addWidget(self.frequency_spinbox)
time_hbox.addLayout(vbox_frequqency)
vbox_iterations = QVBoxLayout()
vbox_iterations.addWidget(QLabel("Iterations"))
self.iterations_spinbox = QDoubleSpinBox()
self.iterations_spinbox.setDecimals(5)
self.iterations_spinbox.setRange(0, 10)
self.iterations_spinbox.setSingleStep(1)
self.iterations_spinbox.valueChanged.connect(self.on_iterations_value)
self.iterations_spinbox.setValue(self.iterations)
vbox_iterations.addWidget(self.iterations_spinbox)
time_hbox.addLayout(vbox_iterations)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel);
title_frame.setFrameShadow(QFrame.Raised);
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Current Position"))
title_hbox.addWidget(QLabel("Amplitude"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i,joint in enumerate(self.chain):
#print i,",",joint
self.joint_widget( vbox, i)
#add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic , JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
@Slot()
def snap_current_callback(self):
self.blockSignals(True)
print "Snapping positions to actual values"
for index, joint in enumerate(self.chain):
for index_state, name_state in enumerate(self.robot_joint_state.name):
if (name_state == joint.name):
joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
self.cur_position_spinbox[index].setValue(joint.position)
break
self.blockSignals(False)
@Slot()
def check_limits_callback(self):
self.blockSignals(True)
print "Check limits callback ..."
valid = True
for index, joint in enumerate(self.chain):
ramp_up = joint.position + joint.amplitude
ramp_down = joint.position - joint.amplitude
p = self.amplitude_spinbox[index].palette()
if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
print "Joint ",joint.name, " is beyond upper limit!"
valid=False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
#print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit
self.amplitude_spinbox[index].setPalette(p)
if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
print "Joint ",joint.name, " is beyond lower limit!"
valid=False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white)# <<<<<<<<<<<----------------- This isn't working as intended
#print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit
if (self.frequency > self.frequency_limit or self.frequency <= 0):
print "invalid frequency. must be between 0 and ", self.frequency_limit, ". value is: ", self.frequency
valid = False
if (not valid):
print "ERROR: Invalid input!"
else:
print " Valid values!"
self.blockSignals(False)
return valid
@Slot()
def zero_values_callback(self):
self.blockSignals(True)
print "Zero ramps callback ..."
for index, joint in enumerate(self.chain):
self.amplitude_spinbox[index].setValue(0.0)
self.blockSignals(False)
@Slot()
def apply_command_callback(self):
self.blockSignals(True)
print "Send trajectory"
if self.calculate_trajectory():
#print self.robot_command;
self.commandPublisher.publish(self.robot_command)
else:
print "Trajectory calculation failure - do not publish!"
self.blockSignals(False)
@Slot()
def save_trajectory_callback(self):
self.blockSignals(True)
print "Save gains"
# Save data to file that we can read in
# TODO - invalid file names
fileName = QFileDialog.getSaveFileName()
#if fileName[0] checks to ensure that the file dialog has not been canceled
if fileName[0]:
if self.calculate_trajectory():
print self.robot_command;
newFileptr = open(fileName[0], 'w')
# Suggested format for files to make it easy to combine different outputs
newFileptr.write('# Trajectory \n')
newFileptr.write(self.robot_command)
newFileptr.write('\n')
newFileptr.close()
else:
print "Trajectory calculation failure - do not save!"
else:
print "Save cancelled."
newFilePtr.close()
self.blockSignals(False)
#
@Slot()
def stateCallbackFnc(self, atlasState_msg):
# Handle the state message for actual positions
time = atlasState_msg.header.stamp.to_sec()
if ((time - self.prior_time) > 0.02):
# Only update at 50hz
# relay the ROS messages through a Qt Signal to switch to the GUI thread
self.updateStateSignal.emit(atlasState_msg)
self.prior_time = time
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, joint_state_msg):
#print joint_state_msg
self.robot_joint_state = joint_state_msg
def on_delay_time_value(self, value):
self.delay_time = copy.deepcopy(value)
def on_frequency_value(self, value):
self.frequency = copy.deepcopy(value)
def on_iterations_value(self, value):
self.iterations = copy.deepcopy(value)
def calculate_trajectory(self):
if not self.check_limits_callback():
print "Invalid limits for trajectory!"
return False
knot_points = 8*self.iterations
if (knot_points < 2):
print "Invalid trajectory - knot_points = ",knot_points
return False
#print "Knot points=",knot_points
self.robot_command.points = []
self.robot_command.points.append(JointTrajectoryPoint())
ndx = 0
self.robot_command.points[ndx].time_from_start = rospy.Duration(0.0)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
self.robot_command.points[ndx].velocities.append(0.0)
self.robot_command.points[ndx].accelerations.append(0.0)
ndx += 1
dt = np.pi / (4.0 * self.frequency)
time_offset = dt
print "dt = ", dt
while ndx < knot_points-1:
self.robot_command.points.append(JointTrajectoryPoint())
time_offset += dt
print " time = ", time_offset
self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position + joint.amplitude * np.sin(self.frequency * time_offset))
self.robot_command.points[ndx].velocities.append(self.frequency * joint.amplitude * np.cos(self.frequency * time_offset))
self.robot_command.points[ndx].accelerations.append(-self.frequency * self.frequency * joint.amplitude * np.sin(self.frequency * time_offset))
ndx += 1
#end position
self.robot_command.points.append(JointTrajectoryPoint())
time_offset += dt
self.robot_command.points[ndx].time_from_start = rospy.Duration(time_offset)
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
self.robot_command.points[ndx].velocities.append(0.0)
self.robot_command.points[ndx].accelerations.append(0.0)
print self.robot_command
print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)
if (ndx != len(self.robot_command.points)-1) or (len(self.robot_command.points) != knot_points):
print "Invalid number of knot points - ignore trajectory"
print self.robot_command
return False
self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return True;
def shutdown_plugin(self):
#Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
#TODO, is the above comment why the plugin gets stuck when we try to shutdown?
print "Shutdown ..."
rospy.sleep(0.1)
self.jointSubscriber.unregister()
self.commandPublisher.unregister()
rospy.sleep(0.5)
print "Done!"
# Define collection of widgets for joint group
def joint_widget( self, main_vbox, index):
joint = self.chain[index]
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel);
frame.setFrameShadow(QFrame.Raised);
hbox = QHBoxLayout()
#hbox.addWidget(frame)
self.prior_time = 0.0
robot_joint = self.robot.joints[self.joint_list[joint.name]]
joint.lower_limit = robot_joint.limit.lower
joint.upper_limit = robot_joint.limit.upper
amplitude_range = (robot_joint.limit.upper-robot_joint.limit.lower)
frequency_range = self.frequency_limit
iterations_range = 10000
print " ",joint.name, " limits(", joint.lower_limit,", ",joint.upper_limit,") range=",amplitude_range
self.cur_position_spinbox.append(QDoubleSpinBox())
self.cur_position_spinbox[index].setDecimals(5)
self.cur_position_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
self.cur_position_spinbox[index].setSingleStep((robot_joint.limit.upper-robot_joint.limit.lower)/50.0)
self.cur_position_spinbox[index].valueChanged.connect(joint.on_position_value)
self.amplitude_spinbox.append(QDoubleSpinBox())
self.amplitude_spinbox[index].setDecimals(5)
self.amplitude_spinbox[index].setRange(-amplitude_range, amplitude_range)
self.amplitude_spinbox[index].setSingleStep(amplitude_range/50.0)
self.amplitude_spinbox[index].valueChanged.connect(joint.on_amplitude_value)
self.amplitude_spinbox[index].setValue(joint.amplitude)
hbox.addWidget(QLabel(joint.name))
hbox.addWidget(self.cur_position_spinbox[index])
hbox.addWidget(self.amplitude_spinbox[index])
# Add horizontal layout to frame for this joint group
frame.setLayout(hbox)
# Add frame to main vertical layout
main_vbox.addWidget(frame)
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
#Sound textbox
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text
sound_textbox.setFixedWidth(450)
#Set a handle on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15,20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet.')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
up_head = Head(Head.UP)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
#large_box.addWidget(up_head_button)
down_head = Head(Head.DOWN)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
#large_box.addWidget(down_head_button)
right_head = Head(Head.RIGHT)
right_head_button = self.create_button('>', right_head.create_closure())
#large_box.addWidget(right_head_button)
left_head = Head(Head.LEFT)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235,20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275,20))
left_right_head_box.addItem(QtGui.QSpacerItem(160,20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60,20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225,20))
down_head_box.addItem(QtGui.QSpacerItem(235,20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275,20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
#large_box.addItem(QtGui.QSpacerItem(500,20))
#large_box.addWidget(left_head_button)
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN)
right_gripper = self.create_button('Right gripper!', gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN)
left_gripper = self.create_button('Left gripper!', gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100,250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75,20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450,20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75,20))
large_box.addLayout(gripper_box)
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100,100))
#forward
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD)
forward_base_button = self.create_button('move forward', forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(forward_base_box)
#large_box.addWidget(forward_base_button)
#left
left_right_base_box = QtGui.QHBoxLayout()
left_base= Base(Base.LEFT)
left_base_button = self.create_button('move left', left_base.create_closure())
#large_box.addWidget(left_base_button)
#right
right_base= Base(Base.RIGHT)
right_base_button= self.create_button('move right', right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50,20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
base_box.addLayout(left_right_base_box)
#large_box.addWidget(right_base_button)
#backward
backward_base_box = QtGui.QHBoxLayout()
backward_base= Base(Base.BACKWARD)
backward_base_button = self.create_button('move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(backward_base_box)
#large_box.addWidget(backward_base_button)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
#turn left
turnleft_base= Base(Base.TURNLEFT)
turnleft_base_button = self.create_button(' /\n<--', turnleft_base.create_closure())
#large_box.addWidget(turnleft_base_button)
#turn right
turnright_base= Base(Base.TURNRIGHT)
turnright_base_button = self.create_button('\\\n -->', turnright_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75,20))
turn_base_box.addWidget(turnright_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225,20))
turn_base_box.addWidget(turnleft_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100,20))
large_box.addLayout(turn_base_box)
#large_box.addWidget(turnright_base_button)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet("QWidget { image: url(%s) }" % "/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg")
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SpectrogramPlugin(Plugin):
update_signal = QtCore.pyqtSignal()
subscriber_signal = QtCore.pyqtSignal(str)
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.line_edit = QLineEdit()
layout_.addWidget(self.line_edit)
self.apply_btn = QPushButton("Apply")
self.apply_btn.clicked.connect(self.apply_clicked)
layout_.addWidget(self.apply_btn)
layout.addLayout(layout_)
self.fig = Figure((5, 4), dpi=100)
self.canvas = FigureCanvas(self.fig)
self.axes = self.fig.add_subplot(111)
self.fig.tight_layout()
layout.addWidget(self.canvas)
context.add_widget(self._widget)
self.update_signal.connect(self.update_spectrogram)
self.subscriber_signal.connect(self.update_subscriber)
self.subscriber_signal.emit('spec')
def spectrum_callback(self, data):
nch = data.nch
len = data.nfreq
if self.spectrogram is None:
self.spectrogram = zeros([len, 1000])
s = array(data.data).reshape([nch, len, 2])[-1]
s = linalg.norm(s, axis=1)
s += 1e-8
log(s, s)
self.spectrogram = roll(self.spectrogram, -1, 1)
self.spectrogram[:, -1] = s
if data.header.seq % 100 == 0:
self.update_signal.emit()
def apply_clicked(self):
self.update_subscriber(self.line_edit.displayText())
def update_spectrogram(self):
if self.spectrogram is not None:
self.axes.clear()
self.axes.imshow(self.spectrogram,
aspect="auto",
origin="lower",
cmap="hot")
self.axes.grid(None)
self.axes.set_ylabel("Freq. [bin]")
self.axes.set_xlabel("Time [frame]")
self.fig.tight_layout()
self.canvas.draw()
QApplication.processEvents()
def update_subscriber(self, topic_name):
self.topic_name = topic_name
self.line_edit.setText(self.topic_name)
if hasattr(self, 'sub'):
self.sub.unregister()
self.spectrogram = None
self.sub = rospy.Subscriber(topic_name,
Spectrum,
self.spectrum_callback,
queue_size=500)
def restore_settings(self, plugin_settings, instance_settings):
topic_name = instance_settings.value('topic_name')
self.subscriber_signal.emit(topic_name)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic_name', self.topic_name)
def shutdown_plugin(self):
pass
class SensorParamControlDialog(Plugin):
def __init__(self, context):
super(SensorParamControlDialog, self).__init__(context)
self.setObjectName('SensorParamControlDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
### Multisense ###
ms_groupbox = QGroupBox( "Multisense" )
ms_vbox = QVBoxLayout()
ms_gain_hbox = QHBoxLayout()
self.ms_gain_label = QLabel("Image Gain [1, 1, 8]")
ms_gain_hbox.addWidget( self.ms_gain_label )
self.ms_gain = QDoubleSpinBox()
self.ms_gain.setSingleStep(.01)
self.ms_gain.setRange(1,8)
ms_gain_hbox.addWidget( self.ms_gain )
ms_vbox.addLayout( ms_gain_hbox )
ms_exp_hbox = QHBoxLayout()
self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]")
ms_exp_hbox.addWidget( self.ms_exp_auto )
self.ms_exp = QDoubleSpinBox()
self.ms_exp.setSingleStep( .001 )
self.ms_exp.setRange( .025,.5 )
ms_exp_hbox.addWidget( self.ms_exp )
ms_vbox.addLayout( ms_exp_hbox )
ms_spindle_hbox = QHBoxLayout()
ms_spindle_label = QLabel("Spindle Speed [0, 5.2]")
ms_spindle_hbox.addWidget( ms_spindle_label )
self.ms_spindle = QDoubleSpinBox()
self.ms_spindle.setSingleStep(.01)
self.ms_spindle.setRange( 0,15.2 )
ms_spindle_hbox.addWidget( self.ms_spindle )
ms_vbox.addLayout( ms_spindle_hbox )
ms_light_hbox = QHBoxLayout()
ms_light_label = QLabel("Light Brightness")
ms_light_hbox.addWidget(ms_light_label)
self.ms_light = QSlider(Qt.Horizontal)
self.ms_light.setRange(0,100)
ms_light_hbox.addWidget( self.ms_light )
ms_vbox.addLayout( ms_light_hbox )
ms_button_hbox = QHBoxLayout()
ms_button_hbox.addStretch(1)
ms_button_get = QPushButton("Get Settings")
ms_button_get.pressed.connect(self.ms_get_callback)
#ms_button_hbox.addWidget( ms_button_get )
ms_button_set = QPushButton("Set Settings")
ms_button_set.pressed.connect(self.ms_set_callback)
ms_button_hbox.addWidget( ms_button_set )
ms_vbox.addLayout( ms_button_hbox )
ms_groupbox.setLayout( ms_vbox )
vbox.addWidget( ms_groupbox )
### Left SA ###
sa_left_groupbox = QGroupBox( "Left SA Camera" )
sa_left_vbox = QVBoxLayout()
sa_left_gain_hbox = QHBoxLayout()
sa_left_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_left_gain_hbox.addWidget( sa_left_gain_label )
self.sa_left_gain = QDoubleSpinBox()
self.sa_left_gain.setSingleStep(.01)
self.sa_left_gain.setRange( 0, 25 )
sa_left_gain_hbox.addWidget( self.sa_left_gain )
sa_left_vbox.addLayout( sa_left_gain_hbox )
sa_left_exp_hbox = QHBoxLayout()
sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_left_exp_hbox.addWidget( sa_left_exp_label )
self.sa_left_exp = QDoubleSpinBox()
self.sa_left_exp.setSingleStep(.01)
self.sa_left_exp.setRange( 0, 25 )
sa_left_exp_hbox.addWidget( self.sa_left_exp )
sa_left_vbox.addLayout( sa_left_exp_hbox )
sa_left_button_hbox = QHBoxLayout()
sa_left_button_hbox.addStretch(1)
sa_left_button_get = QPushButton("Get Settings")
sa_left_button_get.pressed.connect(self.sa_left_get_callback)
#sa_left_button_hbox.addWidget( sa_left_button_get )
sa_left_button_set = QPushButton("Set Settings")
sa_left_button_set.pressed.connect(self.sa_left_set_callback)
sa_left_button_hbox.addWidget( sa_left_button_set )
sa_left_vbox.addLayout( sa_left_button_hbox )
sa_left_groupbox.setLayout( sa_left_vbox )
vbox.addWidget(sa_left_groupbox)
### Right SA ###
sa_right_groupbox = QGroupBox( "Right SA Camera" )
sa_right_vbox = QVBoxLayout()
sa_right_gain_hbox = QHBoxLayout()
sa_right_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_right_gain_hbox.addWidget( sa_right_gain_label )
self.sa_right_gain = QDoubleSpinBox()
self.sa_right_gain.setSingleStep(.01)
self.sa_right_gain.setRange( 0, 25 )
sa_right_gain_hbox.addWidget( self.sa_right_gain )
sa_right_vbox.addLayout( sa_right_gain_hbox )
sa_right_exp_hbox = QHBoxLayout()
sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_right_exp_hbox.addWidget( sa_right_exp_label )
self.sa_right_exp = QDoubleSpinBox()
self.sa_right_exp.setSingleStep(.01)
self.sa_right_exp.setRange( 0, 25 )
sa_right_exp_hbox.addWidget( self.sa_right_exp )
sa_right_vbox.addLayout( sa_right_exp_hbox )
sa_right_button_hbox = QHBoxLayout()
sa_right_button_hbox.addStretch(1)
sa_right_button_get = QPushButton("Get Settings")
sa_right_button_get.pressed.connect(self.sa_right_get_callback)
#sa_right_button_hbox.addWidget( sa_right_button_get )
sa_right_button_set = QPushButton("Set Settings")
sa_right_button_set.pressed.connect(self.sa_right_set_callback)
sa_right_button_hbox.addWidget( sa_right_button_set )
sa_right_vbox.addLayout( sa_right_button_hbox )
sa_right_groupbox.setLayout( sa_right_vbox )
vbox.addWidget(sa_right_groupbox)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10)
self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10)
self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10)
self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10)
self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)
# self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
def shutdown_plugin(self):
print "Shutdown ..."
# self.param_pub.unregister()
print "Done!"
def ms_get_callback(self):
print "fill values"
def ms_set_callback(self):
ms_cam = Float64MultiArray()
ms_cam_dim_h = MultiArrayDimension()
ms_cam_dim_h.label = "height"
ms_cam_dim_h.size = 1
ms_cam_dim_h.stride = 2
ms_cam.layout.dim.append( ms_cam_dim_h )
ms_cam_dim_w = MultiArrayDimension()
ms_cam_dim_w.label = "width"
ms_cam_dim_w.size = 2
ms_cam_dim_w.stride = 2
ms_cam.layout.dim.append(ms_cam_dim_w)
ms_cam.data.append( self.ms_gain.value() )
if self.ms_exp_auto.isChecked() :
ms_cam.data.append( 1 )
else:
ms_cam.data.append( self.ms_exp.value() )
self.ms_cam_pub.publish( ms_cam )
light = Float64()
light.data = self.ms_light.value()/100.0
self.ms_light_pub.publish( light )
spindle_speed = Float64()
spindle_speed.data = self.ms_spindle.value()
self.ms_spindle_pub.publish( spindle_speed )
def sa_left_get_callback(self):
print "fill values"
def sa_left_set_callback(self):
sa_left_cam = Float64MultiArray()
sa_left_cam_dim_h = MultiArrayDimension()
sa_left_cam_dim_h.label = "height"
sa_left_cam_dim_h.size = 1
sa_left_cam_dim_h.stride = 2
sa_left_cam.layout.dim.append( sa_left_cam_dim_h )
sa_left_cam_dim_w = MultiArrayDimension()
sa_left_cam_dim_w.label = "width"
sa_left_cam_dim_w.size = 2
sa_left_cam_dim_w.stride = 2
sa_left_cam.layout.dim.append(sa_left_cam_dim_w)
sa_left_cam.data.append( self.sa_left_gain.value() )
sa_left_cam.data.append( self.sa_left_exp.value() )
self.sa_left_cam_pub.publish( sa_left_cam )
def sa_right_get_callback(self):
print "fill values"
def sa_right_set_callback(self):
sa_right_cam = Float64MultiArray()
sa_right_cam_dim_h = MultiArrayDimension()
sa_right_cam_dim_h.label = "height"
sa_right_cam_dim_h.size = 1
sa_right_cam_dim_h.stride = 2
sa_right_cam.layout.dim.append( sa_right_cam_dim_h )
sa_right_cam_dim_w = MultiArrayDimension()
sa_right_cam_dim_w.label = "width"
sa_right_cam_dim_w.size = 2
sa_right_cam_dim_w.stride = 2
sa_right_cam.layout.dim.append(sa_right_cam_dim_w)
sa_right_cam.data.append( self.sa_right_gain.value() )
sa_right_cam.data.append( self.sa_right_exp.value() )
self.sa_right_cam_pub.publish( sa_right_cam )
class ArmGUI(Plugin):
joint_sig = Signal(JointState)
def __init__(self, context):
super(ArmGUI, self).__init__(context)
self.setObjectName('ArmGUI')
self._widget = QWidget()
print "init"
# Action/service/message clients or servers
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
# saving our poses
self.saved_r_arm_poses = defaultdict(partial(list))
self.saved_l_arm_poses = defaultdict(partial(list))
self.saved_pose_sets = set()
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# controls time between poses
self.time_between_poses = 2.0
large_box = QtGui.QVBoxLayout()
button_box1 = QtGui.QHBoxLayout()
button_box1.addWidget(self.create_button('Relax arms'))
button_box1.addWidget(self.create_button('Freeze arms'))
button_box1.addStretch(1)
large_box.addLayout(button_box1)
large_box.addItem(QtGui.QSpacerItem(100,20))
button_box2 = QtGui.QHBoxLayout()
self.pose_set_text = QtGui.QLineEdit(self._widget)
button_box2.addWidget(self.pose_set_text)
button_box2.addWidget(self.create_button('Add to Pose Set'))
button_box2.addStretch(1)
large_box.addLayout(button_box2)
large_box.addItem(QtGui.QSpacerItem(100,20))
# box with a slider controlling speed robot moves to pose with
speed_box = QtGui.QHBoxLayout()
speed_label = QtGui.QLabel('Pose speed: ')
speed_box.addWidget(speed_label)
speed_box.addWidget(QtGui.QLabel('Slow'))
sldr = QtGui.QSlider(QtCore.Qt.Horizontal)
sldr.setSliderPosition(50)
speed_box.addWidget(sldr)
#sldr.setGeometry(50, 50, 130, 30);
sldr.valueChanged[int].connect(self.set_pose_speed)
speed_box.addWidget(QtGui.QLabel('Fast'))
speed_box.addStretch(1)
large_box.addLayout(speed_box)
button_box3 = QtGui.QHBoxLayout()
self.pose_selector = QtGui.QComboBox()
self.pose_selector.setSizeAdjustPolicy(QtGui.QComboBox.AdjustToContents)
self.pose_selector.currentIndexChanged[str].connect(self.update_pose_set_length)
button_box3.addWidget(self.pose_selector)
self.pose_set_length_label = QtGui.QLabel()
button_box3.addWidget(self.pose_set_length_label)
self.update_pose_set_length()
button_box3.addWidget(self.create_button('Do Pose Set'))
button_box3.addItem(QtGui.QSpacerItem(50,20))
button_box3.addWidget(self.create_button('Delete Pose Set'))
button_box3.addStretch(1)
large_box.addLayout(button_box3)
large_box.addItem(QtGui.QSpacerItem(100,20))
poses_box = QtGui.QVBoxLayout()
self.status_message_label = QtGui.QLabel('No Saved Poses')
poses_box.addWidget(self.status_message_label)
large_box.addLayout(poses_box)
large_box.addStretch(1)
self._widget.setObjectName('ArmGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
rospy.loginfo('GUI initialization complete.')
# currently maps from a quarter second between poses to ~3.5 seconds;
# pos comes in as the slider value which ranges from 0 to 100. it
# seems like even with the delay set to 0, there are automatic
# constraints, but to be safe the quarter second is added. there also
# seem to be poses that are intrinsically hard for the robot to move
# between, causing slow movement even with a low delay set.
def set_pose_speed(self, pos):
self.time_between_poses = (100 - pos) / 30 + .25
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(self.command_cb)
return btn
# This is a really janky way of doing this, you should check
# self._widget.sender() to figure out where the event originated.
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax arms'):
self.relax_arm('r')
self.relax_arm('l')
elif (button_name == 'Freeze arms'):
self.freeze_arm('r')
self.freeze_arm('l')
elif (button_name == 'Add to Pose Set'):
self.save_pose()
elif (button_name == 'Do Pose Set'):
self.move_arm()
elif (button_name == 'Delete Pose Set'):
self.delete_pose()
self.update_pose_set_length()
def save_pose(self):
pose_set = self.pose_set_text.text()
if len(pose_set) is 0 or pose_set is None:
self.status_message_label.setText("Invalid pose name")
return
# if not already saved, make a new pose set
if pose_set not in self.saved_pose_sets:
self.pose_selector.addItem(pose_set)
self.saved_pose_sets.add(pose_set)
# auto select the saved pose
index = self.pose_selector.findText(pose_set)
self.pose_selector.setCurrentIndex(index)
r_joint_state = self.get_joint_state('r')
l_joint_state = self.get_joint_state('l')
self.saved_r_arm_poses[pose_set].append(r_joint_state)
self.saved_l_arm_poses[pose_set].append(l_joint_state)
self.saved_pose_sets.add(pose_set)
self.status_message_label.setText('Pose saved!')
def update_pose_set_length(self):
pose_set = self.pose_selector.currentText()
text = "(%d)" % len(self.saved_r_arm_poses[pose_set])
self.pose_set_length_label.setText(text)
def move_arm(self):
pose_set = self.pose_selector.currentText()
if not pose_set in self.saved_pose_sets:
rospy.logerr("Target pose set does not exist, I cannot move my little arms!")
self.status_message_label.setText("No pose set")
return
if pose_set in self.saved_r_arm_poses:
self.freeze_arm('r')
self.move_to_joints('r', self.saved_r_arm_poses[pose_set], self.time_between_poses)
if pose_set in self.saved_l_arm_poses:
self.freeze_arm('l')
self.move_to_joints('l', self.saved_l_arm_poses[pose_set], self.time_between_poses)
self.status_message_label.setText('Pose executing!')
def delete_pose(self):
pose_set = self.pose_selector.currentText()
rospy.loginfo('Clearing pose set %s' % pose_set)
# removing from the select box
pose_set_len = len(self.saved_r_arm_poses[pose_set])
index = self.pose_selector.findText(pose_set)
self.pose_selector.removeItem(index)
# removing from the pose set maps
self.saved_r_arm_poses.pop(pose_set, None)
self.saved_l_arm_poses.pop(pose_set, None)
self.saved_pose_sets.remove(pose_set)
self.status_message_label.setText('Pose deleted!')
# unsure if the time_to_joint input is still necessary
# with self.time_between_poses
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
time_move = time_to_joint + 3
print "using following positions %s" % positions
for pose in positions:
velocities = [0] * len(pose)
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose,
velocities=velocities, time_from_start=rospy.Duration(time_move)))
time_move = time_move + time_to_joint
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
instance_settings.set_value("r_arm_poses", self.saved_r_arm_poses)
instance_settings.set_value("l_arm_poses", self.saved_l_arm_poses)
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
r_arm_poses = instance_settings.value("r_arm_poses")
if not r_arm_poses is None:
self.saved_r_arm_poses = r_arm_poses
for pose in self.saved_r_arm_poses.keys():
if not pose is None and pose != "":
self.pose_selector.addItem(pose)
self.saved_pose_sets.add(pose)
l_arm_poses = instance_settings.value("l_arm_poses")
if not l_arm_poses is None:
self.saved_l_arm_poses = l_arm_poses
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
#Sound textbox
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text
sound_textbox.setFixedWidth(450)
#Set a handle on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet.')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
up_head = Head(Head.UP)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
#large_box.addWidget(up_head_button)
down_head = Head(Head.DOWN)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
#large_box.addWidget(down_head_button)
right_head = Head(Head.RIGHT)
right_head_button = self.create_button('>',
right_head.create_closure())
#large_box.addWidget(right_head_button)
left_head = Head(Head.LEFT)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
#large_box.addItem(QtGui.QSpacerItem(500,20))
#large_box.addWidget(left_head_button)
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN)
right_gripper = self.create_button('Right gripper!',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN)
left_gripper = self.create_button('Left gripper!',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
#forward
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
#large_box.addWidget(forward_base_button)
#left
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT)
left_base_button = self.create_button('move left',
left_base.create_closure())
#large_box.addWidget(left_base_button)
#right
right_base = Base(Base.RIGHT)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
#large_box.addWidget(right_base_button)
#backward
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
#large_box.addWidget(backward_base_button)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
#turn left
turnleft_base = Base(Base.TURNLEFT)
turnleft_base_button = self.create_button(
' /\n<--', turnleft_base.create_closure())
#large_box.addWidget(turnleft_base_button)
#turn right
turnright_base = Base(Base.TURNRIGHT)
turnright_base_button = self.create_button(
'\\\n -->', turnright_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(turnright_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(turnleft_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
#large_box.addWidget(turnright_base_button)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
"/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg"
)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def __init__(self, context):
super(SteeringInterfaceDialog, self).__init__(context)
self.setObjectName('SteeringInterfaceDialog')
self.time_step = rospy.get_param('~time_step', 1.0)
self.listener = tf.TransformListener()
# Assuming right arm control for now
self.r_arm_pub = rospy.Publisher('/flor/r_arm_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.r_leg_pub = rospy.Publisher('/flor/r_leg_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.markers_pub = rospy.Publisher('/flor_footstep_planner/footsteps_array',MarkerArray, None, False, True, None, queue_size=10)
self.r_leg_state = RightLegState()
self.r_arm_state = RightArmState()
# External data coming in (current joint states, and template pose (utility vehicle model)
self.joint_data = JointState()
self.template_selection = TemplateSelection()
self.joints_sub = rospy.Subscriber('/atlas/joint_states',JointState, self.joint_states_callback )
self.template_sub = rospy.Subscriber('/template/template_selected',TemplateSelection, self.template_selection_callback )
# ------------------------------------------------------------------------------------
# TODO--- Define these transforms relative to the origin of the large polaris mesh
#Name X Y Z
#Parking Break -0.07 0.48 -0.87
#Parking Break Free -0.07 0.53 -0.85
#FNR Switch 0.02 0.56 -0.90
#FNR Switch F 0.02 0.60 -0.90
#FNR Switch R 0.02 0.55 -0.95
#Gas Pedal -0.10 0.60 -1.50
#Gas Pedal Half -0.10 0.62 -1.53
#Gas Pedal Full -0.10 0.65 -1.56
#Break Pedal -0.27 0.60 -1.50
#Break Pedal Slow -0.27 0.63 -1.53
#Break Pedal Stop -0.27 0.66 -1.55
# Transform brake and throttle to car template (static transforms relative to the car)
# leg
self._c_T_b = [-0.27, 0.60,-1.50] # (b)rake
self._c_T_b_slow = [-0.27, 0.63,-1.53]
self._c_T_b_stop = [-0.27, 0.66,-1.55]
self._c_T_t = [-0.10, 0.60,-1.50] # (t)hrottle
self._c_T_t_slow = [-0.10, 0.62,-1.53]
self._c_T_t_fast = [-0.10, 0.65,-1.56]
self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
# need to add them in order for the transforms to work
#FAST = 0 self._c_T_t_fast = [-0.10, 0.65,-1.56]
#SLOW = 1 self._c_T_t_slow = [-0.10, 0.62,-1.53]
#NEUTRAL = 2 self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
#BRAKE = 3 self._c_T_b_slow = [-0.27, 0.63,-1.53]
#STOP = 4 self._c_T_b_stop = [-0.27, 0.66,-1.55]
self.r_leg_state._c_T_target_positions.append(self._c_T_t_fast)
self.r_leg_state._c_T_target_positions.append(self._c_T_t_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_n)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_stop)
# arm
self._c_T_hl = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_l = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_fwd = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_r = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_hr = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
self._c_T_p_free = [-0.07, 0.53,-0.85]
self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
self._c_T_f_f = [ 0.02, 0.60,-0.90]
self._c_T_f_r = [ 0.02, 0.55,-0.95]
# need to add them in order
#HLEFT = 0
#LEFT = 1
#FWD = 2
#RIGHT = 3
#HRIGHT = 4
#PRE_FORWARD = 5 self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
#FORWARD = 6 self._c_T_f_f = [ 0.02, 0.60,-0.90]
#REVERSE = 7 self._c_T_f_r = [ 0.02, 0.55,-0.95]
#PRE_PARKING = 8 self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
#PARKING = 9 self._c_T_p_free = [-0.07, 0.53,-0.85]
self.r_arm_state._c_T_target_positions.append(self._c_T_hl)
self.r_arm_state._c_T_target_positions.append(self._c_T_l)
self.r_arm_state._c_T_target_positions.append(self._c_T_fwd)
self.r_arm_state._c_T_target_positions.append(self._c_T_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_hr)
self.r_arm_state._c_T_target_positions.append(self._c_T_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_p)
self.r_arm_state._c_T_target_positions.append(self._c_T_p_free)
# ---------------------------------------------------------------------------------
# Widget layout stuff
# -----------------------------------------------------------------------
self._widget = QWidget()
vbox = QVBoxLayout()
top_widget = QWidget()
top_hbox = QHBoxLayout()
# top row
hard_left_command = QPushButton("H L")
hard_left_command.clicked.connect(self.hard_left_command_callback)
top_hbox.addWidget(hard_left_command)
left_command = QPushButton("Left")
left_command.clicked.connect(self.left_command_callback)
top_hbox.addWidget(left_command)
straight_command = QPushButton("Fwd")
straight_command.clicked.connect(self.straight_command_callback)
top_hbox.addWidget(straight_command)
right_command = QPushButton("Right")
right_command.clicked.connect(self.right_command_callback)
top_hbox.addWidget(right_command)
hard_right_command = QPushButton("H R")
hard_right_command.clicked.connect(self.hard_right_command_callback)
top_hbox.addWidget(hard_right_command)
top_widget.setLayout(top_hbox)
vbox.addWidget(top_widget)
# Center column
bottom_widget = QWidget()
bottom_hbox = QHBoxLayout()
calc_widget = QWidget()
calc_vbox = QVBoxLayout()
calc_vbox.addStretch()
calc_command = QPushButton("Calc")
calc_command.clicked.connect(self.calc_command_callback)
calc_vbox.addWidget(calc_command)
calc_widget.setLayout(calc_vbox)
bottom_hbox.addWidget(calc_widget)
throttle_widget = QWidget()
throttle_vbox = QVBoxLayout()
fast_command = QPushButton("Fast")
fast_command.clicked.connect(self.fast_command_callback)
throttle_vbox.addWidget(fast_command)
slow_command = QPushButton("Slow")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
neutral_command = QPushButton("Neutral")
neutral_command.clicked.connect(self.neutral_command_callback)
throttle_vbox.addWidget(neutral_command)
slow_command = QPushButton("Brake")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
throttle_vbox.addWidget(stop_command)
throttle_widget.setLayout(throttle_vbox)
bottom_hbox.addWidget(throttle_widget)
right_widget = QWidget()
right_vbox = QVBoxLayout()
right_vbox.addStretch()
parking_command = QPushButton("Parking")
parking_command.clicked.connect(self.parking_command_callback)
right_vbox.addWidget(parking_command)
go_button_command = QPushButton("Fwd")
go_button_command.clicked.connect(self.go_button_command_callback)
right_vbox.addWidget(go_button_command)
reverse_button_command = QPushButton("R")
reverse_button_command.clicked.connect(self.reverse_button_command_callback)
right_vbox.addWidget(reverse_button_command)
right_widget.setLayout(right_vbox)
bottom_hbox.addWidget(right_widget)
bottom_widget.setLayout(bottom_hbox)
vbox.addWidget(bottom_widget)
self._widget.setLayout(vbox)
# --------------------------------------------------- End Widget stuff -------------------------------
context.add_widget(self._widget)
class SteeringInterfaceDialog(Plugin):
def __init__(self, context):
super(SteeringInterfaceDialog, self).__init__(context)
self.setObjectName('SteeringInterfaceDialog')
self.time_step = rospy.get_param('~time_step', 1.0)
self.listener = tf.TransformListener()
# Assuming right arm control for now
self.r_arm_pub = rospy.Publisher('/flor/r_arm_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.r_leg_pub = rospy.Publisher('/flor/r_leg_controller/trajectory',JointTrajectory, None, False, True, None, queue_size=10)
self.markers_pub = rospy.Publisher('/flor_footstep_planner/footsteps_array',MarkerArray, None, False, True, None, queue_size=10)
self.r_leg_state = RightLegState()
self.r_arm_state = RightArmState()
# External data coming in (current joint states, and template pose (utility vehicle model)
self.joint_data = JointState()
self.template_selection = TemplateSelection()
self.joints_sub = rospy.Subscriber('/atlas/joint_states',JointState, self.joint_states_callback )
self.template_sub = rospy.Subscriber('/template/template_selected',TemplateSelection, self.template_selection_callback )
# ------------------------------------------------------------------------------------
# TODO--- Define these transforms relative to the origin of the large polaris mesh
#Name X Y Z
#Parking Break -0.07 0.48 -0.87
#Parking Break Free -0.07 0.53 -0.85
#FNR Switch 0.02 0.56 -0.90
#FNR Switch F 0.02 0.60 -0.90
#FNR Switch R 0.02 0.55 -0.95
#Gas Pedal -0.10 0.60 -1.50
#Gas Pedal Half -0.10 0.62 -1.53
#Gas Pedal Full -0.10 0.65 -1.56
#Break Pedal -0.27 0.60 -1.50
#Break Pedal Slow -0.27 0.63 -1.53
#Break Pedal Stop -0.27 0.66 -1.55
# Transform brake and throttle to car template (static transforms relative to the car)
# leg
self._c_T_b = [-0.27, 0.60,-1.50] # (b)rake
self._c_T_b_slow = [-0.27, 0.63,-1.53]
self._c_T_b_stop = [-0.27, 0.66,-1.55]
self._c_T_t = [-0.10, 0.60,-1.50] # (t)hrottle
self._c_T_t_slow = [-0.10, 0.62,-1.53]
self._c_T_t_fast = [-0.10, 0.65,-1.56]
self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
# need to add them in order for the transforms to work
#FAST = 0 self._c_T_t_fast = [-0.10, 0.65,-1.56]
#SLOW = 1 self._c_T_t_slow = [-0.10, 0.62,-1.53]
#NEUTRAL = 2 self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
#BRAKE = 3 self._c_T_b_slow = [-0.27, 0.63,-1.53]
#STOP = 4 self._c_T_b_stop = [-0.27, 0.66,-1.55]
self.r_leg_state._c_T_target_positions.append(self._c_T_t_fast)
self.r_leg_state._c_T_target_positions.append(self._c_T_t_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_n)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_slow)
self.r_leg_state._c_T_target_positions.append(self._c_T_b_stop)
# arm
self._c_T_hl = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_l = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_fwd = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_r = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_hr = [ 0.00, 0.00, 0.00] # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< NEED THESE POSITIONS
self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
self._c_T_p_free = [-0.07, 0.53,-0.85]
self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
self._c_T_f_f = [ 0.02, 0.60,-0.90]
self._c_T_f_r = [ 0.02, 0.55,-0.95]
# need to add them in order
#HLEFT = 0
#LEFT = 1
#FWD = 2
#RIGHT = 3
#HRIGHT = 4
#PRE_FORWARD = 5 self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
#FORWARD = 6 self._c_T_f_f = [ 0.02, 0.60,-0.90]
#REVERSE = 7 self._c_T_f_r = [ 0.02, 0.55,-0.95]
#PRE_PARKING = 8 self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
#PARKING = 9 self._c_T_p_free = [-0.07, 0.53,-0.85]
self.r_arm_state._c_T_target_positions.append(self._c_T_hl)
self.r_arm_state._c_T_target_positions.append(self._c_T_l)
self.r_arm_state._c_T_target_positions.append(self._c_T_fwd)
self.r_arm_state._c_T_target_positions.append(self._c_T_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_hr)
self.r_arm_state._c_T_target_positions.append(self._c_T_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_f)
self.r_arm_state._c_T_target_positions.append(self._c_T_f_r)
self.r_arm_state._c_T_target_positions.append(self._c_T_p)
self.r_arm_state._c_T_target_positions.append(self._c_T_p_free)
# ---------------------------------------------------------------------------------
# Widget layout stuff
# -----------------------------------------------------------------------
self._widget = QWidget()
vbox = QVBoxLayout()
top_widget = QWidget()
top_hbox = QHBoxLayout()
# top row
hard_left_command = QPushButton("H L")
hard_left_command.clicked.connect(self.hard_left_command_callback)
top_hbox.addWidget(hard_left_command)
left_command = QPushButton("Left")
left_command.clicked.connect(self.left_command_callback)
top_hbox.addWidget(left_command)
straight_command = QPushButton("Fwd")
straight_command.clicked.connect(self.straight_command_callback)
top_hbox.addWidget(straight_command)
right_command = QPushButton("Right")
right_command.clicked.connect(self.right_command_callback)
top_hbox.addWidget(right_command)
hard_right_command = QPushButton("H R")
hard_right_command.clicked.connect(self.hard_right_command_callback)
top_hbox.addWidget(hard_right_command)
top_widget.setLayout(top_hbox)
vbox.addWidget(top_widget)
# Center column
bottom_widget = QWidget()
bottom_hbox = QHBoxLayout()
calc_widget = QWidget()
calc_vbox = QVBoxLayout()
calc_vbox.addStretch()
calc_command = QPushButton("Calc")
calc_command.clicked.connect(self.calc_command_callback)
calc_vbox.addWidget(calc_command)
calc_widget.setLayout(calc_vbox)
bottom_hbox.addWidget(calc_widget)
throttle_widget = QWidget()
throttle_vbox = QVBoxLayout()
fast_command = QPushButton("Fast")
fast_command.clicked.connect(self.fast_command_callback)
throttle_vbox.addWidget(fast_command)
slow_command = QPushButton("Slow")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
neutral_command = QPushButton("Neutral")
neutral_command.clicked.connect(self.neutral_command_callback)
throttle_vbox.addWidget(neutral_command)
slow_command = QPushButton("Brake")
slow_command.clicked.connect(self.slow_command_callback)
throttle_vbox.addWidget(slow_command)
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
throttle_vbox.addWidget(stop_command)
throttle_widget.setLayout(throttle_vbox)
bottom_hbox.addWidget(throttle_widget)
right_widget = QWidget()
right_vbox = QVBoxLayout()
right_vbox.addStretch()
parking_command = QPushButton("Parking")
parking_command.clicked.connect(self.parking_command_callback)
right_vbox.addWidget(parking_command)
go_button_command = QPushButton("Fwd")
go_button_command.clicked.connect(self.go_button_command_callback)
right_vbox.addWidget(go_button_command)
reverse_button_command = QPushButton("R")
reverse_button_command.clicked.connect(self.reverse_button_command_callback)
right_vbox.addWidget(reverse_button_command)
right_widget.setLayout(right_vbox)
bottom_hbox.addWidget(right_widget)
bottom_widget.setLayout(bottom_hbox)
vbox.addWidget(bottom_widget)
self._widget.setLayout(vbox)
# --------------------------------------------------- End Widget stuff -------------------------------
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutting down ..."
self.r_arm_pub.unregister()
self.r_leg_pub.unregister()
self.joints_sub.unregister()
self.template_sub.unregister()
print "Done!"
def joint_states_callback(self,data):
# Store the latest joint data
self.joint_states = data
# Update structures of the state machines
self.r_leg_state.current_joint_positions = []
for k in self.r_leg_state.joint_to_osrf.keys():
self.r_leg_state.current_joint_positions.append(self.joint_states.position[self.r_leg_state.joint_to_osrf[k]])
self.r_arm_state.current_joint_positions = []
for k in self.r_arm_state.joint_to_osrf.keys():
self.r_arm_state.current_joint_positions.append(self.joint_states.position[self.r_arm_state.joint_to_osrf[k]])
#print "Updated joint state data at ",self.joint_states.header.stamp.to_sec()
def template_selection_callback(self, template):
self.template_selection = template
print "Updated template selection with ",self.template_selection.pose
def publish_trajectory(self, pub, joint_traj_positions):
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.points = [JointTrajectoryPoint() for p in joint_traj_positions]
for i,position in enumerate(joint_traj_positions):
trajectory.points[i].positions = joint_traj_positions[i]
trajectory.points[i].velocities = 0.0
trajectory.points[i].time_from_start = rospy.Duration(i*self.time_step)
print "Publishing trajectory at ",trajectory.header.stamp
pub.publish(trajectory)
# Define system command strings
def hard_left_command_callback(self):
# Define trajectory from current point positions to hard right target
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.HLEFT)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send hard left command:",trajectory_positions
def left_command_callback(self):
# Define trajectory from current point positions to left target
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.LEFT)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send left command:",trajectory_positions
def straight_command_callback(self):
# Define trajectory from current point positions to straight steering target
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.FWD)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send forward command:",trajectory_positions
def right_command_callback(self):
# Define trajectory from current point positions to right target
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.RIGHT)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send right command:",trajectory_positions
def hard_right_command_callback(self):
# Define trajectory from current point positions to hard right target
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.HRIGHT)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send hard right command:",trajectory_positions
def fast_command_callback(self):
# This defines trajectory for moving leg through neutral position
# to apply light throttle
trajectory_positions = self.r_arm_state.get_trajectory_points(RightLegState.FAST)
self.publish_trajectory(self.r_leg_pub, trajectory_positions)
print "Send fast command:",trajectory_positions
def slow_command_callback(self):
# This defines trajectory for moving leg through neutral position
# to apply light throttle
trajectory_positions = self.r_arm_state.get_trajectory_points(RightLegState.SLOW)
self.publish_trajectory(self.r_leg_pub, trajectory_positions)
print "Send slow command:",trajectory_positions
def neutral_command_callback(self):
# This defines trajectory for moving leg to neutral position
# just needs two points, current and neutral position
trajectory_positions = self.r_arm_state.get_trajectory_points(RightLegState.NEUTRAL)
self.publish_trajectory(self.r_leg_pub, trajectory_positions)
print "Send neutral command:",trajectory_positions
def brake_command_callback(self):
# This slight braking trajectory if not currently applying brake
# if currently in throttle, move joints to neutral position, then to foot on brake position
# generate a 3 point trajectory for right leg from current --> neutral --> braking
trajectory_positions = self.r_arm_state.get_trajectory_points(RightLegState.BRAKE)
self.publish_trajectory(self.r_leg_pub, trajectory_positions)
print "Send brake command:",trajectory_positions
def stop_command_callback(self):
# This defines stomp on brake trajectory if not currently applying brake
# if currently in throttle, move joints to neutral position, then full stop position
# generate a 3 point trajectory for right leg from current --> neutral --> full stop
trajectory_positions = self.r_arm_state.get_trajectory_points(RightLegState.STOP)
self.publish_trajectory(self.r_leg_pub, trajectory_positions)
print "Send fast command:",trajectory_positions
def go_button_command_callback(self):
# this should publish the pose of go button as target for moveit planner
# can plan online
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.FORWARD)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send go button command:",trajectory_positions
def reverse_button_command_callback(self):
# this should publish the pose of go button as target for moveit planner
# can plan online
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.REVERSE)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send go button command:",trajectory_positions
def parking_command_callback(self):
# this should publish the pose of parking brake as target for grasping/moveit planner
# can plan online
trajectory_positions = self.r_arm_state.get_trajectory_points(RightArmState.PARKING)
self.publish_trajectory(self.r_arm_pub, trajectory_positions)
print "Send parking command:",trajectory_positions
def calc_command_callback(self):
print "Calculate joint targets = "
# Get robot pose in world (
# wTr =
#now = rospy.Time.now()
#self.listener.waitForTransform("/world", "/pelvis", now, rospy.Duration(4.0))
#w_T_r = self.listener.lookupTransform("/world", "/pelvis", now)
#print w_T_r
# Get vehicle pose in world from Template selection message
# wTc =
# this is already in tf
#Should be:
#self.listener.waitForTransform("/world", "/car", now, rospy.Duration(4.0)) #TODO change /car with the right frame name
#w_T_c = self.listener.lookupTransform("/world", "/car", now)
#print w_T_c
# initialize joint positions structure
print "setting the transforms for moveit"
self.r_leg_state.joint_positions = RightLegState.NUM_STATES*[len(self.r_leg_state.joint_to_osrf.keys())*[0.0]]
self.r_arm_state.joint_positions = RightArmState.NUM_STATES*[len(self.r_arm_state.joint_to_osrf.keys())*[0.0]]
# For each transform, call to moveit to generate
# Joint positions for each target relative to current robot pose
#
# EXAMPLE
# Brake position in world
# wTb = wTc * cTb (static pose)
# ATTENTION: THIS WILL ONLY WORK IF THERE IS A TEMPLATE IN THE OCS AND THE TEMPLATE NODELET IS RUNNING
#RightLegState:
#FAST = 0 self._c_T_t_fast = [-0.10, 0.65,-1.56]
#SLOW = 1 self._c_T_t_slow = [-0.10, 0.62,-1.53]
#NEUTRAL = 2 self._c_T_n = [-0.185,0.60,-1.50] # (n)eutral // in between neutral gas/brake
#BRAKE = 3 self._c_T_b_slow = [-0.27, 0.63,-1.53]
#STOP = 4 self._c_T_b_stop = [-0.27, 0.66,-1.55]
cube = Point()
cube.x=cube.y=cube.z=0.045
markers = MarkerArray()
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/world"
marker.action = 0#Marker.ADD
marker.scale = cube
marker.type = 6#Marker.CUBE_LIST
marker.ns = "test"
wTc = posemath.fromMsg(self.template_selection.pose.pose)
for i,position in enumerate(self.r_leg_state._c_T_target_positions):
tmp = PoseStamped()
tmp.header.stamp = rospy.Time.now()
#tmp.header.frame_id = "/template_tf_"+str(self.template_selection.template_id.data)
tmp.pose.position.x = position[0]
tmp.pose.position.y = position[1]
tmp.pose.position.z = position[2]
tmp.pose.orientation.w = 1.0
tmp.pose.orientation.x = 0.0
tmp.pose.orientation.y = 0.0
tmp.pose.orientation.z = 0.0
#transformed_pose = self.listener.transformPose("/world",tmp)
f1 = posemath.fromMsg(tmp.pose)
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_r[1][0],w_T_r[1][1],w_T_r[1][2],w_T_r[1][3]),PyKDL.Vector(w_T_r[0][0],w_T_r[0][1],w_T_r[0][2]))
#Should be:
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_c[1][0],w_T_c[1][1],w_T_c[1][2],w_T_c[1][3]),PyKDL.Vector(w_T_c[0][0],w_T_c[0][1],w_T_c[0][2]))
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_r[1][0],w_T_r[1][1],w_T_r[1][2],w_T_r[1][3]),PyKDL.Vector(0.98,0.0,1.10)) #TOREMOVE this is just an example
#We want w_T_b = w_T_c * c_T_b
f = wTc * f1
tmp.pose = posemath.toMsg(f)
marker.points.append(tmp.pose.position)
marker.colors.append(self.r_leg_state.colors[i])
# call moveit with transformed_pose
#joint_positions = moveit
#self.r_leg_state.joint_positions.append(joint_positions)
#markers.markers.append(marker)
#RightArmState:
#HLEFT = 0
#LEFT = 1
#FWD = 2
#RIGHT = 3
#HRIGHT = 4
#PRE_FORWARD = 5 self._c_T_f = [ 0.02, 0.56,-0.90] # (f)wd_button
#FORWARD = 6 self._c_T_f_f = [ 0.02, 0.60,-0.90]
#REVERSE = 7 self._c_T_f_r = [ 0.02, 0.55,-0.95]
#PRE_PARKING = 8 self._c_T_p = [-0.07, 0.48,-0.87] # (p)arking brake
#PARKING = 9 self._c_T_p_free = [-0.07, 0.53,-0.85]
#marker = Marker()
#marker.header.stamp = rospy.Time.now()
#marker.header.frame_id = "/world"
#marker.action = 0#Marker.ADD
#marker.scale = cube
#marker.color = green
#marker.type = 6#Marker.CUBE_LIST
#marker.ns = "test"
for i,position in enumerate(self.r_arm_state._c_T_target_positions):
tmp = PoseStamped()
tmp.header.stamp = rospy.Time.now()
#tmp.header.frame_id = "/template_tf_"+str(self.template_selection.template_id.data)
tmp.pose.position.x = position[0]
tmp.pose.position.y = position[1]
tmp.pose.position.z = position[2]
tmp.pose.orientation.w = 1.0
tmp.pose.orientation.x = 0.0
tmp.pose.orientation.y = 0.0
tmp.pose.orientation.z = 0.0
#transformed_pose = self.listener.transformPose("/world",tmp)
f1 = posemath.fromMsg(tmp.pose)
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_r[1][0],w_T_r[1][1],w_T_r[1][2],w_T_r[1][3]),PyKDL.Vector(w_T_r[0][0],w_T_r[0][1],w_T_r[0][2]))
#Should be:
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_c[1][0],w_T_c[1][1],w_T_c[1][2],w_T_c[1][3]),PyKDL.Vector(w_T_c[0][0],w_T_c[0][1],w_T_c[0][2]))
#f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(w_T_r[1][0],w_T_r[1][1],w_T_r[1][2],w_T_r[1][3]),PyKDL.Vector(0.98,0.0,1.10)) #TOREMOVE this is just an example
#We want w_T_b = w_T_c * c_T_b
f = wTc * f1
tmp.pose = posemath.toMsg(f)
marker.points.append(tmp.pose.position)
marker.colors.append(self.r_arm_state.colors[i])
# call moveit with
#joint_positions = moveit
#self.r_arm_state.joint_positions.append(joint_positions)
markers.markers.append(marker)
self.markers_pub.publish(markers)
print self.r_leg_state.joint_positions
print self.r_arm_state.joint_positions
class ImageSnapShotGUI(Plugin):
def __init__(self, context):
super(ImageSnapShotGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('ImageSnapShotGUI')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
self._toolbar = QToolBar()
self._toolbar.addWidget(QLabel('ImageSnapShot'))
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Add a button for ....
self._go_button = QPushButton('Head')
self._go_button.clicked.connect(self._head)
self._layout.addWidget(self._go_button)
self._clear_button = QPushButton('Larm')
self._clear_button.clicked.connect(self._larm)
self._layout.addWidget(self._clear_button)
self._clear_button = QPushButton('Rarm')
self._clear_button.clicked.connect(self._rarm)
self._layout.addWidget(self._clear_button)
self._clear_button = QPushButton('L FishEye')
self._clear_button.clicked.connect(self._lfisheye)
self._layout.addWidget(self._clear_button)
self._clear_button = QPushButton('R FishEye')
self._clear_button.clicked.connect(self._rfisheye)
self._layout.addWidget(self._clear_button)
# self._step_run_button.setStyleSheet('QPushButton {color: black}')
context.add_widget(self._container)
self._head_pub = rospy.Publisher('/head_snap/snapshot', std_msgs.msg.Empty)
self._lhand_pub = rospy.Publisher('/lhand_snap/snapshot', std_msgs.msg.Empty)
self._rhand_pub = rospy.Publisher('/rhand_snap/snapshot', std_msgs.msg.Empty)
self._lfisheye_pub = rospy.Publisher('/lfisheye_snap/snapshot', std_msgs.msg.Empty)
self._rfisheye_pub = rospy.Publisher('/rfisheye_snap/snapshot', std_msgs.msg.Empty)
def _head(self):
#go = rospy.ServiceProxy('/head_snap/snapshot', std_srvs.srv.Empty)
#go()
self._head_pub.publish(std_msgs.msg.Empty())
def _larm(self):
#go = rospy.ServiceProxy('/lhand_snap/snapshot', std_srvs.srv.Empty)
#go()
self._lhand_pub.publish(std_msgs.msg.Empty())
def _rarm(self):
#go = rospy.ServiceProxy('/rhand_snap/snapshot', std_srvs.srv.Empty)
#go()
self._rhand_pub.publish(std_msgs.msg.Empty())
def _lfisheye(self):
#go = rospy.ServiceProxy('/lfisheye_snap/snapshot', std_srvs.srv.Empty)
#go()
self._lfisheye_pub.publish(std_msgs.msg.Empty())
def _rfisheye(self):
#go = rospy.ServiceProxy('/rfisheye_snap/snapshot', std_srvs.srv.Empty)
#go()
self._rfisheye_pub.publish(std_msgs.msg.Empty())
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
combo = QComboBox()
combo.setEditable(True)
combo_completer = TopicCompleter(combo)
#combo_completer.setCompletionMode(QCompleter.InlineCompletion)
combo.lineEdit().setCompleter(combo_completer)
model_tree = QTreeView()
model_tree.setModel(combo_completer.model())
model_tree.expandAll()
for column in range(combo_completer.model().columnCount()):
model_tree.resizeColumnToContents(column)
completion_tree = QTreeView()
completion_tree.setModel(combo_completer.completionModel())
completion_tree.expandAll()
for column in range(combo_completer.completionModel().columnCount()):
completion_tree.resizeColumnToContents(column)
layout.addWidget(model_tree)
layout.addWidget(completion_tree)
layout.addWidget(edit)
layout.addWidget(combo)
layout.setStretchFactor(model_tree, 1)
widget.setLayout(layout)
mw.setCentralWidget(widget)
mw.move(300, 0)
mw.resize(800, 900)
mw.show()
app.exec_()
class cob_teacher_plugin(Plugin):
plugin_chooser = []
current_teacher = []
def __init__(self, context):
super(cob_teacher_plugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('cob_teacher_plugin')
self._widget = QWidget()
self._widget.setObjectName('cob_teacher_plugin')
self.group_layout = QtGui.QVBoxLayout()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
args = self._parse_args(context.argv())
for config_file in args.config_file:
print config_file
self.ym = YamlManager(config_file)
for field in self.ym.getFields():
self.group_layout.addWidget(self.getFieldWidget(field))
placeholder = QtGui.QWidget()
self.group_layout.addWidget(placeholder, 1)
self.save_header = QtGui.QWidget()
self.save_layout = QtGui.QHBoxLayout()
self.save_header.setLayout(self.save_layout)
self.save_button = QtGui.QPushButton("Save")
self.save_button.connect(self.save_button, QtCore.SIGNAL('clicked()'),
self.saveValues)
self.close_button = QtGui.QPushButton("Close")
self.connect(self.close_button, QtCore.SIGNAL('clicked()'), QtGui.qApp,
QtCore.SLOT('quit()'))
self.save_layout.addWidget(self.save_button)
self.save_layout.addWidget(self.close_button)
self.group_layout.addWidget(self.save_header)
self._widget.setLayout(self.group_layout)
def getFieldWidget(self, field):
group = QtGui.QGroupBox()
group.setTitle("select teacher for: '" + field + "'")
field_layout = QtGui.QVBoxLayout()
group.setLayout(field_layout)
teachers_found = self.findTeachers(field)
if (len(teachers_found) > 1):
print "Several plugins were found for fieldtype " + self.ym.getType(
field)
combo = QtGui.QComboBox()
combo.addItem("")
for teacher in teachers_found:
combo.addItem(teacher().getName())
field_layout.addWidget(combo)
self.plugin_chooser.append({
"name": field,
"layout": field_layout,
"widget": None,
"chooser": combo
})
self.current_teacher.append({"name": field, "teacher": None})
self.connect(combo, QtCore.SIGNAL('activated(QString)'),
self.combo_chosen)
elif (len(teachers_found) == 0):
not_found_widget = QtGui.QLabel("No Plugin found for " +
self.ym.getType(field))
field_layout.addWidget(not_found_widget)
else:
teacher = teachers_found[0]()
self.current_teacher.append({"name": field, "teacher": teacher})
teach_widget = teacher.getRQTWidget(field, self.ym.data[field])
field_layout.addWidget(teach_widget)
return group
def saveValues(self):
for teacher in self.current_teacher:
if (teacher["teacher"] != None):
print "Updating:", teacher["name"]
self.ym.updateField(
teacher["name"],
teacher["teacher"].getRQTData(teacher["name"]))
print "saving values"
self.ym.writeFile()
def combo_chosen(self, text):
sender = self.sender()
for chooser in self.plugin_chooser:
if (chooser["chooser"] == sender):
print "Teacher for", chooser["name"], "?"
for teacher in availableTeachers:
if (teacher().getName() == text):
print "Chosen: ", text
this_teacher = teacher()
if this_teacher.getName() == "PoseTouchupTeacher":
target_frame = ""
for t in self.current_teacher:
if t["name"] == "target_frame":
target_frame = t["teacher"].getRQTData(
t["name"])
teach_widget = this_teacher.getRQTWidget(
chooser["name"], self.ym.data[chooser["name"]],
target_frame)
else:
teach_widget = this_teacher.getRQTWidget(
chooser["name"], self.ym.data[chooser["name"]])
if (teach_widget != None):
#remove currently activated plugin
if (chooser["widget"] != None):
chooser["widget"].setParent(None)
for t in self.current_teacher:
if (t["name"] == chooser["name"]):
t["teacher"] = this_teacher
chooser["widget"] = teach_widget
chooser["layout"].addWidget(teach_widget)
def findTeachers(self, fieldName):
teachers_found = []
for teacher in availableTeachers:
if (teacher().getType() == self.ym.getType(fieldName)):
teachers_found.append(teacher)
return teachers_found
def closeEvent(self, event):
reply = QtGui.QMessageBox.question(self, 'Message',
"Are you sure to quit?",
QtGui.QMessageBox.Yes,
QtGui.QMessageBox.No)
if reply == QtGui.QMessageBox.Yes:
event.accept()
else:
event.ignore()
def _parse_args(self, argv):
parser = argparse.ArgumentParser(prog='cob_teacher', add_help=False)
cob_teacher_plugin.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
group = parser.add_argument_group('Options for cob_teacher plugin')
group.add_argument('config_file',
nargs='*',
default=[],
help='Configfile to load')
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_all.stateChanged.connect(self.updateList)
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x,topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def updateList(self, state, topic = None):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState()==Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit((self.item_all.checkState() == Qt.Checked), self.selected_topics)
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class FootstepTerrainControlDialog(Plugin):
def __init__(self, context):
super(FootstepTerrainControlDialog, self).__init__(context)
self.setObjectName('FootstepTerrainControlDialog')
self.request_seq = 0
self._widget = QWidget()
vbox = QVBoxLayout()
self.cube_vbox = QVBoxLayout()
self.cube_groupbox = QGroupBox( "Terrain Box" )
self.cube_min_hbox = QHBoxLayout()
self.cube_min_label = QLabel("Bottom Left")
self.cube_min_hbox.addWidget( self.cube_min_label )
self.cube_min_x = QDoubleSpinBox()
self.cube_min_x.setSingleStep(.01)
self.cube_min_x.setRange(-10.0, 10.0)
self.cube_min_x.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_x)
self.cube_min_y = QDoubleSpinBox()
self.cube_min_y.setSingleStep(.01)
self.cube_min_y.setRange(-10.0, 10.0)
self.cube_min_y.setValue(-3.0)
self.cube_min_hbox.addWidget(self.cube_min_y)
self.cube_min_z = QDoubleSpinBox()
self.cube_min_z.setSingleStep(.01)
self.cube_min_z.setRange(-10.0, 10.0)
self.cube_min_z.setValue(-1.0)
self.cube_min_hbox.addWidget(self.cube_min_z)
self.cube_vbox.addLayout( self.cube_min_hbox )
self.cube_max_hbox = QHBoxLayout()
self.cube_max_label = QLabel("Top Right")
self.cube_max_hbox.addWidget( self.cube_max_label )
self.cube_max_x = QDoubleSpinBox()
self.cube_max_x.setSingleStep(.01)
self.cube_max_x.setRange(-10.0, 10.0)
self.cube_max_x.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_x)
self.cube_max_y = QDoubleSpinBox()
self.cube_max_y.setSingleStep(.01)
self.cube_max_y.setRange(-10.0, 10.0)
self.cube_max_y.setValue(3.0)
self.cube_max_hbox.addWidget(self.cube_max_y)
self.cube_max_z = QDoubleSpinBox()
self.cube_max_z.setSingleStep(.01)
self.cube_max_z.setRange(-10.0, 10.0)
self.cube_max_z.setValue(1.0)
self.cube_max_hbox.addWidget(self.cube_max_z)
self.cube_vbox.addLayout( self.cube_max_hbox )
self.cube_groupbox.setLayout( self.cube_vbox )
vbox.addWidget( self.cube_groupbox )
self.type_hbox = QHBoxLayout()
# self.type_label = QLabel( "Type" )
# self.type_hbox.addWidget( self.type_label )
#
# self.type_combobox = QComboBox()
#
# self.type_combobox.addItem( "Type 1" )
# self.type_combobox.addItem( "Type 2" )
# self.type_combobox.addItem( "Type 3" )
#
# self.type_hbox.addWidget( self.type_combobox )
#
# vbox.addLayout( self.type_hbox )
self.scan_number_hbox = QHBoxLayout()
self.scan_number_label = QLabel("Number of Scans Used")
self.scan_number_hbox.addWidget( self.scan_number_label )
self.scan_number_val = QDoubleSpinBox()
self.scan_number_val.setDecimals(0)
self.scan_number_val.setRange(0,10000)
self.scan_number_val.setValue(2000)
self.scan_number_hbox.addWidget(self.scan_number_val)
vbox.addLayout( self.scan_number_hbox )
# self.use_terrain_checkbox = QCheckBox( "Use Terrain" )
# vbox.addWidget( self.use_terrain_checkbox )
button_hbox = QHBoxLayout()
# button_get = QPushButton("Get Current Values")
# button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.terrain_pub = rospy.Publisher("/flor/terrain_classifier/generate_terrain_model", TerrainModelRequest, queue_size=10 )
button_submit.pressed.connect(self.sendParams)
# self.terrain_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/terrains', FootstepPlannerParams, self.getParamCallbackFcn)
# button_submit.pressed.connect(self.getParams)
def shutdown_plugin(self):
print "Shutdown ..."
print "Done!"
# def getParams( self ):
# msg = FootstepPlannerParams()
#
#
# self.terrain_pub.publish( msg )
def sendParams( self ):
msg = TerrainModelRequest()
msg.use_default_region_request = False
rr = EnvironmentRegionRequest()
rr.header.seq = self.request_seq
self.request_seq = self.request_seq+1
rr.header.stamp = rospy.get_rostime()
rr.header.frame_id = "/world"
rr.bounding_box_min.x = self.cube_min_x.value()
rr.bounding_box_min.y = self.cube_min_y.value()
rr.bounding_box_min.z = self.cube_min_z.value()
rr.bounding_box_max.x = self.cube_max_x.value()
rr.bounding_box_max.y = self.cube_max_y.value()
rr.bounding_box_max.z = self.cube_max_z.value()
rr.resolution = 0
rr.request_augment = 0
msg.region_req = rr
msg.aggregation_size = self.scan_number_val.value()
print msg
self.terrain_pub.publish( msg )
def getParamCallbackFcn(self, msg):
print "Not Defined Yet"
class StableStepDialog(Plugin):
def __init__(self, context):
super(StableStepDialog, self).__init__(context)
self.setObjectName('StableStepDialog')
self.bdi_state_msg = AtlasSimInterfaceState()
self.footstep_plan = rospy.Publisher('/flor/controller/footstep_plan',AtlasFootstepPlan, None, False, True, None, queue_size=10)
self.bdi_state_sub = rospy.Subscriber("/atlas/atlas_sim_interface_state", AtlasSimInterfaceState, self.simStateCallback)
self._widget = QWidget()
vbox = QVBoxLayout()
apply_command = QPushButton("Re-Center Steps")
apply_command.clicked.connect(self.apply_command_callback)
vbox.addWidget(apply_command)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutting down ..."
self.footstep_plan.unregister()
self.bdi_state_sub.unregister()
print "Done!"
def apply_command_callback(self):
print "Generate re-centering footsteps ..."
steps = []
L = 0.15 #self.params["Forward Stride Length"]["value"]
L_lat = 0.15 #self.params["Lateral Stride Length"]["value"]
W = 0.25 #self.params["Stride Width"]["value"]
dX = copy.deepcopy(self.bdi_state_msg.foot_pos_est[1].position.x - self.bdi_state_msg.foot_pos_est[0].position.x)
dY = copy.deepcopy(self.bdi_state_msg.foot_pos_est[1].position.y - self.bdi_state_msg.foot_pos_est[0].position.y)
dW = math.sqrt(dX*dX + dY*dY)
if (dW < 0.01):
print "Invalid starting feet position! dW=",dW
return
if (math.fabs(dW-W) < 0.01):
print "Feet are OK as is - publish empty plan !"
footstep_plan = AtlasFootStepPlan()
footstep_plan.header.stamp = rospy.get_rostime()
footstep_plan.pos_est = self.bdi_state_msg.pos_est
footstep_plan.step_plan = []
self.footstep_plan.publish(footstep_plan)
else:
# Need to centering step
center_x = copy.deepcopy(self.bdi_state_msg.foot_pos_est[0].position.x + 0.5*dX)
center_y = copy.deepcopy(self.bdi_state_msg.foot_pos_est[0].position.y + 0.5*dY)
# unit vector pointing from left foot to right
dx = dX/dW
dy = dY/dW
left_x = copy.deepcopy(center_x - W*0.5*dx - 0.1*dy)
left_y = copy.deepcopy(center_y - W*0.5*dy + 0.1*dx)
right_x = copy.deepcopy(center_x + W*0.5*dx - 0.1*dy)
right_y = copy.deepcopy(center_y + W*0.5*dy + 0.1*dx)
# Right stance
home_step = VigirBehaviorStepData()
home_step.step_index = 0
home_step.foot_index = 1
home_step.pose = copy.deepcopy(self.bdi_state_msg.foot_pos_est[1]) # use right as dummy footstep to calc offset properly
home_step.duration = 0.6
home_step.swing_height = 0.1
steps.append(home_step)
# Left mid point
home_step = VigirBehaviorStepData()
home_step.step_index = 1
home_step.foot_index = 0
home_step.pose = copy.deepcopy(self.bdi_state_msg.foot_pos_est[0]) # use right as dummy footstep to calc offset properly
home_step.pose.position.x = copy.deepcopy(0.5*(self.bdi_state_msg.foot_pos_est[0].position.x + left_x))
home_step.pose.position.y = copy.deepcopy(0.5*(self.bdi_state_msg.foot_pos_est[0].position.y + left_y))
home_step.duration = 0.6
home_step.swing_height = 0.1
steps.append(home_step)
# Right mid point
home_step = VigirBehaviorStepData()
home_step.step_index = 2
home_step.foot_index = 1
home_step.pose = copy.deepcopy(self.bdi_state_msg.foot_pos_est[1]) # use right as dummy footstep to calc offset properly
home_step.pose.position.x = 0.5*(self.bdi_state_msg.foot_pos_est[1].position.x + right_x)
home_step.pose.position.y = 0.5*(self.bdi_state_msg.foot_pos_est[1].position.y + right_y)
home_step.duration = 0.6
home_step.swing_height = 0.1
steps.append(home_step)
# Left center
home_step = VigirBehaviorStepData()
home_step.step_index = 3
home_step.foot_index = 0
home_step.pose = copy.deepcopy(self.bdi_state_msg.foot_pos_est[0]) # use right as dummy footstep to calc offset properly
home_step.pose.position.x = copy.deepcopy(left_x)
home_step.pose.position.y = copy.deepcopy(left_y)
home_step.duration = 0.6
home_step.swing_height = 0.1
steps.append(home_step)
# Right center
home_step = VigirBehaviorStepData()
home_step.step_index = 4
home_step.foot_index = 1
home_step.pose = copy.deepcopy(self.bdi_state_msg.foot_pos_est[1]) # use right as dummy footstep to calc offset properly
home_step.pose.position.x = copy.deepcopy(right_x)
home_step.pose.position.y = copy.deepcopy(right_y)
home_step.duration = 0.6
home_step.swing_height = 0.1
steps.append(home_step)
print "Publish plan ..."
footstep_plan = AtlasFootstepPlan()
footstep_plan.header.stamp = rospy.get_rostime()
footstep_plan.pos_est = self.bdi_state_msg.pos_est
footstep_plan.step_plan = steps
self.footstep_plan.publish(footstep_plan)
print footstep_plan
# Update BDI state
def simStateCallback(self, state_msg):
self.bdi_state_msg = state_msg
class Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(
twi,
len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class Top(Plugin):
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
rospy.wait_for_service('PlayTrajectoryState')
self.play_traj = rospy.ServiceProxy('PlayTrajectoryState',
PlayTrajectoryState)
# Create a container widget and give it a layout
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
context.add_widget(self._container)
# merge sort to pick files generate files to pick
traj_dir = rospy.get_param('traj_dir')
self.traj_list = sorted(os.listdir(traj_dir))
#self.traj_list = self.traj_list[:5]
self.length = len(self.traj_list)
self.work_list = copy.deepcopy(self.traj_list)
self.merge_width = 1
self.merge_ind = 0
self._i0 = self.merge_ind
self._i1 = min(self.merge_ind + self.merge_width, self.length)
self._j = 0
self.comp_res = True
self.valid_comp = False
self.merge()
#random.seed()
#sys.stdin.readline()
# use merge sort to sample traj fiels
#self.merge_traj_files()
# regenerate the trajectories
#self.rand_traj_files()
self.write_to_file = ""
# Add a button to play first trajcetory
self._play_A_btn = QPushButton('Play Trajectory A')
self._layout.addWidget(self._play_A_btn)
self._play_A_btn.clicked.connect(self._play_traj_A)
# Add a slider to trace the first trajectory
self._slider_A = QSlider(Qt.Horizontal)
self._layout.addWidget(self._slider_A)
self._slider_A.setMinimum(1)
self._slider_A.setMaximum(100)
self._slider_A.valueChanged.connect(self._change_slider_A)
# Add a button to play second trajcetory
self._play_B_btn = QPushButton('Play Trajectory B')
self._layout.addWidget(self._play_B_btn)
self._play_B_btn.clicked.connect(self._play_traj_B)
# Add a slider to trace the second trajectory
self._slider_B = QSlider(Qt.Horizontal)
self._layout.addWidget(self._slider_B)
self._slider_B.setMinimum(1)
self._slider_B.setMaximum(100)
self._slider_B.valueChanged.connect(self._change_slider_B)
# Add a button to pause current trajectories
self._pause_btn = QPushButton('Pause current trajectory')
self._layout.addWidget(self._pause_btn)
self._pause_btn.clicked.connect(self._pause_traj)
self.paused = False
# Add a button to pick first trajcetory
self._pick_A_btn = QPushButton('Pick Trajectory A')
self._layout.addWidget(self._pick_A_btn)
self._pick_A_btn.clicked.connect(self._pick_traj_A)
# Add a button to pick second trajcetory
self._pick_B_btn = QPushButton('Pick Trajectory B')
self._layout.addWidget(self._pick_B_btn)
self._pick_B_btn.clicked.connect(self._pick_traj_B)
# Add a button to pick neither of trajcetories
self._pick_none_btn = QPushButton('Pick None of Trajectories')
self._layout.addWidget(self._pick_none_btn)
self._pick_none_btn.clicked.connect(self._pick_none)
def merge_traj_files(self):
print "___________________________________________"
if self.merge_width < self.length:
print "Width = %f" % self.merge_width
if self.merge_ind < self.length:
print "Merge index = %f" % self.merge_ind
# take pair of sublists and merge them
while True:
if self._j < min(self.merge_ind + 2 * self.merge_width,
self.length):
if (self._i0 < min(self.merge_ind + self.merge_width,
self.length)):
if self._i1 >= min(
self.merge_ind + 2 * self.merge_width,
self.length):
self.work_list[self._j] = self.traj_list[
self._i0]
print "it's here 1"
print "B %f assigned A %f" % (self._j,
self._i0)
self._i0 += 1
self.valid_comp = False
else:
if self.valid_comp:
if self.comp_res:
self.work_list[
self._j] = self.traj_list[self._i0]
print "it's here 2"
print "B %f assigned A %f" % (self._j,
self._i0)
self._i0 += 1
self.valid_comp = False
else:
self.work_list[
self._j] = self.traj_list[self._i1]
print "it's here 3"
print "B %f assigned A %f" % (self._j,
self._i1)
self._i1 += 1
self.valid_comp = False
else:
self.merge()
break
else:
self.work_list[self._j] = self.traj_list[self._i1]
print "it's here 4"
print "B %f assigned A %f" % (self._j, self._i1)
self._i1 += 1
self.valid_comp = False
self._j += 1
else:
self.merge_ind = self.merge_ind + 2 * self.merge_width
self._i0 = self.merge_ind
self._i1 = min(self.merge_ind + self.merge_width,
self.length)
self._j = self.merge_ind
self.merge()
print "lists are merged"
print self.traj_list
print self.work_list
temp = copy.deepcopy(self.traj_list)
self.traj_list = copy.deepcopy(self.work_list)
#self.work_list = temp
#self.merge_traj_files()
break
else:
# copyArray()
temp = copy.deepcopy(self.traj_list)
self.traj_list = copy.deepcopy(self.work_list)
self.work_list = temp
self.merge_ind = 0
self.merge_width = 2 * self.merge_width
self._i0 = self.merge_ind
self._i1 = min(self.merge_ind + self.merge_width, self.length)
self._j = self.merge_ind
print "ALL LISTS ARE MERGED"
print self.traj_list
self.merge_traj_files()
else:
print "ALL TRAJECTORIES ARE RANKED"
f_ = open('ranked_traj.txt', 'w')
for item in self.traj_list:
f_.write("%s\n" % item)
f_.close()
def merge(self):
print self._i0
print self._i1
try:
self.fileA = self.traj_list[self._i0]
self.fileB = self.traj_list[self._i1]
except:
try:
self.fileA = self.work_list[0]
self.fileB = self.work_list[min(2 * self.merge_width,
self.length)]
except:
self.fileA = self.traj_list[0]
self.fileB = self.traj_list[1]
print " new files assigned are: %s, %s " % (self.fileA, self.fileB)
def _play_traj_A(self):
if (self.paused):
self._pause_btn.setText("Pause current trajectory")
self.paused = False
req = UiState(Header(), self.fileA, True, False, -1)
resp = self.play_traj(req)
def _change_slider_A(self):
self._slider_B.setValue(0)
req = UiState(Header(), self.fileA, False, True,
float(self._slider_A.value()))
resp = self.play_traj(req)
def _play_traj_B(self):
if (self.paused):
self._pause_btn.setText("Pause current trajectory")
self.paused = False
req = UiState(Header(), self.fileB, True, False, -1)
resp = self.play_traj(req)
def _change_slider_B(self):
self._slider_A.setValue(0)
req = UiState(Header(), self.fileB, False, True,
float(self._slider_B.value()))
resp = self.play_traj(req)
def _pause_traj(self):
if (self.paused):
req = UiState(Header(), "", False, False, -1)
resp = self.play_traj(req)
self._pause_btn.setText("Pause current trajectory")
self.paused = False
else:
req = UiState(Header(), "", False, True, -1)
resp = self.play_traj(req)
self._pause_btn.setText("Continue playing current trajectory")
self.paused = True
def _pick_traj_A(self):
self.comp_res = True
self.valid_comp = True
self.merge_traj_files()
def _pick_traj_B(self):
self.comp_res = False
self.valid_comp = True
self.merge_traj_files()
def _pick_none(self):
#reinvoke the traj ranodm generator
self.rand_traj_files()
# produce two new filenames from the trajectory directory
def rand_traj_files(self):
first_to_pick = random.randint(0, len(self.traj_list) - 1)
while (True):
second_to_pick = random.randint(0, len(self.traj_list) - 1)
if (second_to_pick != first_to_pick):
break
self.fileA = self.traj_list[first_to_pick]
self.fileB = self.traj_list[second_to_pick]
class SimpleGUI(Plugin):
sound_sig = Signal(SoundRequest)
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15,20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>', right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235,20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275,20))
left_right_head_box.addItem(QtGui.QSpacerItem(160,20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60,20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225,20))
down_head_box.addItem(QtGui.QSpacerItem(235,20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275,20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper', gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100,250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75,20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450,20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75,20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100,100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward', forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base= Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base= Base(Base.RIGHT, self)
right_base_button= self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50,20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300,20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base= Base(Base.BACKWARD, self)
backward_base_button = self.create_button('move backward',
backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400,20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base= Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base= Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(' /\n<--',
clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75,20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225,20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100,20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet("QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class BiasCalibrationDialog(Plugin):
def __init__(self, context):
super(BiasCalibrationDialog, self).__init__(context)
self.setObjectName('BiasCalibrationDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
controller_widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointBias(self, roslib.packages.get_pkg_dir('vigir_rqt_bias_calibration') + '/launch/joints.txt',hbox)
controller_widget.setLayout(hbox)
self.marker = Marker()
self.marker.header.frame_id = "/world"
self.marker.type = self.marker.CUBE
self.marker.action = self.marker.ADD
self.marker.scale.x = 14.0
self.marker.scale.y = 14.0
self.marker.scale.z = 0.02
self.marker.color.a = 0.25
self.marker.color.r = 1.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
#self.marker.points = [[0.0, 0.0, 0.0], [7.0, -4.0, 0.0], [7.0, 4.0, 0.0]]
self.marker.pose.orientation.w = 1.0
self.marker.pose.position.x = 0.0
self.marker.pose.position.y = 0.0
self.marker.pose.position.z = 0.0
self.bias_pub = rospy.Publisher('/flor/controller/atlas_biases', JointState, queue_size=10)
self.flor_marker_pub = rospy.Publisher('/flor_plane_marker', Marker, queue_size=10)
self.feet_state_sub = rospy.Subscriber('/atlas/atlas_sim_interface_state', AtlasSimInterfaceState, self.stateCallbackFnc)
vbox.addWidget(controller_widget)
self.save_button = QPushButton("Save Biases")
self.save_button.pressed.connect(self.on_savePressed)
vbox.addWidget(self.save_button)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutdown ..."
self.bias_pub.unregister()
self.flor_marker_pub.unregister()
self.feet_state_sub.unregister()
print "Done!"
def stateCallbackFnc(self, atlasState_msg):
self.marker.pose.position.x = (atlasState_msg.foot_pos_est[0].position.x + atlasState_msg.foot_pos_est[1].position.x)*0.5
self.marker.pose.position.y = (atlasState_msg.foot_pos_est[0].position.y + atlasState_msg.foot_pos_est[1].position.y)*0.5
self.marker.pose.position.z = (atlasState_msg.foot_pos_est[0].position.z + atlasState_msg.foot_pos_est[1].position.z)*0.5
#self.marker.pose.orientation = atlasState_msg.foot_pos_est[0].orientation
self.flor_marker_pub.publish(self.marker)
def on_savePressed(self):
print "Save data to file..."
self.joint_control.saveData(roslib.packages.get_pkg_dir('vigir_rqt_bias_calibration') + '/launch/new_biases.txt')
class SimpleGUI(Plugin):
# For sending speech
sound_sig = Signal(SoundRequest)
# Joints for arm poses
joint_sig = Signal(JointState)
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose) / 2])
pose_right.append(pose[len(pose) / 2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(
pose_left, pose_right, left_gripper_states,
right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav,
self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10, 0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230, 0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80, 0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100, 0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout()
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150, 0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150, 0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [
Pose(Point(2.04748392105, 2.04748010635, 0.000),
Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0),
Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000),
Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000),
Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000),
Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))
]
self.index = 1
rospy.loginfo("Completed GUI initialization")
# Event for when text box is changed
def onChanged(self, text):
self.speech_label.setText(text)
self.speech_label.adjustSize()
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.clicked.connect(self.command_cb)
return btn
def create_pressed_button(self, name):
btn = QtGui.QPushButton(name, self._widget)
btn.setFixedWidth(150)
btn.pressed.connect(self.command_cb)
btn.setAutoRepeat(True) # To make sure the movement repeats itself
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
qWarning('Robot said: ' + sound_request.arg)
self.speech_label.setText(sound_request.arg) #'Robot said: ' +
#a button was clicked
def command_cb(self):
button_name = self._widget.sender().text()
#robot talk button clicked
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.speech_label.text())
self._sound_client.say(self.speech_label.text())
self.show_text_in_rviz("Robot is Speaking")
#gripper button selected
elif ('Gripper' in button_name):
self.gripper_click(button_name)
# Move forward
elif (button_name == '^'):
self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
# Move left
elif (button_name == '<'):
self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0)
# Move right
elif (button_name == '>'):
self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0)
# Move back
elif (button_name == 'v'):
self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0)
#Rotate Left
elif (button_name == 'Rotate Left'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50)
# Rotate Right
elif (button_name == 'Rotate Right'):
self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50)
# A head button selected
elif ('Head' in button_name):
self.rotate_head(button_name)
#An arm button selected
#third param unused in freeze/relax
#Second word in button should be side
elif ('Arm' in button_name):
arm_side = button_name.split()[1]
if ('Freeze' in button_name or 'Relax' in button_name):
new_arm_state = button_name.split()[0]
self.toggle_arm(arm_side[0].lower(), new_arm_state, True)
old_arm_state = ''
if (new_arm_state == 'Relax'):
old_arm_state = 'Freeze'
else:
old_arm_state = 'Relax'
self._widget.sender().setText('%s %s Arm' %
(old_arm_state, arm_side))
elif ('Play Animation' == button_name):
self.animPlay.left_poses = self.saved_animations[
self.saved_animations_list.currentText()].left
self.animPlay.right_poses = self.saved_animations[
self.saved_animations_list.currentText()].right
self.animPlay.left_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
self.saved_animations_list.currentText()].right_gripper
if self.pose_time.text() == '':
self.show_warning('Please enter a duration in seconds.')
else:
self.animPlay.play(self.pose_time.text())
elif ('Animation' in button_name):
if ('Save' in button_name):
if self.animation_name.text() == '':
self.show_warning('Please enter name for animation')
else:
self.save_animation(self.animation_name.text())
self.list_widget.clear()
self.animation_name.setText('')
elif ('Add Current Pose' == button_name):
if self.pose_name_temp.text() == '':
self.show_warning('Insert name for pose')
else:
self.animation_map[self.pose_name_temp.text()] = Quad(
self.get_joint_state('l'), self.get_joint_state('r'),
self.left_gripper_state, self.right_gripper_state)
list_item = QListWidgetItem()
list_item.setText(self.pose_name_temp.text())
self.list_widget.addItem(list_item)
self.pose_name_temp.setText('')
elif ('Move to Bin' == button_name):
self.move_to_bin_action()
elif ('Clean Room' == button_name):
rospy.loginfo("STARTING AUTONOMOUS MODE")
self.tuck_arms()
while (self.index < len(self.locations)):
self.roomNav.move_to_trash_location(self.locations[self.index])
# self.index += 1
self.head_action(1.0, 0, -0.50, True)
# Returns Nonce if nothing, and the point of the first object it sees otherwise
map_point = self.pap.detect_objects()
if (map_point == None):
self.index += 1
else:
self.pick_and_move_trash_action()
rospy.loginfo("FINISHED AUTONOMOUS MODE")
self.index = 1
elif ('Object Detect' == button_name):
self.pick_and_move_trash_action()
def pick_and_move_trash_action(self, map_point=None):
if map_point == None:
self.head_action(1.0, 0, -0.50, True)
map_point = self.pap.detect_objects()
if map_point == None:
return
# Convert to base link and move towards the object 0.50m away
map_point = Transformer.transform(self._tf_listener, map_point.pose,
map_point.header.frame_id,
'/base_link')
rospy.loginfo("Object is " + str(map_point.pose.position.x) + " away")
'''
if(map_point.pose.position.x < 0.8):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
'''
move_back_first = False
if (map_point.pose.position.x < 0.7):
move_back_first = True
map_point.pose.position.x -= 0.450
map_point = Transformer.transform(self._tf_listener, map_point.pose,
'/base_link', '/map')
if (move_back_first):
self.roomNav.move_to_trash_location(self.locations[self.index - 1])
success = self.roomNav.move_to_trash_location(map_point.pose)
'''This part of the code strafes the robot left to get closer to the object'''
'''
rate = rospy.Rate(10)
position = Point()
move_cmd = Twist()
move_cmd.linear.y = 0.25
odom_frame = '/odom_combined'
# Find out if the robot uses /base_link or /base_footprint
try:
self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Get current position
position = self.get_odom()
x_start = position.x
y_start = position.y
# Distance travelled
distance = 0
goal_distance = 0.25
rospy.loginfo("Strafing left")
# Enter the loop to move along a side
while distance < goal_distance:
rospy.loginfo("Distance is at " + str(distance))
# Publish the Twist message and sleep 1 cycle
self.base_action(0, 0.25, 0, 0, 0, 0)
rate.sleep()
# Get the current position
position = self.get_odom()
# Compute the Euclidean distance from the start
distance = abs(position.y - y_start)
'''
if (success):
# Move head to look at the object, this will wait for a result
self.head_action(0, 0.4, 0.55, True)
# Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object
self.animPlay.left_poses = self.saved_animations[
'ready_pickup'].left
self.animPlay.right_poses = self.saved_animations[
'ready_pickup'].right
self.animPlay.left_gripper_states = self.saved_animations[
'ready_pickup'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'ready_pickup'].right_gripper
self.animPlay.play('3.0')
for i in range(0, 3):
success = self.pap.detect_and_pickup()
# Move head back to look forward
# Move head to look at the object, this will wait for a result
self.head_action(1.0, 0.0, 0.55, True)
if success:
break
# Move to bin
self.move_to_bin_action()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans = self._tf_listener.lookupTransform('/odom_combined',
self.base_frame,
rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(trans[0][0], trans[0][1], trans[0][2])
# gripper_type is either 'l' for left or 'r' for right
# gripper position is the position as a parameter to the gripper goal
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
# Moves the torso of the robot down to its maximum
def torso_down(self, wait=False):
self.torso_client = SimpleActionClient(
'/torso_controller/position_joint_action',
SingleJointPositionAction)
torso_goal = SingleJointPositionGoal()
torso_goal.position = 0.0
torso_goal.min_duration = rospy.Duration(5.0)
torso_goal.max_velocity = 1.0
self.torso_client.send_goal(torso_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = self.torso_client.wait_for_result(
rospy.Duration(15))
# Check for success or failure
if not finished_within_time:
self.torso_client.cancel_goal()
rospy.loginfo("Timed out achieving torso movement goal")
else:
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Torso movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Torso movement goal failed with error code: " +
str(state))
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
def tuck_arms(self):
rospy.loginfo('Left tuck arms')
self.animPlay.left_poses = self.saved_animations['left_tuck'].left
self.animPlay.right_poses = self.saved_animations['left_tuck'].right
self.animPlay.left_gripper_states = self.saved_animations[
'left_tuck'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'left_tuck'].right_gripper
self.animPlay.play('3.0')
def move_to_bin_action(self):
# First tuck arms
self.tuck_arms()
# Move to the bin
rospy.loginfo('Clicked the move to bin button')
self.roomNav.move_to_bin()
# Throw the trash away
rospy.loginfo('Throwing trash away with left arm')
self.animPlay.left_poses = self.saved_animations['l_dispose'].left
self.animPlay.right_poses = self.saved_animations['l_dispose'].right
self.animPlay.left_gripper_states = self.saved_animations[
'l_dispose'].left_gripper
self.animPlay.right_gripper_states = self.saved_animations[
'l_dispose'].right_gripper
self.animPlay.play('2.0')
# Tuck arms again
self.tuck_arms()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def show_arrow_in_rviz(self, arrow):
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
arrow=arrow)
self.marker_publisher.publish(marker)
def save_animation(self, animation_name):
if animation_name != '':
filename = os.path.join(self.dir, 'animations/')
anim_file = open(filename + animation_name + '.txt', 'w')
l_animation_queue = []
r_animation_queue = []
l_gripper_queue = []
r_gripper_queue = []
for i in range(self.list_widget.count()):
item = self.list_widget.item(i)
pose_name = item.text()
anim_file.write(
re.sub(
',', '',
str(self.animation_map[pose_name].left).strip('[]') +
' ' +
str(self.animation_map[pose_name].right).strip('[]')))
anim_file.write('\n')
anim_file.write(
str(self.animation_map[pose_name].left_gripper) + ' ' +
str(self.animation_map[pose_name].right_gripper))
l_animation_queue.append(self.animation_map[pose_name].left)
r_animation_queue.append(self.animation_map[pose_name].right)
l_gripper_queue.append(
self.animation_map[pose_name].left_gripper)
r_gripper_queue.append(
self.animation_map[pose_name].right_gripper)
anim_file.write('\n')
anim_file.close()
self.saved_animations[animation_name] = Quad(
l_animation_queue, r_animation_queue, l_gripper_queue,
r_gripper_queue)
self.saved_animations_list.addItem(
animation_name) # Bug? Multiple entries
# Reset the pending queue
self.l_animation_queue = []
self.r_animation_queue = []
else:
self.show_warning('Please insert name for animation')
def gripper_click(self, button_name):
grip_side = ''
grip_side_text = ''
if ('Left' in button_name):
grip_side = 'l'
grip_side_text = 'left'
else:
grip_side = 'r'
grip_side_text = 'right'
if ('Open' in button_name):
grip_action = 20.0
grip_action_text = 'close'
qWarning('Robot opened %s gripper' % (grip_side_text))
else:
grip_action = 0.0
grip_action_text = 'open'
qWarning('Robot closed %s gripper' % (grip_side_text))
self.show_text_in_rviz(
"%sing %s Gripper" %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
self.gripper_action(grip_side, grip_action)
self._widget.sender().setText(
'%s %s Gripper' %
(grip_action_text.capitalize(), grip_side_text.capitalize()))
def rotate_head(self, button_name):
if ('Left' in button_name):
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y > 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y < 0.0):
self.head_x += 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Left")
else:
self.head_x -= 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Left")
qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
elif ('Up' in button_name):
if (self.head_z <= 1.6):
self.head_z += 0.1
self.show_text_in_rviz("Moving Head Up")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look up anymore')
elif ('Down' in button_name):
if (self.head_z >= -2.2):
self.head_z -= 0.1
self.show_text_in_rviz("Moving Head Down")
self.head_action(self.head_x, self.head_y, self.head_z)
else:
self.show_warning('Can\'t look down anymore')
else:
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
if (self.head_x < -0.8 and self.head_y < 0.0):
self.show_warning('Can\'t rotate anymore')
elif (self.head_y > 0.0):
self.head_x += 0.1
self.head_y = (1.0 - self.head_x**2.0)**0.5
self.show_text_in_rviz("Turning Head Right")
else:
self.head_x -= 0.1
self.head_y = -((1.0 - self.head_x**2.0)**0.5)
self.show_text_in_rviz("Turning Head Right")
#qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
self.head_action(self.head_x, self.head_y, self.head_z)
def toggle_arm(self, side, toggle, button):
controller_name = side + '_arm_controller'
start_controllers = []
stop_controllers = []
if (toggle == 'Relax'):
stop_controllers.append(controller_name)
else:
start_controllers.append(controller_name)
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
class WaterPulse(Plugin):
sound_sig = Signal(SoundRequest)
positions = [[
-0.013001995432544766, -0.24895825213211362, -0.0055992576345036404,
-1.7647281786034612, -496.3549908032631, -0.09960750521541717,
25.125128627428623
],
[
0.0029160750999965845, -0.29700816845544387,
0.010917289481594317, -0.8034506550168371,
-496.35799885178454, -0.09965101413551591,
25.12517213634872
]]
def __init__(self, context):
super(WaterPulse, self).__init__(context)
self.setObjectName('WaterPulse')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
l_traj_contoller_name = None
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
# 'Trick or Treat' & 'Thank You' Buttons
halloween_box = QtGui.QHBoxLayout()
halloween_box.addItem(QtGui.QSpacerItem(15, 20))
halloween_box.addWidget(
self.create_button('Trick or Treat', self.command_cb))
halloween_box.addWidget(
self.create_button('Thank You', self.command_cb))
halloween_box.addStretch(1)
large_box.addLayout(halloween_box)
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal)
head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus)
head_speed_sld.setMinimum(1)
head_speed_sld.setMaximum(5)
head_speed_sld.valueChanged[int].connect(Head.set_speed)
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_sld_box = QtGui.QHBoxLayout()
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_sld_box.addWidget(head_speed_sld)
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
head_box.addLayout(head_sld_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
knock_button = self.create_button('Knock', self.knock)
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addWidget(knock_button)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('WaterPulse')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../rosie_background.jpg"))
def show_text_in_rviz(self, text):
marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(Point(0.5, 0.5, 1.45),
Quaternion(0, 0, 0, 1)),
scale=Vector3(0.06, 0.06, 0.06),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text=text)
self.marker_publisher.publish(marker)
def sound_cb(self, sound_request):
qWarning('Received sound.')
self.sound_sig.emit(sound_request)
def create_button(self, name, method):
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
btn.setAutoRepeat(True)
return btn
def sound_sig_cb(self, sound_request):
qWarning('Received sound signal.')
#if (sound_request.command == SoundRequest.SAY):
qWarning('Robot said: ' + sound_request.arg)
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Speak'):
qWarning('Robot will say: ' + self.sound_textbox.text())
self._sound_client.say(self.sound_textbox.text())
elif (button_name == 'Trick or Treat'):
qWarning('Robot will say: Trick or Treat')
self._sound_client.say('Trick or Treat')
elif (button_name == 'Thank You'):
qWarning('Robot will say: Thank You')
self._sound_client.say('Thank You')
def knock(self):
for position in WaterPulse.positions:
velocities = [0] * len(position)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
time_to_joint = 2.0
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=position,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
result = 0
while (result < 2): # ACTIVE or PENDING
self.l_traj_action_client.wait_for_result()
result = self.l_traj_action_client.get_result()
#self.l_traj_action_client.wait_for_result()
#time.sleep(1)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def __init__(self, context):
#super(PositionModeDialog, self).__init__(context)
#self.setObjectName('PositionModeDialog')
super(PositionModeWidget, self).__init__()
self.name = 'PositionModeWidget'
self.updateStateSignal.connect(self.on_updateState)
self.updatePelvisSignal.connect(self.on_updatePelvis)
self.updateGhostSignal.connect(self.on_updateGhost)
## Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-f", "--file",
# dest="file",
# help="Input file name")
#args, unknowns = parser.parse_known_args(context.argv())
#print 'arguments: ', args
#print 'unknowns: ', unknowns
#
#if ((args.file == "") or (args.file == None)):
# print "No file specified - abort!"
# sys.exit(-1)
# Define how long the trajectories will take to execute
self.time_factor = rospy.get_param('~time_factor', 2.1)
print "Using ",self.time_factor, " as the time factor for all trajectories"
# Joint Name : Child Link :OSRF
self.joint_names=[]
self.joint_names.append('back_bkz') # : ltorso : 0
self.joint_names.append('back_bky') # : mtorso : 1
self.joint_names.append('back_bkx') # : utorso : 2
self.joint_names.append('neck_ry') # : head : 3
self.joint_names.append('l_leg_hpz') # : l_uglut : 4
self.joint_names.append('l_leg_hpx') # : l_lglut : 5
self.joint_names.append('l_leg_hpy') # : l_uleg : 6
self.joint_names.append('l_leg_kny') # : l_lleg : 7
self.joint_names.append('l_leg_aky') # : l_talus : 8
self.joint_names.append('l_leg_akx') # : l_foot : 9
self.joint_names.append('r_leg_hpz') # : r_uglut : 10
self.joint_names.append('r_leg_hpx') # : r_lglut : 11
self.joint_names.append('r_leg_hpy') # : r_uleg : 12
self.joint_names.append('r_leg_kny') # : r_lleg : 13
self.joint_names.append('r_leg_aky') # : r_talus : 14
self.joint_names.append('r_leg_akx') # : r_foot : 15
self.joint_names.append('l_arm_shz') # : l_clav : 16
self.joint_names.append('l_arm_shx') # : l_scap : 17
self.joint_names.append('l_arm_ely') # : l_uarm : 18
self.joint_names.append('l_arm_elx') # : l_larm : 19
self.joint_names.append('l_arm_wry') # : l_farm : 20
self.joint_names.append('l_arm_wrx') # : l_hand : 21
self.joint_names.append('l_arm_wry2') # : l_farm : 22
self.joint_names.append('r_arm_shz') # : r_clav : 23
self.joint_names.append('r_arm_shx') # : r_scap : 24
self.joint_names.append('r_arm_ely') # : r_uarm : 25
self.joint_names.append('r_arm_elx') # : r_larm : 26
self.joint_names.append('r_arm_wry') # : r_farm : 27
self.joint_names.append('r_arm_wrx') # : r_hand : 28
self.joint_names.append('r_arm_wry2') # : r_farm : 29
self.joint_states = JointState()
self.pelvis_names=[]
self.joint_names.append('forward') # : 30
self.joint_names.append('lateral') # : 31
self.joint_names.append('height') # : 32
self.joint_names.append('roll') # : 33
self.joint_names.append('pitch') # : 34
self.joint_names.append('yaw') # : 35
self.pelvis_states = JointState()
self.pelvis_states.position = [0.0, 0.0, 0.85, 0.0, 0.0, 0.0] # default pelvis pose
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radioGroup.addButton(self.radio_robot_target,1)
self.radio_ghost_target.setChecked(True)
self.commandGroup = QButtonGroup()
self.commandGroup.setExclusive(True)
self.radio_position_command = QRadioButton()
self.radio_position_command.setText("Position")
self.commandGroup.addButton(self.radio_position_command,0)
self.radio_trajectory_command = QRadioButton()
self.radio_trajectory_command.setText("Trajectory")
self.commandGroup.addButton(self.radio_trajectory_command,1)
self.radio_trajectory_command.setChecked(True)
#print "position button is checked: ",self.radio_position_command.isChecked()
#print "trajectory button is checked: ",self.radio_trajectory_command.isChecked()
print "Default group button checked is ",self.radioGroup.checkedId()
print "Default command button checked is ",self.commandGroup.checkedId()
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
vbox_robot = QVBoxLayout()
widget_robot = QWidget()
widget_robot_sel = QWidget()
hbox_sel = QHBoxLayout()
hbox_radio.addStretch()
hbox_sel.addWidget(self.radio_robot_target)
#hbox_sel.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
widget_robot_sel.setLayout(hbox_sel)
vbox_robot.addWidget(widget_robot_sel);
widget_cmd = QWidget()
hbox_cmd = QHBoxLayout()
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_position_command)
#hbox_cmd.addWidget(QLabel("Position"))
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_trajectory_command)
#hbox_cmd.addWidget(QLabel("Trajectory"))
hbox_radio.addStretch()
widget_cmd.setLayout(hbox_cmd)
vbox_robot.addWidget(widget_cmd);
widget_robot.setLayout(vbox_robot)
hbox_radio.addWidget(widget_robot)
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
positions_widget = QWidget()
hbox = QHBoxLayout()
#self.robot = URDF.from_parameter_server()
self.chains=[]
# Subscribe to Joint States update /atlas/atlas_state
#print "Load parameters"
#print rospy.get_param_names()
#fileName = rospy.get_param('positions_file', '/opt/vigir/catkin_ws/src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch/r_arm_positions.txt')
#print "FileName:",fileName
# Left to right layout
numFiles = rospy.get_param("~num_files");
for i in range(1,numFiles+1):
path = "~position_files/file_" + str(i)
foobar = str(rospy.get_param(path))
self.chains.append(ChainData(self, foobar, hbox))
#add stretch at end so all GUI elements are at left of dialog
hbox.addStretch(1)
positions_widget.setLayout(hbox)
vbox.addWidget(positions_widget)
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.pelvisSubscriber = rospy.Subscriber('/robot_controllers/pelvis_traj_controller/current_states',JointState, self.pelvisCallbackFnc)
class Configuration(Plugin):
isAutoSend = False
def __init__(self, context):
super(Configuration, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PositionControllerConfiguration')
self.__context = context
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_position_controller'), 'resource', 'configuration.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('ControllerUi')
self.__pids = deque()
self.__pids.append(PIDConfiguration('Altitude Pos','altitude_pos'))
self.__pids.append(PIDConfiguration('Altitude','altitude'))
self.__pids.append(PIDConfiguration('Pitch Pos','pitch_pos'))
self.__pids.append(PIDConfiguration('Pitch','pitch'))
self.__pids.append(PIDConfiguration('Roll Pos','roll_pos'))
self.__pids.append(PIDConfiguration('Roll','roll'))
self.__pids.append(PIDConfiguration('Yaw Angle','yaw_angle'))
self.__pids.append(PIDConfiguration('Yaw','yaw'))
self.__pidLayout = QVBoxLayout()
for pid in self.__pids:
self.__pidLayout.addWidget(pid)
pid.updated.connect(self.onPidUpdate)
self._scrollWidget = QWidget()
self._scrollWidget.setLayout(self.__pidLayout)
self._widget.pidScrollArea.setWidget(self._scrollWidget)
self._widget.pushButtonRefresh.clicked.connect(self.refresh)
self._widget.pushButtonSend.clicked.connect(self.send)
self._widget.checkBoxAutoSend.stateChanged.connect(self.changedAutoSend)
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.refresh()
# update all pids
def refresh(self):
for pid in self.__pids:
pid.refresh()
# send all pid configs to remote
def send(self):
for pid in self.__pids:
pid.send_current()
def onPidUpdate(self):
if self.isAutoSend:
self.sender().send_current()
def changedAutoSend(self, state):
self.isAutoSend = state > 0
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class PbDGUI(Plugin):
exp_state_sig = Signal(ExperimentState)
def __init__(self, context):
super(PbDGUI, self).__init__(context)
self.setObjectName('PbDGUI')
self._widget = QWidget()
self.speech_cmd_publisher = rospy.Publisher('recognized_command',
Command)
self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand)
rospy.Subscriber('experiment_state', ExperimentState,
self.exp_state_cb)
rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.exp_state_sig.connect(self.update_state)
self.commands = dict()
self.commands[Command.CREATE_NEW_ACTION] = 'New action'
self.commands[Command.TEST_MICROPHONE] = 'Test microphone'
self.commands[Command.NEXT_ACTION] = 'Next action'
self.commands[Command.PREV_ACTION] = 'Previous action'
self.commands[Command.SAVE_POSE] = 'Save pose'
self.commands[Command.RELAX_RIGHT_ARM] = 'Relax right arm'
self.commands[Command.RELAX_LEFT_ARM] = 'Relax left arm'
self.commands[Command.FREEZE_RIGHT_ARM] = 'Freeze right arm'
self.commands[Command.FREEZE_LEFT_ARM] = 'Freeze left arm'
self.commands[Command.OPEN_RIGHT_HAND] = 'Open right hand'
self.commands[Command.OPEN_LEFT_HAND] = 'Open left hand'
self.commands[Command.CLOSE_RIGHT_HAND] = 'Close right hand'
self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand'
self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand'
self.commands[Command.EXECUTE_ACTION] = 'Execute action'
self.commands[Command.STOP_EXECUTION] = 'Stop execution'
self.commands[Command.DELETE_ALL_STEPS] = 'Delete all'
self.commands[Command.DELETE_LAST_STEP] = 'Delete last'
self.commands[Command.RECORD_OBJECT_POSE] = 'Record object poses'
self.currentAction = -1
self.currentStep = -1
allWidgetsBox = QtGui.QVBoxLayout()
actionBox = QGroupBox('Actions', self._widget)
self.actionGrid = QtGui.QGridLayout()
self.actionGrid.setHorizontalSpacing(0)
for i in range(6):
self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i,
QtCore.Qt.AlignCenter)
self.actionGrid.setColumnStretch(i, 0)
self.actionIcons = dict()
actionBoxLayout = QtGui.QHBoxLayout()
actionBoxLayout.addLayout(self.actionGrid)
actionBox.setLayout(actionBoxLayout)
actionButtonGrid = QtGui.QHBoxLayout()
actionButtonGrid.addWidget(
self.create_button(Command.CREATE_NEW_ACTION))
self.stepsBox = QGroupBox('No actions created yet', self._widget)
self.stepsGrid = QtGui.QGridLayout()
self.l_model = QtGui.QStandardItemModel(self)
self.l_view = self._create_table_view(self.l_model,
self.l_row_clicked_cb)
self.r_model = QtGui.QStandardItemModel(self)
self.r_view = self._create_table_view(self.r_model,
self.r_row_clicked_cb)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3)
self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3)
self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0)
self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2)
self.stepsGrid.addWidget(self.l_view, 1, 0)
self.stepsGrid.addWidget(self.r_view, 1, 2)
stepsBoxLayout = QtGui.QHBoxLayout()
stepsBoxLayout.addLayout(self.stepsGrid)
self.stepsBox.setLayout(stepsBoxLayout)
stepsButtonGrid = QtGui.QHBoxLayout()
stepsButtonGrid.addWidget(self.create_button(Command.SAVE_POSE))
stepsButtonGrid.addWidget(self.create_button(Command.EXECUTE_ACTION))
stepsButtonGrid.addWidget(self.create_button(Command.STOP_EXECUTION))
stepsButtonGrid.addWidget(self.create_button(Command.DELETE_ALL_STEPS))
stepsButtonGrid.addWidget(self.create_button(Command.DELETE_LAST_STEP))
misc_grid = QtGui.QHBoxLayout()
misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE))
misc_grid.addWidget(self.create_button(Command.RECORD_OBJECT_POSE))
misc_grid.addStretch(1)
misc_grid2 = QtGui.QHBoxLayout()
misc_grid2.addWidget(self.create_button(Command.RELAX_RIGHT_ARM))
misc_grid2.addWidget(self.create_button(Command.RELAX_LEFT_ARM))
misc_grid2.addWidget(self.create_button(Command.FREEZE_RIGHT_ARM))
misc_grid2.addWidget(self.create_button(Command.FREEZE_LEFT_ARM))
misc_grid2.addStretch(1)
misc_grid3 = QtGui.QHBoxLayout()
misc_grid3.addWidget(self.create_button(Command.OPEN_RIGHT_HAND))
misc_grid3.addWidget(self.create_button(Command.OPEN_LEFT_HAND))
misc_grid3.addWidget(self.create_button(Command.CLOSE_RIGHT_HAND))
misc_grid3.addWidget(self.create_button(Command.CLOSE_LEFT_HAND))
misc_grid3.addStretch(1)
misc_grid4 = QtGui.QHBoxLayout()
misc_grid4.addWidget(self.create_button(Command.PREV_ACTION))
misc_grid4.addWidget(self.create_button(Command.NEXT_ACTION))
misc_grid4.addStretch(1)
speechGroupBox = QGroupBox('Robot Speech', self._widget)
speechGroupBox.setObjectName('RobotSpeechGroup')
speechBox = QtGui.QHBoxLayout()
self.speechLabel = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speechLabel.setPalette(palette)
speechBox.addWidget(self.speechLabel)
speechGroupBox.setLayout(speechBox)
allWidgetsBox.addWidget(actionBox)
allWidgetsBox.addLayout(actionButtonGrid)
allWidgetsBox.addWidget(self.stepsBox)
allWidgetsBox.addLayout(stepsButtonGrid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid2)
allWidgetsBox.addLayout(misc_grid3)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addLayout(misc_grid4)
allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20))
allWidgetsBox.addWidget(speechGroupBox)
allWidgetsBox.addStretch(1)
# Fix layout and add main widget to the user interface
QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique'))
vAllBox = QtGui.QVBoxLayout()
vAllBox.addLayout(allWidgetsBox)
vAllBox.addStretch(1)
hAllBox = QtGui.QHBoxLayout()
hAllBox.addLayout(vAllBox)
hAllBox.addStretch(1)
self._widget.setObjectName('PbDGUI')
self._widget.setLayout(hAllBox)
context.add_widget(self._widget)
rospy.loginfo('Will wait for the experiment state service...')
rospy.wait_for_service('get_experiment_state')
exp_state_srv = rospy.ServiceProxy('get_experiment_state',
GetExperimentState)
rospy.loginfo('Got response from the experiment state service...')
response = exp_state_srv()
self.update_state(response.state)
@staticmethod
def loginfo(message):
'''Because all other ROS logging has some kind of information
about what's being emitted, and qWarning doesn't, we're going to
wrap the function to provide a little bit of additional info for
newbies.
Args:
message (str): The message to log
'''
qWarning('[INFO] [pbd_gui.py] ' + message)
def _create_table_view(self, model, row_click_cb):
proxy = QtGui.QSortFilterProxyModel(self)
proxy.setSourceModel(model)
view = QtGui.QTableView(self._widget)
verticalHeader = view.verticalHeader()
verticalHeader.sectionClicked.connect(row_click_cb)
view.setModel(proxy)
view.setMaximumWidth(250)
view.setSortingEnabled(False)
view.setCornerButtonEnabled(False)
return view
def get_uid(self, arm_index, index):
'''Returns a unique id of the marker'''
return (2 * (index + 1) + arm_index)
def get_arm_and_index(self, uid):
'''Returns a unique id of the marker'''
arm_index = uid % 2
index = (uid - arm_index) / 2
return (arm_index, (index - 1))
def r_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(0, logicalIndex))
def l_row_clicked_cb(self, logicalIndex):
self.step_pressed(self.get_uid(1, logicalIndex))
def create_button(self, command):
btn = QtGui.QPushButton(self.commands[command], self._widget)
btn.clicked.connect(self.command_cb)
return btn
def update_state(self, state):
PbDGUI.loginfo('Received new state')
n_actions = len(self.actionIcons.keys())
if n_actions < state.n_actions:
for i in range(n_actions, state.n_actions):
self.new_action()
if (self.currentAction != (state.i_current_action - 1)):
self.delete_all_steps()
self.action_pressed(state.i_current_action - 1, False)
n_steps = self.n_steps()
if (n_steps < state.n_steps):
for i in range(n_steps, state.n_steps):
self.save_pose()
elif (n_steps > state.n_steps):
n_to_remove = n_steps - state.n_steps
self.r_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
self.l_model.invisibleRootItem().removeRows(
state.n_steps, n_to_remove)
## TODO: DEAL with the following arrays!!!
state.r_gripper_states
state.l_gripper_states
state.r_ref_frames
state.l_ref_frames
state.objects
if (self.currentStep != state.i_current_step):
if (self.n_steps() > 0):
self.currentStep = state.i_current_step
arm_index, index = self.get_arm_and_index(self.currentStep)
if (arm_index == 0):
self.r_view.selectRow(index)
else:
self.l_view.selectRow(index)
def save_pose(self, actionIndex=None):
nColumns = 9
if actionIndex is None:
actionIndex = self.currentAction
stepIndex = self.n_steps(actionIndex)
r_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem('Go to pose'),
QtGui.QStandardItem('Absolute')
]
l_step = [
QtGui.QStandardItem('Step' + str(stepIndex + 1)),
QtGui.QStandardItem('Go to pose'),
QtGui.QStandardItem('Absolute')
]
self.r_model.invisibleRootItem().appendRow(r_step)
self.l_model.invisibleRootItem().appendRow(l_step)
self.update_table_view()
self.currentStep = stepIndex
def update_table_view(self):
self.l_view.setColumnWidth(0, 50)
self.l_view.setColumnWidth(1, 100)
self.l_view.setColumnWidth(2, 70)
self.r_view.setColumnWidth(0, 50)
self.r_view.setColumnWidth(1, 100)
self.r_view.setColumnWidth(2, 70)
def n_steps(self, actionIndex=None):
return self.l_model.invisibleRootItem().rowCount()
def delete_all_steps(self, actionIndex=None):
if actionIndex is None:
actionIndex = self.currentAction
n_steps = self.n_steps()
if (n_steps > 0):
self.l_model.invisibleRootItem().removeRows(0, n_steps)
self.r_model.invisibleRootItem().removeRows(0, n_steps)
def n_actions(self):
return len(self.actionIcons.keys())
def new_action(self):
nColumns = 6
actionIndex = self.n_actions()
for key in self.actionIcons.keys():
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed)
self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns),
actionIndex % nColumns)
self.actionIcons[actionIndex] = actIcon
def step_pressed(self, step_index):
gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index)
self.gui_cmd_publisher.publish(gui_cmd)
def action_pressed(self, actionIndex, isPublish=True):
for i in range(len(self.actionIcons.keys())):
key = self.actionIcons.keys()[i]
if key == actionIndex:
self.actionIcons[key].selected = True
else:
self.actionIcons[key].selected = False
self.actionIcons[key].updateView()
self.currentAction = actionIndex
self.stepsBox.setTitle('Steps for Action ' +
str(self.currentAction + 1))
if isPublish:
gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION,
(actionIndex + 1))
self.gui_cmd_publisher.publish(gui_cmd)
def command_cb(self):
clickedButtonName = self._widget.sender().text()
for key in self.commands.keys():
if (self.commands[key] == clickedButtonName):
PbDGUI.loginfo('Sending speech command: ' + key)
command = Command()
command.command = key
self.speech_cmd_publisher.publish(command)
def robotSoundReceived(self, soundReq):
if (soundReq.command == SoundRequest.SAY):
PbDGUI.loginfo('Robot said: ' + soundReq.arg)
self.speechLabel.setText('Robot sound: ' + soundReq.arg)
def exp_state_cb(self, state):
self.exp_state_sig.emit(state)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.speech_cmd_publisher.unregister()
self.gui_cmd_publisher.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class HalloweenGUI(Plugin):
joint_sig = Signal(JointState)
def __init__(self, context):
super(HalloweenGUI, self).__init__(context)
self.setObjectName('HalloweenGUI')
self._widget = QWidget()
self._widget.setFixedSize(525, 300)
self.arm_db = ArmDB()
self._tf_listener = TransformListener()
# Action/service/message clients or servers
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
self.saved_r_arm_pose = None
self.saved_l_arm_pose = None
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
large_box = QtGui.QVBoxLayout()
arm_box = QtGui.QHBoxLayout()
right_arm_box = QtGui.QVBoxLayout()
left_arm_box = QtGui.QVBoxLayout()
left_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addWidget(self.create_button('Relax right arm'))
right_arm_box.addWidget(self.create_button('Freeze right arm'))
left_arm_box.addWidget(self.create_button('Relax left arm'))
left_arm_box.addWidget(self.create_button('Freeze left arm'))
left_arm_box.addItem(QtGui.QSpacerItem(50, 20))
right_arm_box.addItem(QtGui.QSpacerItem(50, 20))
left_pose_saver = PoseSaver(PoseSaver.LEFT, self)
right_pose_saver = PoseSaver(PoseSaver.RIGHT, self)
left_arm_box.addWidget(
self.create_button("Create left arm pose",
left_pose_saver.create_closure()))
right_arm_box.addWidget(
self.create_button("Create right arm pose",
right_pose_saver.create_closure()))
left_arm_box.addItem(QtGui.QSpacerItem(50, 20))
right_arm_box.addItem(QtGui.QSpacerItem(50, 20))
# Dropdown boxes for saved poses
left_pose_loader = PoseLoader(PoseLoader.LEFT, self)
right_pose_loader = PoseLoader(PoseLoader.RIGHT, self)
self.combo_box_left = left_pose_loader.create_button()
self.combo_box_right = right_pose_loader.create_button()
left_arm_box.addWidget(self.combo_box_left)
right_arm_box.addWidget(self.combo_box_right)
left_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box.addWidget(self.create_button('Move to pose (R)'))
left_pose_option_box.addWidget(self.create_button('Move to pose (L)'))
# Buttons for deleting poses for left/right arms
left_pose_option_box.addWidget(self.create_button('Delete pose (L)'))
right_pose_option_box.addWidget(self.create_button('Delete pose (R)'))
left_arm_box.addLayout(left_pose_option_box)
right_arm_box.addLayout(right_pose_option_box)
left_arm_box.addItem(QtGui.QSpacerItem(50, 50))
right_arm_box.addItem(QtGui.QSpacerItem(50, 50))
arm_box.addLayout(left_arm_box)
arm_box.addItem(QtGui.QSpacerItem(20, 20))
arm_box.addLayout(right_arm_box)
large_box.addLayout(arm_box)
# Initialize state of saved arm poses for selected dropdowns
self.update_saved_l_arm_pose()
self.update_saved_r_arm_pose()
# Update saved arm pose data on the changing of selected pose
self.combo_box_left.connect(
self.combo_box_left, QtCore.SIGNAL("currentIndexChanged(QString)"),
self.update_saved_l_arm_pose)
self.combo_box_right.connect(
self.combo_box_right,
QtCore.SIGNAL("currentIndexChanged(QString)"),
self.update_saved_r_arm_pose)
self._widget.setObjectName('HalloweenGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../arm_gui_bg_large.png"))
rospy.loginfo('GUI initialization complete.')
def create_button(self, name, method=None):
if method == None:
method = self.command_cb
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax right arm'):
self.relax_arm('r')
elif (button_name == 'Freeze right arm'):
self.freeze_arm('r')
elif (button_name == 'Relax left arm'):
self.relax_arm('l')
elif (button_name == 'Freeze left arm'):
self.freeze_arm('l')
elif (button_name == 'Move to pose (R)'):
self.move_arm('r')
elif (button_name == 'Move to pose (L)'):
self.move_arm('l')
elif (button_name == 'Delete pose (L)'):
self.delete_pose_left()
elif (button_name == 'Delete pose (R)'):
self.delete_pose_right()
def update_saved_l_arm_pose(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index == -1):
self.saved_l_arm_pose = None
else:
self.saved_l_arm_pose = self.combo_box_left.itemData(
selected_index)
def update_saved_r_arm_pose(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index == -1):
self.saved_r_arm_pose = None
else:
self.saved_r_arm_pose = self.combo_box_right.itemData(
selected_index)
def delete_pose_left(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('l',
self.combo_box_left.itemText(selected_index))
self.combo_box_left.removeItem(selected_index)
def delete_pose_right(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('r',
self.combo_box_right.itemText(selected_index))
self.combo_box_right.removeItem(selected_index)
def pose_distance(self, source, dest):
dist = 0
dist += (source.position.x - dest.position.x) ^ 2
dist += (source.position.y - dest.position.y) ^ 2
dist += (source.position.z - dest.position.z) ^ 2
return dist
def move_arm(self, side_prefix):
# forward kinematics
if (side_prefix == 'r'):
if self.saved_r_arm_pose is None:
rospy.logerr('Target pose for right arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
time_to_joints = 2.0
self.move_to_joints(side_prefix, self.saved_r_arm_pose,
time_to_joints)
else: # side_prefix == 'l'
if self.saved_l_arm_pose is None:
rospy.logerr('Target pose for left arm is None, cannot move.')
else:
self.freeze_arm(side_prefix)
time_to_joints = 2.0
self.move_to_joints(side_prefix, self.saved_l_arm_pose,
time_to_joints)
pass
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SystemCommandDialog(Plugin):
def __init__(self, context):
super(SystemCommandDialog, self).__init__(context)
self.setObjectName('SystemCommandDialog')
self.sys_command_pub = rospy.Publisher('/syscommand',String, None, False, True, None, queue_size=10)
self._widget = QWidget()
vbox = QVBoxLayout()
reset_world_command = QPushButton("Reset World Model")
reset_world_command.clicked.connect(self.apply_reset_world_command_callback)
vbox.addWidget(reset_world_command)
save_octomap = QPushButton("Save Octomap")
save_octomap.clicked.connect(self.apply_save_octomap_command_callback)
vbox.addWidget(save_octomap)
save_pointcloud = QPushButton("Save Pointcloud")
save_pointcloud.clicked.connect(self.apply_save_pointcloud_command_callback)
vbox.addWidget(save_pointcloud)
save_image_left_eye = QPushButton("Save Image Head")
save_image_left_eye.clicked.connect(self.apply_save_image_left_eye_command_callback)
vbox.addWidget(save_image_left_eye)
save_image_left_hand = QPushButton("Save Left Hand Image")
save_image_left_hand.clicked.connect(self.apply_save_image_left_hand_command_callback)
vbox.addWidget(save_image_left_hand)
save_image_right_hand = QPushButton("Save Right Hand Image")
save_image_right_hand.clicked.connect(self.apply_save_image_right_hand_command_callback)
vbox.addWidget(save_image_right_hand)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
def shutdown_plugin(self):
print "Shutting down ..."
self.sys_command_pub.unregister()
print "Done!"
# Define system command strings
def apply_reset_world_command_callback(self):
msg = String("reset")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
def apply_save_octomap_command_callback(self):
msg = String("save_octomap")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
def apply_save_pointcloud_command_callback(self):
msg = String("save_pointcloud")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
def apply_save_image_left_eye_command_callback(self):
msg = String("save_image_left_eye")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
def apply_save_image_left_hand_command_callback(self):
msg = String("save_image_left_hand")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
def apply_save_image_right_hand_command_callback(self):
msg = String("save_image_right_hand")
print "Send system command = <",msg.data,">"
self.sys_command_pub.publish(msg)
class ArmGUI(Plugin):
joint_sig = Signal(JointState)
def __init__(self, context):
super(ArmGUI, self).__init__(context)
self.setObjectName('ArmGUI')
self._widget = QWidget()
self._widget.setFixedSize(525, 300)
self.arm_db = ArmDB()
# Action/service/message clients or servers
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(switch_srv_name,
SwitchController)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
self.saved_r_arm_pose = None
self.saved_l_arm_pose = None
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
large_box = QtGui.QVBoxLayout()
arm_box = QtGui.QHBoxLayout()
right_arm_box = QtGui.QVBoxLayout()
left_arm_box = QtGui.QVBoxLayout()
left_arm_box.addItem(QtGui.QSpacerItem(50,50))
right_arm_box.addItem(QtGui.QSpacerItem(50,50))
right_arm_box.addWidget(self.create_button('Relax right arm'))
right_arm_box.addWidget(self.create_button('Freeze right arm'))
left_arm_box.addWidget(self.create_button('Relax left arm'))
left_arm_box.addWidget(self.create_button('Freeze left arm'))
left_arm_box.addItem(QtGui.QSpacerItem(50,20))
right_arm_box.addItem(QtGui.QSpacerItem(50,20))
left_pose_saver = PoseSaver(PoseSaver.LEFT, self)
right_pose_saver = PoseSaver(PoseSaver.RIGHT, self)
left_arm_box.addWidget(self.create_button("Create left arm pose",
left_pose_saver.create_closure()))
right_arm_box.addWidget(self.create_button("Create right arm pose",
right_pose_saver.create_closure()))
left_arm_box.addItem(QtGui.QSpacerItem(50,20))
right_arm_box.addItem(QtGui.QSpacerItem(50,20))
# Dropdown boxes for saved poses
left_pose_loader = PoseLoader(PoseLoader.LEFT, self)
right_pose_loader = PoseLoader(PoseLoader.RIGHT, self)
self.combo_box_left = left_pose_loader.create_button()
self.combo_box_right = right_pose_loader.create_button()
left_arm_box.addWidget(self.combo_box_left)
right_arm_box.addWidget(self.combo_box_right)
left_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box = QtGui.QHBoxLayout()
right_pose_option_box.addWidget(self.create_button('Move to pose (R)'))
left_pose_option_box.addWidget(self.create_button('Move to pose (L)'))
# Buttons for deleting poses for left/right arms
left_pose_option_box.addWidget(self.create_button('Delete pose (L)'))
right_pose_option_box.addWidget(self.create_button('Delete pose (R)'))
left_arm_box.addLayout(left_pose_option_box)
right_arm_box.addLayout(right_pose_option_box)
left_arm_box.addItem(QtGui.QSpacerItem(50,50))
right_arm_box.addItem(QtGui.QSpacerItem(50,50))
arm_box.addLayout(left_arm_box)
arm_box.addItem(QtGui.QSpacerItem(20, 20))
arm_box.addLayout(right_arm_box)
large_box.addLayout(arm_box)
# Initialize state of saved arm poses for selected dropdowns
self.update_saved_l_arm_pose()
self.update_saved_r_arm_pose()
# Update saved arm pose data on the changing of selected pose
self.combo_box_left.connect(self.combo_box_left,
QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_l_arm_pose)
self.combo_box_right.connect(self.combo_box_right,
QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_r_arm_pose)
self._widget.setObjectName('ArmGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet("QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../arm_gui_bg_large.png"))
rospy.loginfo('GUI initialization complete.')
def create_button(self, name, method=None):
if method == None:
method = self.command_cb
btn = QtGui.QPushButton(name, self._widget)
btn.clicked.connect(method)
return btn
def command_cb(self):
button_name = self._widget.sender().text()
if (button_name == 'Relax right arm'):
self.relax_arm('r')
elif (button_name == 'Freeze right arm'):
self.freeze_arm('r')
elif (button_name == 'Relax left arm'):
self.relax_arm('l')
elif (button_name == 'Freeze left arm'):
self.freeze_arm('l')
elif (button_name == 'Move to pose (R)'):
self.move_arm('r')
elif (button_name == 'Move to pose (L)'):
self.move_arm('l')
elif (button_name == 'Delete pose (L)'):
self.delete_pose_left()
elif (button_name == 'Delete pose (R)'):
self.delete_pose_right()
def update_saved_l_arm_pose(self):
selected_index = self.combo_box_left.currentIndex()
if(selected_index == -1):
self.saved_l_arm_pose = None
else:
self.saved_l_arm_pose = self.combo_box_left.itemData(selected_index)
def update_saved_r_arm_pose(self):
selected_index = self.combo_box_right.currentIndex()
if(selected_index == -1):
self.saved_r_arm_pose = None
else:
self.saved_r_arm_pose = self.combo_box_right.itemData(selected_index)
def delete_pose_left(self):
selected_index = self.combo_box_left.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('l', self.combo_box_left.itemText(selected_index))
self.combo_box_left.removeItem(selected_index)
def delete_pose_right(self):
selected_index = self.combo_box_right.currentIndex()
if (selected_index != -1):
self.arm_db.rmPos('r', self.combo_box_right.itemText(selected_index))
self.combo_box_right.removeItem(selected_index)
def move_arm(self, side_prefix):
if (side_prefix == 'r'):
if self.saved_r_arm_pose is None:
rospy.logerr('Target pose for right arm is None, cannot move.')
else:
self.freeze_arm('r')
self.move_to_joints('r', self.saved_r_arm_pose, 2.0)
else:
if self.saved_l_arm_pose is None:
rospy.logerr('Target pose for left arm is None, cannot move.')
else:
self.freeze_arm('l')
self.move_to_joints('l', self.saved_l_arm_pose, 2.0)
pass
def move_to_joints(self, side_prefix, positions, time_to_joint):
'''Moves the arm to the desired joints'''
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
if (side_prefix == 'r'):
traj_goal.trajectory.joint_names = self.r_joint_names
self.r_traj_action_client.send_goal(traj_goal)
else:
traj_goal.trajectory.joint_names = self.l_joint_names
self.l_traj_action_client.send_goal(traj_goal)
def relax_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = []
stop_controllers = [controller_name]
self.set_arm_mode(start_controllers, stop_controllers)
def freeze_arm(self, side_prefix):
controller_name = side_prefix + '_arm_controller'
start_controllers = [controller_name]
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def set_arm_mode(self, start_controllers, stop_controllers):
try:
self.switch_service_client(start_controllers, stop_controllers, 1)
except rospy.ServiceException:
rospy.logerr('Could not change arm mode.')
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.joint_sig.emit(msg)
self.lock.release()
def joint_sig_cb(self, msg):
pass
def get_joint_state(self, side_prefix):
'''Returns position for arm joints on the requested side (r/l)'''
if side_prefix == 'r':
joint_names = self.r_joint_names
else:
joint_names = self.l_joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received yet!\n")
return None
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return None
self.lock.release()
return positions
def shutdown_plugin(self):
# TODO unregister all publishers here
# Leave both arm controllers on
start_controllers = ['r_arm_controller', 'l_arm_controller']
stop_controllers = []
self.set_arm_mode(start_controllers, stop_controllers)
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class TrapezoidalTrajectoryDialog(Plugin):
updateStateSignal = Signal(object)
def __init__(self, context):
super(TrapezoidalTrajectoryDialog, self).__init__(context)
self.setObjectName("TrapezoidalTrajectoryDialog")
# self.updateStateSignal = Signal(object)
self.updateStateSignal.connect(self.on_updateState)
self.robot = URDF.from_parameter_server()
self.joint_list = {}
for ndx, jnt in enumerate(self.robot.joints):
self.joint_list[jnt.name] = ndx
self.chain = []
self.chain_file = rospy.get_param("~chain_file")
self.chain_name = rospy.get_param("~chain_name")
print
print "Define order of splines used to approximate trapezoid"
self.spline_order = rospy.get_param("~spline_order", 5) # 1 # 3 # 5 # linear, cubic, quintic
if (self.spline_order == 1) or (self.spline_order == 3) or (self.spline_order == 5):
print "Spline order=", self.spline_order
else:
print "Invalid spline order! Must be 1, 3, or 5"
print "Spline order=", self.spline_order
sys.exit(-1)
yaml_file = self.chain_file + self.chain_name + "_chain.yaml"
print "Chain definition file:"
print yaml_file
print
# define structures
self.robot_state = JointState()
self.robot_command = JointTrajectory()
stream = open(yaml_file, "r")
jointChain = yaml.load_all(stream)
for ndx, data in enumerate(jointChain):
print ndx, " : ", data
self.delay_time = data["delay_time"]
self.ramp_up_time = data["ramp_up_time"]
self.dwell_time = data["dwell_time"]
self.ramp_down_time = data["ramp_down_time"]
self.hold_time = data["hold_time"]
self.ramp_start_fraction = data["ramp_start_fraction"]
self.ramp_end_fraction = data["ramp_end_fraction"]
self.joint_state_topic = data["joint_state_topic"]
self.trajectory_topic = data["trajectory_topic"]
if rospy.search_param(data["chain_param_name"]):
print "Found ", data["chain_param_name"]
else:
print "Failed to find the ", data["chain_param_name"], " in the parameter server!"
sys.exit(-1)
joint_names = rospy.get_param(data["chain_param_name"])
for joint in joint_names:
print joint
self.robot_state.name.append(joint)
self.chain.append(JointData(self, joint))
self.robot_command.joint_names = self.robot_state.name
stream.close()
self.robot_state.position = [0.0] * len(self.robot_state.name)
self.robot_state.velocity = [0.0] * len(self.robot_state.name)
self.robot_state.effort = [0.0] * len(self.robot_state.name)
self.robot_joint_state = JointState()
print "delay_time =", self.delay_time
print "ramp_up_time =", self.ramp_up_time
print "dwell_time =", self.dwell_time
print "ramp_down_time=", self.ramp_down_time
print "hold_time =", self.hold_time
print "spline order =", self.spline_order
if (
(self.ramp_start_fraction < 0.001)
or (self.ramp_end_fraction > 0.999)
or (self.ramp_start_fraction >= self.ramp_end_fraction)
):
print "Invalid ramp fractions - abort!"
print "0.0 < ", self.ramp_start_fraction, " < ", self.ramp_end_fraction, " < 1.0"
return
print "Robot State Structure", self.robot_state
print "Robot Command Structure", self.robot_command
# initialize structure to hold widget handles
self.cmd_spinbox = []
self.ramp_up_spinbox = []
self.ramp_down_spinbox = []
self._widget = QWidget()
vbox = QVBoxLayout()
# Push buttons
hbox = QHBoxLayout()
snap_command = QPushButton("Snap Position")
snap_command.clicked.connect(self.snap_current_callback)
hbox.addWidget(snap_command)
check_limits = QPushButton("Check Limits Gains")
check_limits.clicked.connect(self.check_limits_callback)
hbox.addWidget(check_limits)
apply_command = QPushButton("Send Trajectory")
apply_command.clicked.connect(self.apply_command_callback)
hbox.addWidget(apply_command)
save_trajectory = QPushButton("Save Trajectory")
save_trajectory.clicked.connect(self.save_trajectory_callback)
hbox.addWidget(save_trajectory)
zero_ramp = QPushButton("Zero Ramps")
zero_ramp.clicked.connect(self.zero_ramp_callback)
hbox.addWidget(zero_ramp)
vbox.addLayout(hbox)
time_hbox = QHBoxLayout()
vbox_delay = QVBoxLayout()
vbox_delay.addWidget(QLabel("Delay"))
self.delay_time_spinbox = QDoubleSpinBox()
self.delay_time_spinbox.setDecimals(5)
self.delay_time_spinbox.setRange(0, 10.0)
self.delay_time_spinbox.setSingleStep(0.1)
self.delay_time_spinbox.valueChanged.connect(self.on_delay_time_value)
self.delay_time_spinbox.setValue(self.delay_time)
vbox_delay.addWidget(self.delay_time_spinbox)
time_hbox.addLayout(vbox_delay)
vbox_ramp_up = QVBoxLayout()
vbox_ramp_up.addWidget(QLabel("Ramp Up"))
self.ramp_up_time_spinbox = QDoubleSpinBox()
self.ramp_up_time_spinbox.setDecimals(5)
self.ramp_up_time_spinbox.setRange(0, 10.0)
self.ramp_up_time_spinbox.setSingleStep(0.1)
self.ramp_up_time_spinbox.valueChanged.connect(self.on_ramp_up_time_value)
self.ramp_up_time_spinbox.setValue(self.ramp_up_time)
vbox_ramp_up.addWidget(self.ramp_up_time_spinbox)
time_hbox.addLayout(vbox_ramp_up)
#
vbox_dwell = QVBoxLayout()
vbox_dwell.addWidget(QLabel("Dwell"))
self.dwell_time_spinbox = QDoubleSpinBox()
self.dwell_time_spinbox.setDecimals(5)
self.dwell_time_spinbox.setRange(0, 10.0)
self.dwell_time_spinbox.setSingleStep(0.1)
self.dwell_time_spinbox.valueChanged.connect(self.on_dwell_time_value)
self.dwell_time_spinbox.setValue(self.dwell_time)
vbox_dwell.addWidget(self.dwell_time_spinbox)
time_hbox.addLayout(vbox_dwell)
vbox_ramp_down = QVBoxLayout()
vbox_ramp_down.addWidget(QLabel("Down"))
self.ramp_down_time_spinbox = QDoubleSpinBox()
self.ramp_down_time_spinbox.setDecimals(5)
self.ramp_down_time_spinbox.setRange(0, 10.0)
self.ramp_down_time_spinbox.setSingleStep(0.1)
self.ramp_down_time_spinbox.valueChanged.connect(self.on_ramp_down_time_value)
self.ramp_down_time_spinbox.setValue(self.ramp_down_time)
vbox_ramp_down.addWidget(self.ramp_down_time_spinbox)
time_hbox.addLayout(vbox_ramp_down)
vbox_hold = QVBoxLayout()
vbox_hold.addWidget(QLabel("Hold"))
self.hold_time_spinbox = QDoubleSpinBox()
self.hold_time_spinbox.setDecimals(5)
self.hold_time_spinbox.setRange(0, 10.0)
self.hold_time_spinbox.setSingleStep(0.1)
self.hold_time_spinbox.valueChanged.connect(self.on_hold_time_value)
self.hold_time_spinbox.setValue(self.hold_time)
vbox_hold.addWidget(self.hold_time_spinbox)
time_hbox.addLayout(vbox_hold)
vbox.addLayout(time_hbox)
# Joints title
title_frame = QFrame()
title_frame.setFrameShape(QFrame.StyledPanel)
title_frame.setFrameShadow(QFrame.Raised)
title_hbox = QHBoxLayout()
title_hbox.addWidget(QLabel("Joints"))
title_hbox.addWidget(QLabel("Start"))
title_hbox.addWidget(QLabel("Ramp Up"))
title_hbox.addWidget(QLabel("Ramp Down"))
title_frame.setLayout(title_hbox)
vbox.addWidget(title_frame)
# Define the widgets for each joint
for i, joint in enumerate(self.chain):
# print i,",",joint
self.joint_widget(vbox, i)
# add stretch at end so all GUI elements are at top of dialog
vbox.addStretch(1)
self._widget.setLayout(vbox)
# Define the connections to the outside world
self.jointSubscriber = rospy.Subscriber(self.joint_state_topic, JointState, self.stateCallbackFnc)
self.commandPublisher = rospy.Publisher(self.trajectory_topic, JointTrajectory, queue_size=10)
# Add the widget
context.add_widget(self._widget)
@Slot()
def snap_current_callback(self):
self.blockSignals(True)
print "Snapping positions to actual values"
for index, joint in enumerate(self.chain):
for index_state, name_state in enumerate(self.robot_joint_state.name):
if name_state == joint.name:
joint.position = copy.deepcopy(self.robot_joint_state.position[index_state])
self.cmd_spinbox[index].setValue(joint.position)
break
self.blockSignals(False)
@Slot()
def check_limits_callback(self):
self.blockSignals(True)
print "Check limits callback ..."
valid = True
for index, joint in enumerate(self.chain):
ramp_up = joint.position + joint.ramp_up
ramp_down = joint.ramp_down + ramp_up
p = self.ramp_up_spinbox[index].palette()
if (ramp_up > joint.upper_limit) or (ramp_up < joint.lower_limit):
print "Joint ", joint.name, " is over limit on ramp up!"
valid = False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white) # <<<<<<<<<<<----------------- This isn't working as intended
# print joint.lower_limit," < ", ramp_up, " < ",joint.upper_limit
self.ramp_up_spinbox[index].setPalette(p)
if (ramp_down > joint.upper_limit) or (ramp_down < joint.lower_limit):
print "Joint ", joint.name, " is over limit on ramp down!"
valid = False
p.setColor(QPalette.Window, Qt.red) # <<<<<<<<<<<----------------- This isn't working as intended
else:
p.setColor(QPalette.Window, Qt.white) # <<<<<<<<<<<----------------- This isn't working as intended
# print joint.lower_limit," < ", ramp_down, " < ",joint.upper_limit
if not valid:
print "Invalid joint limits on ramp!"
else:
print " Valid ramp!"
self.blockSignals(False)
return valid
@Slot()
def zero_ramp_callback(self):
self.blockSignals(True)
print "Zero ramps callback ..."
for index, joint in enumerate(self.chain):
self.ramp_up_spinbox[index].setValue(0.0)
self.ramp_down_spinbox[index].setValue(0.0)
self.blockSignals(False)
@Slot()
def apply_command_callback(self):
self.blockSignals(True)
print "Send trajectory"
if self.calculate_trajectory():
# print self.robot_command;
self.commandPublisher.publish(self.robot_command)
else:
print "Trajectory calculation failure - do not publish!"
self.blockSignals(False)
@Slot()
def save_trajectory_callback(self):
self.blockSignals(True)
print "Save gains"
# Save data to file that we can read in
# TODO - invalid file names
fileName = QFileDialog.getSaveFileName()
# if fileName[0] checks to ensure that the file dialog has not been canceled
if fileName[0]:
if self.calculate_trajectory():
print self.robot_command
newFileptr = open(fileName[0], "w")
# Suggested format for files to make it easy to combine different outputs
newFileptr.write("# Trajectory \n")
newFileptr.write(self.robot_command)
newFileptr.write("\n")
newFileptr.close()
else:
print "Trajectory calculation failure - do not save!"
else:
print "Save cancelled."
newFilePtr.close()
self.blockSignals(False)
#
@Slot()
def stateCallbackFnc(self, atlasState_msg):
# Handle the state message for actual positions
time = atlasState_msg.header.stamp.to_sec()
if (time - self.prior_time) > 0.02:
# Only update at 50hz
# relay the ROS messages through a Qt Signal to switch to the GUI thread
self.updateStateSignal.emit(atlasState_msg)
self.prior_time = time
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, joint_state_msg):
# print joint_state_msg
self.robot_joint_state = joint_state_msg
def on_delay_time_value(self, value):
self.delay_time = copy.deepcopy(value)
def on_ramp_up_time_value(self, value):
self.ramp_up_time = copy.deepcopy(value)
def on_ramp_down_time_value(self, value):
self.ramp_down_time = copy.deepcopy(value)
def on_dwell_time_value(self, value):
self.dwell_time = copy.deepcopy(value)
def on_hold_time_value(self, value):
self.hold_time = copy.deepcopy(value)
def calculate_trajectory(self):
if not self.check_limits_callback():
print "Invalid limits for trajectory!"
return False
knot_points = 1
if self.delay_time > 0.002:
knot_points += 1
if self.ramp_up_time > 0.002:
knot_points += 1
if self.dwell_time > 0.002:
knot_points += 1
if self.ramp_down_time > 0.002:
knot_points += 1
if self.hold_time > 0.002:
knot_points += 1
if knot_points < 2:
print "Invalid trajectory - knot_points = ", knot_points
return False
# print "Minimum knot points=",knot_points
self.robot_command.points = []
self.robot_command.points.append(JointTrajectoryPoint())
ros_time_offset = rospy.Duration(0.0)
ndx = 0
self.robot_command.points[ndx].time_from_start = ros_time_offset
for jnt, joint in enumerate(self.chain):
self.robot_command.points[ndx].positions.append(joint.position)
if self.spline_order > 1:
self.robot_command.points[ndx].velocities.append(0.0)
if self.spline_order > 3:
self.robot_command.points[ndx].accelerations.append(0.0)
ndx += 1
if self.delay_time > 0.002:
ros_time_offset += rospy.Duration(self.delay_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
if self.ramp_up_time > 0.002:
ramp_up_count = self.calculate_ramp_up_section(ndx)
if ramp_up_count:
ndx += ramp_up_count
ros_time_offset += rospy.Duration(self.ramp_up_time)
else:
return False # Invalid ramp calculation
if self.dwell_time > 0.002:
ros_time_offset += rospy.Duration(self.dwell_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
if self.ramp_down_time > 0.002:
ramp_dn_count = self.calculate_ramp_down_section(ndx)
if ramp_dn_count > 0:
ndx += ramp_dn_count
ros_time_offset += rospy.Duration(self.ramp_down_time)
else:
return False # Invalid ramp calculation
if self.hold_time > 0.002:
ros_time_offset += rospy.Duration(self.hold_time)
self.robot_command.points.append(copy.deepcopy(self.robot_command.points[ndx - 1]))
self.robot_command.points[ndx].time_from_start = ros_time_offset
ndx += 1
# print self.robot_command
# print "Ndx=",ndx, " of ",knot_points, "=",len(self.robot_command.points)
if ndx != len(self.robot_command.points):
print "Invalid number of knot points - ignore trajectory"
print "Ndx=", ndx, " of ", len(self.robot_command.points), " = ", knot_points
print self.robot_command
return False
self.robot_command.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
return True
def calculate_ramp_up_section(self, ndx):
prior = self.robot_command.points[ndx - 1]
# Create next 3 knot points
self.robot_command.points.append(copy.deepcopy(prior))
if self.spline_order > 1:
# Add transition points
self.robot_command.points.append(copy.deepcopy(prior))
self.robot_command.points.append(copy.deepcopy(prior))
dT0 = self.ramp_start_fraction * self.ramp_up_time
dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_up_time
dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_up_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
ndx
].time_from_start + rospy.Duration(dT1)
self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
ndx + 1
].time_from_start + rospy.Duration(dT2)
else:
dT0 = self.ramp_up_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
# Now calculate the interior values for each joint individually
for jnt, joint in enumerate(self.chain):
if joint.ramp_up != 0.0:
starting_position = copy.deepcopy(prior.positions[jnt])
ending_position = starting_position + joint.ramp_up
print "calculating ramp up for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_up_time, " seconds"
if self.spline_order > 1:
# Either quintic or cubic - either way we have transition -> linear -> transition
# for quintic,
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 unknowns
x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
# first interior point
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
# linear segment
self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
# last interior point
self.robot_command.points[ndx + 2].positions[jnt] = ending_position
self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
else:
# Piecewise linear
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)
if self.spline_order > 3:
# Quintic spline
self.robot_command.points[ndx].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0
if self.spline_order < 3:
return 1
return 3 # At least cubic and has 3 transition segments
def calculate_ramp_down_section(self, ndx):
prior = self.robot_command.points[ndx - 1]
# Create next knot points
self.robot_command.points.append(copy.deepcopy(prior))
if self.spline_order > 1:
# Add transition points
self.robot_command.points.append(copy.deepcopy(prior))
self.robot_command.points.append(copy.deepcopy(prior))
dT0 = self.ramp_start_fraction * self.ramp_down_time
dT1 = (self.ramp_end_fraction - self.ramp_start_fraction) * self.ramp_down_time
dT2 = (1.0 - self.ramp_end_fraction) * self.ramp_down_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
self.robot_command.points[ndx + 1].time_from_start = self.robot_command.points[
ndx
].time_from_start + rospy.Duration(dT1)
self.robot_command.points[ndx + 2].time_from_start = self.robot_command.points[
ndx + 1
].time_from_start + rospy.Duration(dT2)
else:
dT0 = self.ramp_down_time
self.robot_command.points[ndx].time_from_start = prior.time_from_start + rospy.Duration(dT0)
# Now calculate the interior values for each joint individually
for jnt, joint in enumerate(self.chain):
if joint.ramp_up != 0.0:
starting_position = copy.deepcopy(prior.positions[jnt])
ending_position = starting_position + joint.ramp_down
print "calculating ramp down for ", joint.name, " from ", starting_position, " to ", ending_position, " over ", self.ramp_down_time, " seconds"
if self.spline_order > 1:
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 unknowns
x = self.solve_ramp_matrices(dT0, dT1, dT2, starting_position, ending_position)
# first interior point
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(x[7][0])
self.robot_command.points[ndx].velocities[jnt] = copy.deepcopy(x[6][0])
# linear segment
self.robot_command.points[ndx + 1].positions[jnt] = copy.deepcopy(x[13][0])
self.robot_command.points[ndx + 1].velocities[jnt] = copy.deepcopy(x[6][0])
# last interior point
self.robot_command.points[ndx + 2].positions[jnt] = ending_position
self.robot_command.points[ndx + 2].velocities[jnt] = 0.0
else:
self.robot_command.points[ndx].positions[jnt] = copy.deepcopy(ending_position)
if self.spline_order > 3:
self.robot_command.points[ndx + 2].accelerations[jnt] = 0.0
self.robot_command.points[ndx].accelerations[jnt] = 0.0
self.robot_command.points[ndx + 1].accelerations[jnt] = 0.0
if self.spline_order < 3:
return 1
return 3 # At least cubic and has 3 transition segments
def solve_ramp_matrices(self, dT0, dT1, dT2, starting_position, ending_position):
if self.spline_order < 4:
# assume cubic spline
return self.solve_ramp_cubic_matrices(dT0, dT1, dT2, starting_position, ending_position)
# This is the quintic version
# Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# 6 + 2 + 6 = 14 unknowns
A = np.zeros((14, 14))
b = np.zeros((14, 1))
# Eqn 1 - Known starting position
A[0][5] = 1
b[0][0] = starting_position
# Eqn 2 - Zero velocity at start
A[1][4] = 1.0
# Eqn 3 - Zero acceleration at start
A[2][3] = 2.0
# Eqn 4 - Continuous velocity at first interior knot point
A[3][0] = 5 * dT0 ** 4
A[3][1] = 4 * dT0 ** 3
A[3][2] = 3 * dT0 ** 2
A[3][3] = 2 * dT0
A[3][4] = 1.0
A[3][5] = 0.0
A[3][6] = -1.0
# Eqn 5 - Zero acceleration at first interior knot point
A[4][0] = 20 * dT0 ** 3
A[4][1] = 12 * dT0 ** 2
A[4][2] = 6 * dT0
A[4][3] = 2
A[4][4] = 0.0
A[4][5] = 0.0
A[4][6] = 0.0
# Eqn 6 - Zero jerk at first interior knot point
A[5][0] = 60 * dT0 ** 2
A[5][1] = 24 * dT0
A[5][2] = 6
A[5][3] = 0.0
A[5][4] = 0.0
A[5][5] = 0.0
A[5][6] = 0.0
# Eqn 7 - Continuous velocity at second interior knot point
A[6][8] = 0.0
A[6][9] = 0.0
A[6][10] = 0.0
A[6][11] = 0.0
A[6][12] = 1.0
A[6][13] = 0.0
A[6][6] = -1.0
# Eqn 8 - Zero acceleration at second interior knot point
A[7][8] = 0.0
A[7][9] = 0.0
A[7][10] = 0.0
A[7][11] = 2
A[7][12] = 0.0
A[7][13] = 0.0
A[7][6] = 0.0
# Eqn 9 - Zero jerk at second interior knot point
A[8][8] = 0.0
A[8][9] = 0.0
A[8][10] = 6
A[8][11] = 0.0
A[8][12] = 0.0
A[8][13] = 0.0
A[8][6] = 0.0
# Eqn 10 - Equal (but unknown) positions at first interior knot point
A[9][0] = dT0 ** 5
A[9][1] = dT0 ** 4
A[9][2] = dT0 ** 3
A[9][3] = dT0 ** 2
A[9][4] = dT0
A[9][5] = 1.0
A[9][7] = -1.0
# Eqn 11 - Equal (but unknown) positions at second interior knot point
A[10][13] = 1.0
A[10][6] = -dT1
A[10][7] = -1.0
# Eqn 12 - Known final position
A[11][8] = dT2 ** 5
A[11][9] = dT2 ** 4
A[11][10] = dT2 ** 3
A[11][11] = dT2 ** 2
A[11][12] = dT2
A[11][13] = 1.0
b[11][0] = ending_position
# Eqn 13 - Known final velocity = 0
A[12][8] = 5 * dT2 ** 4
A[12][9] = 4 * dT2 ** 3
A[12][10] = 3 * dT2 ** 2
A[12][11] = 2 * dT2
A[12][12] = 1.0
A[12][13] = 0.0
# Eqn 14 - Known final acceleration = 0
A[13][8] = 20 * dT2 ** 3
A[13][9] = 12 * dT2 ** 2
A[13][10] = 6 * dT2
A[13][11] = 2.0
A[13][12] = 0.0
A[13][13] = 0.0
params = np.linalg.solve(A, b)
# np.set_printoptions(precision=9, suppress=True, linewidth=250)
# print "A=",A
# print "b=",b
# print "condition number(A)=",np.linalg.cond(A)
# print "x=",params
return params
def solve_ramp_cubic_matrices(self, dT0, dT1, dT2, starting_position, ending_position):
# This is the cubic version
# Quintic version: Ax=b where x=[a0 b0 c0 d0 e0 f0 e1 f1 a2 b2 c2 d2 e2 f2] with p=a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
# Cubic version: Ax=b where x=[c0 d0 e0 f0 e1 f1 c2 d2 e2 f2] with p= c*t^3 + d*t^2 + e*t +f for first and last, and p=e*t + f for middle
#
# 4 + 2 + 4 = 10 unknowns
A = np.zeros((10, 10))
b = np.zeros((10, 1))
# Eqn 1 - Known starting position
A[0][3] = 1
b[0][0] = starting_position
# Eqn 2 - Zero velocity at start
A[1][2] = 1.0
# Eqn 3 - Continuous velocity at first interior knot point
A[2][0] = 3 * dT0 ** 2
A[2][1] = 2 * dT0
A[2][2] = 1.0
A[2][3] = 0.0
A[2][4] = -1.0
# Eqn 4 - Zero acceleration at first interior knot point
A[3][0] = 6 * dT0
A[3][1] = 2
A[3][2] = 0.0
A[3][3] = 0.0
# Eqn 5 - Continuous velocity at second interior knot point
A[4][6] = 0.0
A[4][7] = 0.0
A[4][8] = 1.0
A[4][9] = 0.0
A[4][4] = -1.0
# Eqn 6 - Zero acceleration at second interior knot point
A[5][6] = 0.0
A[5][7] = 2.0
A[5][8] = 0.0
A[5][9] = 0.0
# Eqn 7 - Equal (but unknown) positions at first interior knot point
A[6][0] = dT0 ** 3
A[6][1] = dT0 ** 2
A[6][2] = dT0
A[6][3] = 1.0
A[6][5] = -1.0
# Eqn 8 - Equal (but unknown) positions at second interior knot point
A[7][9] = 1.0
A[7][4] = -dT1
A[7][5] = -1.0
# Eqn 9 - Known final position
A[8][6] = dT2 ** 3
A[8][7] = dT2 ** 2
A[8][8] = dT2
A[8][9] = 1.0
b[8][0] = ending_position
# Eqn 10 - Known final velocity = 0
A[9][6] = 3 * dT2 ** 2
A[9][7] = 2 * dT2
A[9][8] = 1.0
A[9][9] = 0.0
params = np.linalg.solve(A, b)
quintic_params = np.zeros((14, 1))
quintic_params[2:8] = params[0:6]
quintic_params[10:14] = params[6:10]
np.set_printoptions(precision=9, suppress=True, linewidth=250)
print "A=", A
print "b=", b
print "condition number(A)=", np.linalg.cond(A)
print "x=", params
# print "-----------------------------------------------------------------------"
# print "--------- Cubic solve -------------------------------------------------"
# print "cubic params=",params
# print "quintic params = ", quintic_params
# print "-----------------------------------------------------------------------"
return quintic_params
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
# TODO, is the above comment why the plugin gets stuck when we try to shutdown?
print "Shutdown ..."
rospy.sleep(0.1)
self.jointSubscriber.unregister()
self.commandPublisher.unregister()
rospy.sleep(0.5)
print "Done!"
# Define collection of widgets for joint group
def joint_widget(self, main_vbox, index):
joint = self.chain[index]
frame = QFrame()
frame.setFrameShape(QFrame.StyledPanel)
frame.setFrameShadow(QFrame.Raised)
hbox = QHBoxLayout()
# hbox.addWidget(frame)
self.prior_time = 0.0
robot_joint = self.robot.joints[self.joint_list[joint.name]]
joint.lower_limit = robot_joint.limit.lower
joint.upper_limit = robot_joint.limit.upper
ramp_range = robot_joint.limit.upper - robot_joint.limit.lower
print " ", joint.name, " limits(", joint.lower_limit, ", ", joint.upper_limit, ") range=", ramp_range
self.cmd_spinbox.append(QDoubleSpinBox())
self.cmd_spinbox[index].setDecimals(5)
self.cmd_spinbox[index].setRange(joint.lower_limit, joint.upper_limit)
self.cmd_spinbox[index].setSingleStep((robot_joint.limit.upper - robot_joint.limit.lower) / 50.0)
self.cmd_spinbox[index].valueChanged.connect(joint.on_cmd_value)
self.ramp_up_spinbox.append(QDoubleSpinBox())
self.ramp_up_spinbox[index].setDecimals(5)
self.ramp_up_spinbox[index].setRange(-ramp_range, ramp_range)
self.ramp_up_spinbox[index].setSingleStep(ramp_range / 50.0)
self.ramp_up_spinbox[index].valueChanged.connect(joint.on_ramp_up_value)
self.ramp_down_spinbox.append(QDoubleSpinBox())
self.ramp_down_spinbox[index].setDecimals(5)
self.ramp_down_spinbox[index].setRange(-ramp_range, ramp_range)
self.ramp_down_spinbox[index].setSingleStep(ramp_range / 50.0)
self.ramp_down_spinbox[index].valueChanged.connect(joint.on_ramp_down_value)
hbox.addWidget(QLabel(joint.name))
hbox.addWidget(self.cmd_spinbox[index])
hbox.addWidget(self.ramp_up_spinbox[index])
hbox.addWidget(self.ramp_down_spinbox[index])
# Add horizontal layout to frame for this joint group
frame.setLayout(hbox)
# Add frame to main vertical layout
main_vbox.addWidget(frame)
combo = QComboBox()
combo.setEditable(True)
combo_completer = TopicCompleter(combo)
# combo_completer.setCompletionMode(QCompleter.InlineCompletion)
combo.lineEdit().setCompleter(combo_completer)
model_tree = QTreeView()
model_tree.setModel(combo_completer.model())
model_tree.expandAll()
for column in range(combo_completer.model().columnCount()):
model_tree.resizeColumnToContents(column)
completion_tree = QTreeView()
completion_tree.setModel(combo_completer.completionModel())
completion_tree.expandAll()
for column in range(combo_completer.completionModel().columnCount()):
completion_tree.resizeColumnToContents(column)
layout.addWidget(model_tree)
layout.addWidget(completion_tree)
layout.addWidget(edit)
layout.addWidget(combo)
layout.setStretchFactor(model_tree, 1)
widget.setLayout(layout)
mw.setCentralWidget(widget)
mw.move(300, 0)
mw.resize(800, 900)
mw.show()
app.exec_()
class InteractiveClientUI(QMainWindow):
# pyqt signals are always defined as class attributes
signal_interactions_updated = Signal()
def __init__(self,
parent,
title,
application,
rocon_master_uri='localhost',
host_name='localhost',
with_rqt=False):
super(InteractiveClientUI, self).__init__(parent)
self.rocon_master_uri = rocon_master_uri
self.host_name = host_name
self.with_rqt = with_rqt
self.cur_selected_interaction = None
self.cur_selected_role = 0
self.interactions = {}
# desktop taskbar icon
self.application = application
if self.application:
rospack = rospkg.RosPack()
icon_file = os.path.join(rospack.get_path('rocon_icons'), 'icons',
'rocon_logo.png')
self.application.setWindowIcon(QIcon(icon_file))
# create a few directories for caching icons and ...
utils.setup_home_dirs()
# connect to the ros master with init node
self.interactive_client_interface = InteractiveClientInterface(
stop_interaction_postexec_fn=lambda: self.
signal_interactions_updated.emit())
if self.with_rqt:
(result, message) = self.interactive_client_interface._connect(
self.rocon_master_uri, self.host_name)
else:
(result, message
) = self.interactive_client_interface._connect_with_ros_init_node(
self.rocon_master_uri, self.host_name)
if not result:
QMessageBox.warning(self, 'Connection Failed',
"%s." % message.capitalize(), QMessageBox.Ok)
self._switch_to_master_chooser()
return
# interactive_client_ui widget setting
self._interactive_client_ui_widget = QWidget()
self._interactive_client_ui_layout = QVBoxLayout()
self._role_chooser = QRoleChooser(self.interactive_client_interface,
with_rqt)
self._role_chooser.bind_function('shutdown',
self._switch_to_master_chooser)
self._role_chooser.bind_function('back',
self._switch_to_master_chooser)
self._role_chooser.bind_function('select_role',
self._switch_to_interactions_list)
self._interactions_chooser = QInteractionsChooser(
self.interactive_client_interface)
self._interactions_chooser.bind_function(
'shutdown', self._switch_to_master_chooser)
self._interactions_chooser.bind_function('back',
self._switch_to_role_list)
self._interactive_client_ui_layout.addWidget(
self._interactions_chooser.interactions_widget)
self._interactive_client_ui_layout.addWidget(
self._role_chooser.roles_widget)
self._interactive_client_ui_widget.setLayout(
self._interactive_client_ui_layout)
self.signal_interactions_updated.connect(
self.interactions_updated_relay)
self._init()
def _init(self):
"""
Initialization of interactive client UI. First, show the role chooser and hide interactions chooser.
"""
if (len(self._role_chooser.role_list) != 1):
self._interactive_client_ui_widget.setWindowTitle('Role Chooser')
self._interactive_client_ui_widget.show()
self._role_chooser.show()
self._interactions_chooser.hide()
else:
console.logdebug(
"interface skipping the role chooser and switching directly to the interactions list [exactly one role available]"
)
self._switch_to_interactions_list()
def get_main_ui_handle(self):
"""
Returning an instance of interactive client ui widget. It is used to handle it in other widget.
:return: return main qt widget.
:rtype: python_qt_binding.QtGui.QWidget
"""
return self._interactive_client_ui_widget
def shutdown(self):
"""
Public method to enable shutdown of the script - this function is primarily for
shutting down the InteractiveClientUI from external signals (e.g. CTRL-C on the command
line).
"""
self.interactive_client_interface.shutdown()
def _switch_to_master_chooser(self):
"""
Switch to master chooser from role chooser. If it was launced by rqt or rqt standalone, It is just shutdown.
"""
self.shutdown()
if not self.with_rqt:
console.logdebug(
"InteractiveClientUI : switching back to the master chooser")
os.execv(QMasterChooser.rocon_remocon_script, ['', self.host_name])
def _switch_to_interactions_list(self):
"""
Take the selected role and switch to an interactions view of that role.
"""
console.logdebug(
"InteractiveClientUI : switching to the interactions list")
self._interactive_client_ui_widget.setWindowTitle(
'Interactions Chooser')
self._interactions_chooser.select_role(
self._role_chooser.cur_selected_role)
self._interactions_chooser.show(self._role_chooser.pos())
self._role_chooser.hide()
def _switch_to_role_list(self):
"""
Switch to role chooser from interactions chooser.
"""
self._interactive_client_ui_widget.setWindowTitle('Role Chooser')
console.logdebug("InteractiveClientUI : switching to the role list")
self._role_chooser.show(
self._interactions_chooser.interactions_widget.pos())
self._interactions_chooser.hide()
def interactions_updated_relay(self):
"""
Called by the underlying interactive client whenever the gui needs to be updated with
fresh information. Using this relay saves us from having to embed qt functions in the
underlying class but makes sure we signal across threads so the gui can update things
in its own thread.
Currently this only handles updates caused by termination of an interaction. If we wished to
handle additional situations, we should use an argument here indicating what kind of interaction
update occurred.
"""
console.logdebug("InteractiveClientUI : interactions_updated_relay")
self._interactions_chooser.refresh_interactions_list()
self._role_chooser.refresh_role_list()
