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()