class MyPlugin(Plugin): def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') rp = rospkg.RosPack() # 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 is a sibling of this file # in this example the .ui and .py file are in the same folder ui_file = os.path.join(rp.get_path('rqt_mypkg'), 'resource', '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('MyPluginUi') # 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._widget.btnTest.clicked.connect(self.test_button_clicked) def test_button_clicked(self): print('Hello') 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 create_main_widget(self): ## Main widget widget = QWidget() ui_file = os.path.join(rospkg.RosPack().get_path('frame_editor'), 'src/frame_editor', 'FrameEditorGUI.ui') loadUi(ui_file, widget) widget.setObjectName('FrameEditorGUIUi') #if context.serial_number() > 1: # widget.setWindowTitle(widget.windowTitle() + (' (%d)' % context.serial_number())) ## Undo View #widget.undo_frame.layout().addWidget(QtWidgets.QUndoView(self.editor.undo_stack)) ## Views self.editor.init_views() self.interface_style = FrameEditor_StyleWidget(self.editor) widget.style_frame.layout().addWidget(self.interface_style.get_widget()) ## Connections ## ## widget.btn_add.clicked.connect(self.btn_add_clicked) widget.btn_delete.clicked.connect(self.btn_delete_clicked) widget.btn_duplicate.clicked.connect(self.btn_duplicate_clicked) widget.list_frames.currentTextChanged.connect(self.selected_frame_changed) widget.btn_refresh.clicked.connect(self.update_tf_list) widget.btn_set_parent_rel.clicked.connect(self.btn_set_parent_rel_clicked) widget.btn_set_parent_abs.clicked.connect(self.btn_set_parent_abs_clicked) widget.btn_set_pose.clicked.connect(self.btn_set_pose_clicked) widget.btn_set_position.clicked.connect(self.btn_set_position_clicked) widget.btn_set_orientation.clicked.connect(self.btn_set_orientation_clicked) widget.btn_set_x.clicked.connect(self.btn_set_x_clicked) widget.btn_set_y.clicked.connect(self.btn_set_y_clicked) widget.btn_set_z.clicked.connect(self.btn_set_z_clicked) widget.btn_set_a.clicked.connect(self.btn_set_a_clicked) widget.btn_set_b.clicked.connect(self.btn_set_b_clicked) widget.btn_set_c.clicked.connect(self.btn_set_c_clicked) widget.btn_reset_position_rel.clicked.connect(self.btn_reset_position_rel_clicked) widget.btn_reset_position_abs.clicked.connect(self.btn_reset_position_abs_clicked) widget.btn_reset_orientation_rel.clicked.connect(self.btn_reset_orientation_rel_clicked) widget.btn_reset_orientation_abs.clicked.connect(self.btn_reset_orientation_abs_clicked) widget.txt_x.editingFinished.connect(self.x_valueChanged) widget.txt_y.editingFinished.connect(self.y_valueChanged) widget.txt_z.editingFinished.connect(self.z_valueChanged) widget.txt_a.editingFinished.connect(self.a_valueChanged) widget.txt_b.editingFinished.connect(self.b_valueChanged) widget.txt_c.editingFinished.connect(self.c_valueChanged) widget.btn_rad.toggled.connect(self.update_fields) widget.combo_style.currentIndexChanged.connect(self.frame_style_changed) return widget
class MyPlugin(Plugin): def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') rospy.loginfo("Remora: Basic Controller Online") # 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 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__)), 'remora_gui.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('MyPluginUi') # 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. self._widget.setWindowTitle('Basic Console') 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) # Register all subsribers here return def shutdown_plugin(self): 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 beast_settings_gui(Plugin): def __init__(self, context): super(beast_settings_gui, self).__init__(context) # Give QObjects reasonable names self.setObjectName('BEAST Settings') # Create QWidget self._widget = QWidget() ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'beast_settings_gui.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('BEAST Settings') self._widget.setWindowTitle('BEAST Settings') self.disturbance_type_combo = self._widget.findChild( QComboBox, 'disturbance_type_combo') self.disturbance_type_combo.addItems(['No Force', 'Sudden Force']) self.load_combo = self._widget.findChild(QComboBox, 'load_combo') self.load_combo.addItems(['0', '9000']) self.start_already_gripping_combo = self._widget.findChild( QComboBox, 'start_already_gripping_combo') self.start_already_gripping_combo.addItems(['False', 'True']) context.add_widget(self._widget) self.benchmark_params_service = rospy.Service( 'beast/gui/benchmark_params', BeastBenchmarkParams, self.benchmark_params_callback) 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 benchmark_params_callback(self, _): benchmark_params_response = BeastBenchmarkParamsResponse() benchmark_params_response.disturbance_type = str( self.disturbance_type_combo.currentText()) benchmark_params_response.load = float(self.load_combo.currentText()) benchmark_params_response.start_already_gripping = self.start_already_gripping_combo.currentText( ) == 'True' return benchmark_params_response
class PID(Plugin): def __init__(self, context): super(PID, self).__init__(context) # Give QObjects reasonable names self.setObjectName('CamConfig') # 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 'Running rqt_pid' # 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_pid'), 'resource', 'pid.ui') # Extend the widget with all attributes and children from UI file loadUi('/home/krishan/catkin_ws/src/rqt_pid/resource/pid.ui', self._widget, {'PIDView': 'PIDView'}) # Give QObjects reasonable names self._widget.setObjectName('PID_UI') # 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) 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 Visualizer(Plugin): def __init__(self, context): super(Visualizer, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Visualizer') # 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('consai2_gui'), 'resource', 'Visualizer.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget, {"PaintWidget": PaintWidget}) # Give QObjects reasonable names self._widget.setObjectName('VisualizerUi') # 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._redraw_timer = QTimer() self._redraw_timer.setInterval(16) self._redraw_timer.timeout.connect(self._widget.update) self._redraw_timer.start() def shutdown_plugin(self): pass def save_settings(self, plugin_settings, instance_settings): pass def restore_settings(self, plugin_settings, instance_settings): pass
class MissionModePlugin(Plugin): def __init__(self, context): super(MissionModePlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MissionModePlugin') # Create QWidget self._widget = QWidget() rp = rospkg.RosPack() # Get path to UI file which should be in the "resource" folder of this package ui_file = os.path.join(rp.get_path('behavior_selector'), 'resource', 'MissionModePlugin.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('MissionModePluginUi') # 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._widget.start_push_button.pressed.connect(self._on_start_pressed) self._widget.end_push_button.pressed.connect(self._on_end_pressed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) def _on_start_pressed(self): mode = START self._change_mode(mode) def _on_end_pressed(self): mode = END self._change_mode(mode) def _on_stop_pressed(self): mode = KILL self._change_mode(mode) def _change_mode(self, mode): rospy.wait_for_service('change_mode', 1) try: mm = rospy.ServiceProxy('change_mode', MissionModeChange) mm(mode) except rospy.ServiceException as e: print("Service call failed: %s" % e)
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 JoyVis(Plugin): def __init__(self, context): super(JoyVis, self).__init__(context) self.setObjectName('Joy') from argparse import ArgumentParser parser = ArgumentParser() 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._widget = QWidget() ui_file = os.path.join(rospkg.RosPack().get_path('rqt_joy'), 'resource', 'Joy.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('JoyUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.joy_1_x.setValue(0) self._widget.joy_1_y.setValue(0) rospy.Subscriber("joy", Joy, self.joyMessageCallback) def shutdown_plugin(self): pass def save_settings(self, plugin_settings, instance_settings): pass def restore_settings(self, plugin_settings, instance_settings): pass def joyMessageCallback(self, joy): self._widget.joy_1_x.setValue(joy.axes[0] * 100) self._widget.joy_1_y.setValue(joy.axes[1] * 100)
class SeeScopeControl(Plugin): """ Graphical frontend for a C{JointTrajectoryController}. There are two modes for interacting with a controller: 1. B{Monitor mode} Joint displays are updated with the state reported by the controller. This is a read-only mode and does I{not} send control commands. Every time a new controller is selected, it starts in monitor mode for safety reasons. 2. B{Control mode} Joint displays update the control command that is sent to the controller. Commands are sent periodically evan if the displays are not being updated by the user. To control the aggressiveness of the motions, the maximum speed of the sent commands can be scaled down using the C{Speed scaling control} This plugin is able to detect and keep track of all active controller managers, as well as the JointTrajectoryControllers that are I{running} in each controller manager. For a controller to be compatible with this plugin, it must comply with the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - The controller exposes the C{command} and C{state} topics in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. A reference implementation of the C{JointTrajectoryController} is available in the C{joint_trajectory_controller} ROS package. """ def __init__(self, context): super(SeeScopeControl, self).__init__(context) self.setObjectName('SeeScopeControl') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_see_scope_control'), 'resource', 'see_scope_control.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('SeeScopeControlUi') 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) cmd_topic = '/see_scope/projector/command' self._cmd_pub = rospy.Publisher(cmd_topic, String, queue_size=1) w = self._widget w.enable_button.toggled.connect(self._on_ssc_enabled) def _enable_fringes(self): self._cmd_pub.publish(String("fringes")) def _disable_fringes(self): self._cmd_pub.publish(String("white")) def _on_ssc_enabled(self, val): if val: self._enable_fringes(); else: self._disable_fringes();
class MavManagerUi(Plugin): def __init__(self, context): super(MavManagerUi, self).__init__(context) self.setObjectName('MavManagerUi') self._publisher = None self.robot_name = 'quadrotor' self.mav_node_name = 'mav_services' self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_mav_manager'), 'resource', 'MavManager.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('MavManagerWidget') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.robot_name_line_edit.textChanged.connect( self._on_robot_name_changed) self._widget.node_name_line_edit.textChanged.connect( self._on_node_name_changed) self._widget.motors_on_push_button.pressed.connect( self._on_motors_on_pressed) self._widget.motors_off_push_button.pressed.connect( self._on_motors_off_pressed) self._widget.hover_push_button.pressed.connect(self._on_hover_pressed) self._widget.ehover_push_button.pressed.connect( self._on_ehover_pressed) self._widget.land_push_button.pressed.connect(self._on_land_pressed) self._widget.eland_push_button.pressed.connect(self._on_eland_pressed) self._widget.estop_push_button.pressed.connect(self._on_estop_pressed) self._widget.goto_push_button.pressed.connect(self._on_goto_pressed) self._widget.takeoff_push_button.pressed.connect( self._on_takeoff_pressed) self._widget.gohome_push_button.pressed.connect( self._on_gohome_pressed) def _on_robot_name_changed(self, robot_name): self.robot_name = str(robot_name) def _on_node_name_changed(self, node_name): self.mav_node_name = str(node_name) def _on_motors_on_pressed(self): try: motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' rospy.wait_for_service(motors_topic, timeout=1.0) motors_on = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) resp = motors_on(True) print('Motors on ', resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_motors_off_pressed(self): try: motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' rospy.wait_for_service(motors_topic, timeout=1.0) motors_off = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) resp = motors_off(False) print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_hover_pressed(self): try: hover_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/hover' rospy.wait_for_service(hover_topic, timeout=1.0) hover = rospy.ServiceProxy(hover_topic, std_srvs.srv.Trigger) resp = hover() print('Hover ', resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_ehover_pressed(self): try: ehover_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/ehover' rospy.wait_for_service(ehover_topic, timeout=1.0) ehover = rospy.ServiceProxy(ehover_topic, std_srvs.srv.Trigger) resp = ehover() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_land_pressed(self): try: land_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/land' rospy.wait_for_service(land_topic, timeout=1.0) land = rospy.ServiceProxy(land_topic, std_srvs.srv.Trigger) resp = land() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_eland_pressed(self): try: eland_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/eland' rospy.wait_for_service(eland_topic, timeout=1.0) eland = rospy.ServiceProxy(eland_topic, std_srvs.srv.Trigger) resp = eland() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_estop_pressed(self): try: estop_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/estop' rospy.wait_for_service(estop_topic, timeout=1.0) estop = rospy.ServiceProxy(estop_topic, std_srvs.srv.Trigger) resp = estop() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_takeoff_pressed(self): try: takeoff_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/takeoff' rospy.wait_for_service(takeoff_topic, timeout=1.0) takeoff = rospy.ServiceProxy(takeoff_topic, std_srvs.srv.Trigger) resp = takeoff() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_gohome_pressed(self): try: gohome_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/goHome' rospy.wait_for_service(gohome_topic, timeout=1.0) gohome = rospy.ServiceProxy(gohome_topic, std_srvs.srv.Trigger) resp = gohome() print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) except (rospy.ROSException) as e: print("Service call failed: %s" % e) def _on_goto_pressed(self): req = kr_mav_manager.srv.Vec4Request() req.goal[0] = self._widget.x_doubleSpinBox.value() req.goal[1] = self._widget.y_doubleSpinBox.value() req.goal[2] = self._widget.z_doubleSpinBox.value() req.goal[3] = self._widget.yaw_doubleSpinBox.value() print(req.goal) if (self._widget.relative_checkbox.isChecked()): try: goto = rospy.ServiceProxy( '/' + self.robot_name + '/' + self.mav_node_name + '/goToRelative', kr_mav_manager.srv.Vec4) resp = goto(req) print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) else: try: goto_relative = rospy.ServiceProxy( '/' + self.robot_name + '/' + self.mav_node_name + '/goTo', kr_mav_manager.srv.Vec4) resp = goto_relative(req) print(resp.success) except rospy.ServiceException as e: print("Service call failed: %s" % e) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('robot_name', self._widget.robot_name_line_edit.text()) instance_settings.set_value('node_name', self._widget.node_name_line_edit.text()) def restore_settings(self, plugin_settings, instance_settings): #Override saved value with param value if set value = instance_settings.value('robot_name', "quadrotor") param_value = rospy.get_param("robot_name", value) self.robot_name = param_value self._widget.robot_name_line_edit.setText(param_value) value = instance_settings.value('node_name', "mav_services") self.node_name = value self._widget.node_name_line_edit.setText(value)
class ControllerManager(Plugin): """ Graphical frontend for managing ros_control controllers. """ _cm_update_freq = 1 # Hz def __init__(self, context): super(ControllerManager, self).__init__(context) self.setObjectName('ControllerManager') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_manager.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('ControllerManagerUi') # Pop-up that displays controller information self._popup_widget = QWidget() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_info.ui') loadUi(ui_file, self._popup_widget) self._popup_widget.setObjectName('ControllerInfoUi') # 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) # Initialize members self._cm_ns = [] # Namespace of the selected controller manager self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None # Controller manager service proxies self._load_srv = None self._unload_srv = None self._switch_srv = None # Controller state icons rospack = rospkg.RosPack() path = rospack.get_path('rqt_controller_manager') self._icons = {'running': QIcon(path + '/resource/led_green.png'), 'stopped': QIcon(path + '/resource/led_red.png'), 'uninitialized': QIcon(path + '/resource/led_off.png'), 'initialized': QIcon(path + '/resource/led_red.png')} # Controllers display table_view = self._widget.table_view table_view.setContextMenuPolicy(Qt.CustomContextMenu) table_view.customContextMenuRequested.connect(self._on_ctrl_menu) table_view.doubleClicked.connect(self._on_ctrl_info) header = table_view.horizontalHeader() header.setSectionResizeMode(QHeaderView.ResizeToContents) header.setContextMenuPolicy(Qt.CustomContextMenu) header.customContextMenuRequested.connect(self._on_header_menu) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_ctrl_list_timer = QTimer(self) self._update_ctrl_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_ctrl_list_timer.timeout.connect(self._update_controllers) self._update_ctrl_list_timer.start() # Signal connections w = self._widget w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) def shutdown_plugin(self): self._update_cm_list_timer.stop() self._update_ctrl_list_timer.stop() self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns # Setup services for communicating with the selected controller manager self._set_cm_services(cm_ns) # Controller lister for the selected controller manager if cm_ns: self._controller_lister = ControllerLister(cm_ns) self._update_controllers() else: self._controller_lister = None def _set_cm_services(self, cm_ns): if cm_ns: # NOTE: Persistent services are used for performance reasons. # If the controller manager dies, we detect it and disconnect from # it anyway load_srv_name = _append_ns(cm_ns, 'load_controller') self._load_srv = rospy.ServiceProxy(load_srv_name, LoadController, persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') self._unload_srv = rospy.ServiceProxy(unload_srv_name, UnloadController, persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') self._switch_srv = rospy.ServiceProxy(switch_srv_name, SwitchController, persistent=True) else: self._load_srv = None self._unload_srv = None self._switch_srv = None def _update_controllers(self): # Find controllers associated to the selected controller manager controllers = self._list_controllers() # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch def _list_controllers(self): """ @return List of controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with configurations loaded in the parameter server. @rtype [str] """ if not self._cm_ns: return [] # Add loaded controllers first controllers = self._controller_lister() # Append potential controller configs found in the parameter server all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) for name in get_rosparam_controller_names(all_ctrls_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(all_ctrls_ns, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='uninitialized') controllers.append(uninit_ctrl) return controllers def _show_controllers(self): table_view = self._widget.table_view self._table_model = ControllerTable(self._controllers, self._icons) table_view.setModel(self._table_model) def _on_ctrl_menu(self, pos): # Get data of selected controller row = self._widget.table_view.rowAt(pos.y()) if row < 0: return # Cursor is not under a valid item ctrl = self._controllers[row] # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'running': action_stop = menu.addAction(self._icons['stopped'], 'Stop') action_kill = menu.addAction(self._icons['uninitialized'], 'Stop and Unload') elif ctrl.state == 'stopped': action_start = menu.addAction(self._icons['running'], 'Start again') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'initialized': action_start = menu.addAction(self._icons['running'], 'Start') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'uninitialized': action_load = menu.addAction(self._icons['stopped'], 'Load') action_spawn = menu.addAction(self._icons['running'], 'Load and Start') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action if ctrl.state == 'running': if action is action_stop: self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) elif ctrl.state == 'stopped' or ctrl.state == 'initialized': if action is action_start: self._start_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) elif ctrl.state == 'uninitialized': if action is action_load: self._load_controller(ctrl.name) if action is action_spawn: self._load_controller(ctrl.name) self._start_controller(ctrl.name) def _on_ctrl_info(self, index): popup = self._popup_widget ctrl = self._controllers[index.row()] popup.ctrl_name.setText(ctrl.name) popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() model_root = QStandardItem('Claimed Resources') res_model.appendRow(model_root) for hw_res in ctrl.claimed_resources: hw_iface_item = QStandardItem(hw_res.hardware_interface) model_root.appendRow(hw_iface_item) for res in hw_res.resources: res_item = QStandardItem(res) hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) popup.resource_tree.expandAll() popup.move(QCursor.pos()) popup.show() def _on_header_menu(self, pos): header = self._widget.table_view.horizontalHeader() # Show context menu menu = QMenu(self._widget.table_view) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # Evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setSectionResizeMode(QHeaderView.Interactive) else: header.setSectionResizeMode(QHeaderView.ResizeToContents) def _load_controller(self, name): self._load_srv.call(LoadControllerRequest(name=name)) def _unload_controller(self, name): self._unload_srv.call(UnloadControllerRequest(name=name)) def _start_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[name], stop_controllers=[], strictness=strict) self._switch_srv.call(req) def _stop_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[], stop_controllers=[name], strictness=strict) self._switch_srv.call(req)
class LevelSelectorPlugin(Plugin): button_signal = pyqtSignal() button_status_signal = pyqtSignal() def __init__(self, context): super(LevelSelectorPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('LevelSelectorPlugin') # Create QWidget self._widget = QWidget() self._widget.setFont(QFont("Times", 15, QFont.Bold)) self._button_layout = QVBoxLayout(self._widget) self.buttons = [] self.text_label = QLabel("Waiting for MultiLevelMapData...", self._widget) self._button_layout.addWidget(self.text_label) self._widget.setObjectName('LevelSelectorPluginUI') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self.button_signal.connect(self.update_buttons) self.button_status_signal.connect(self.update_button_status) # Subscribe to the multi level map data to get information about all the maps. self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData, self.process_multimap) self.levels = [] self.current_level = None rospy.loginfo('level selector: subscribed to maps') # Subscribe to the current level we are on. self.status_subscriber = None # Create a service proxy to change the current level. self.level_selector_proxy = rospy.ServiceProxy("level_mux/change_current_level", ChangeCurrentLevel) self.level_selector_proxy.wait_for_service() rospy.loginfo('level selector: found "level_mux/change_current_level" service') def process_multimap(self, msg): """ Map metadata topic callback. """ rospy.loginfo('level selector: map metadata received.') self.levels = msg.levels # update level buttons in the selection window self.button_signal.emit() def update_buttons(self): """ Update buttons in Qt window. """ rospy.loginfo('level selector: update_buttons called') self.clean() for index, level in enumerate(self.levels): button = QPushButton(level.level_id, self._widget) button.clicked[bool].connect(self.handle_button) button.setCheckable(True) self._button_layout.addWidget(button) self.buttons.append(button) # Subscribe to the current level we are on. if self.status_subscriber is None: self.status_subscriber = rospy.Subscriber("level_mux/current_level", LevelMetaData, self.process_level_status) def update_button_status(self): """ Update button status Qt push button callback. """ rospy.loginfo('level selector: update_button_status') if not self.buttons or not self.current_level: rospy.logwarn('level selector: current level not available') return rospy.logdebug('buttons: ' + str(self.buttons)) for index, level in enumerate(self.levels): rospy.logdebug('level[' + str(index) + ']: ' + str(level.level_id)) if self.current_level == level.level_id: self.buttons[index].setChecked(True) else: self.buttons[index].setChecked(False) def process_level_status(self, msg): """ level_mux/current_level topic callback. """ rospy.loginfo('level selector: current_level topic message received') level_found = False for level in self.levels: if msg.level_id == level.level_id: self.current_level = level.level_id level_found = True break if not level_found: self.current_level = None self.button_status_signal.emit() def handle_button(self): source = self.sender() if source.text() == self.current_level: source.setChecked(True) return # Construct a identity pose. The level selector cannot be used # to choose the initialpose, as it does not have the interface # for specifying the position. The position should be # specified via rviz. origin_pose = PoseWithCovarianceStamped() origin_pose.header.frame_id = frameIdFromLevelId(source.text()) origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid. origin_pose.pose.covariance[0] = 1.0 origin_pose.pose.covariance[7] = 1.0 origin_pose.pose.covariance[14] = 1.0 origin_pose.pose.covariance[21] = 1.0 origin_pose.pose.covariance[28] = 1.0 origin_pose.pose.covariance[35] = 1.0 # Don't actually publish the initial pose via the level # selector. It doesn't know any better. self.level_selector_proxy(source.text(), False, origin_pose) def clean(self): while self._button_layout.count(): item = self._button_layout.takeAt(0) item.widget().deleteLater() def save_settings(self, plugin_settings, instance_settings): pass def restore_settings(self, plugin_settings, instance_settings): pass
class MyPlugin(Plugin): sig_sysmsg = Signal(str) l_location = pyqtSignal(float, float) r_location = pyqtSignal(float, float) def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') rp = rospkg.RosPack() # 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 ## def goCoords(): ## def resetError(): ## # coordsEdit.setStyleSheet('') ## ## try: ## print("work") ## #latitude, longitude = coordsEdit.text().split(",") ## ## except ValueError: ## #coordsEdit.setStyleSheet("color: red;") ## QTimer.singleShot(500, resetError) ## else: ## map.centerAt(latitude, longitude) ## # map.moveMarker("MyDragableMark", latitude, longitude) def onMarkerMoved(key, latitude, longitude): print("Moved!!", key, latitude, longitude) #coordsEdit.setText("{}, {}".format(latitude, longitude)) #self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01) def onMarkerRClick(key): print("RClick on ", key) # map.setMarkerOptions(key, draggable=False) def onMarkerLClick(key): print("LClick on ", key) def onMarkerDClick(key): print("DClick on ", key) # map.setMarkerOptions(key, draggable=True) def onMapMoved(latitude, longitude): print("Moved to ", latitude, longitude) def onMapRClick(latitude, longitude): print("RClick on ", latitude, longitude) def onMapLClick(latitude, longitude): print("LClick on ", latitude, longitude) def onMapDClick(latitude, longitude): print("DClick on ", latitude, longitude) def move_l_mark(lat_l, lon_l): #onMarkerMoved("local gps",lat_,lon_) #print("get location",lat_,lon_) self.map.moveMarker("local GPS", lat_l, lon_l) l_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_l, lon_l)) def local_callback(llocation): llat = llocation.data[0] llon = llocation.data[1] #move_mark(self,lat,lon) self.l_location.emit(llat, llon) def move_r_mark(lat_r, lon_r): #onMarkerMoved("local gps",lat_,lon_) #print("get location",lat_,lon_) self.map.moveMarker("remote GPS", lat_r, lon_r) r_coordsEdit.setText("{0:.8}, {1:.8}".format(lat_r, lon_r)) def remote_callback(rlocation): rlat = rlocation.data[0] rlon = rlocation.data[1] #move_mark(self,lat,lon) self.r_location.emit(rlat, rlon) # Create QWidget self._widget = QWidget() h = QVBoxLayout(self._widget) #h = QVBoxLayout(w) v = QHBoxLayout() # l = QFormLayout() # v.addLayout(l) l_coordsEdit = QLineEdit() r_coordsEdit = QLineEdit() lgps_label = QLabel('LGPS:') rgps_label = QLabel('RGPS:') v.addWidget(lgps_label) v.addWidget(l_coordsEdit) v.addWidget(rgps_label) v.addWidget(r_coordsEdit) h.addLayout(v) # l.addRow('Coords:', coordsEdit) # l.addRow('lcoords:',r_coordsEdit) #h.addLayout(v) #l = QFormLayout() #h.addLayout(l) #coordsEdit = QLineEdit() #l.addRow('Coords:', coordsEdit) #coordsEdit.editingFinished.connect(goCoords) self.map = QOSM(self._widget) self.map.mapMoved.connect(onMapMoved) #self.map.markerMoved.connect(onMarkerMoved) self.map.mapClicked.connect(onMapLClick) self.map.mapDoubleClicked.connect(onMapDClick) self.map.mapRightClicked.connect(onMapRClick) self.map.markerClicked.connect(onMarkerLClick) self.map.markerDoubleClicked.connect(onMarkerDClick) self.map.markerRightClicked.connect(onMarkerRClick) self.l_location.connect(move_l_mark) self.r_location.connect(move_r_mark) h.addWidget(self.map) self.map.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.MinimumExpanding) # 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(rp.get_path('osm_maps_node'), 'resource', '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('MyPluginUi') # 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.map.waitUntilReady() self.map.centerAt(32.7471012, -96.883642) self.map.setZoom(12) # Many icons at: https://sites.google.com/site/gmapsdevelopment/ coords = self.map.center() self.map.addMarker( "local GPS", *coords, **dict( icon= "http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_gray.png", draggable=True, title="locat GPS marker!")) coords = coords[0] + 0.1, coords[1] + 0.1 self.map.addMarker( "remote GPS", *coords, **dict( icon= "http://maps.gstatic.com/mapfiles/ridefinder-images/mm_20_red.png", draggable=True, title="remote GPS marker")) sub1 = rospy.Subscriber("/local_gps", Float64MultiArray, local_callback) sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray, remote_callback) #time.sleep(10) 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 RosPackGraph(Plugin): _deferred_fit_in_view = Signal() def __init__(self, context): super(RosPackGraph, self).__init__(context) self.initialized = False self._current_dotcode = None self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) self._nodes = {} self._edges = {} self._options = {} self._options_serialized = '' self.setObjectName('RosPackGraph') rospack = rospkg.RosPack() rosstack = rospkg.RosStack() # factory builds generic dotcode items self.dotcode_factory = PydotFactory() # self.dotcode_factory = PygraphvizFactory() # generator builds rosgraph self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack) # dot_to_qt transforms into Qt elements using dot layout self.dot_to_qt = DotToQtGenerator() self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui') loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) self._widget.setObjectName('RosPackGraphUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._scene = QGraphicsScene() self._scene.setBackgroundBrush(Qt.white) self._widget.graphics_view.setScene(self._scene) self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1) self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2) self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3) self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4) self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5) self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0) self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1) self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2) self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3) self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2) self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1) self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack) completer = RepeatedWordCompleter(completionmodel, self) completer.setCompletionMode(QCompleter.PopupCompletion) completer.setWrapAround(True) completer.setCaseSensitivity(Qt.CaseInsensitive) self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph) self._widget.filter_line_edit.setCompleter(completer) self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter) self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph) self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph) self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph) self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph) self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph) self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph) self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph) self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) self._widget.load_dot_push_button.pressed.connect(self._load_dot) self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_dot_push_button.pressed.connect(self._save_dot) self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) self._widget.save_as_image_push_button.pressed.connect(self._save_image) self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) self._deferred_fit_in_view.emit() context.add_widget(self._widget) # If in either of following case, this turnes True # - 1st filtering key is already input by user # - filtering key is restored self._filtering_started = False def shutdown_plugin(self): self._update_thread.kill() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex()) instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex()) instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex()) instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked()) instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked()) instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked()) instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked()) instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) def restore_settings(self, plugin_settings, instance_settings): _str_filter = instance_settings.value('filter_line_edit_text', '') if (_str_filter == None or _str_filter == '') and \ not self._filtering_started: _str_filter = '(Separate pkgs by comma)' else: self._filtering_started = True self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0))) self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0))) self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0))) self._widget.filter_line_edit.setText(_str_filter) self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true']) self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true']) self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true']) self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true']) self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) self.initialized = True self._refresh_rospackgraph() def _update_rospackgraph(self): # re-enable controls customizing fetched ROS graph self._widget.depth_combo_box.setEnabled(True) self._widget.directions_combo_box.setEnabled(True) self._widget.package_type_combo_box.setEnabled(True) self._widget.filter_line_edit.setEnabled(True) self._widget.with_stacks_check_box.setEnabled(True) self._widget.mark_check_box.setEnabled(True) self._widget.colorize_check_box.setEnabled(True) self._widget.hide_transitives_check_box.setEnabled(True) self._refresh_rospackgraph(force_update=True) def _update_options(self): self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex()) self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex()) self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex()) self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked() self._options['mark_selected'] = self._widget.mark_check_box.isChecked() self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked() # TODO: Allow different color themes self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None self._options['names'] = self._widget.filter_line_edit.text().split(',') if self._options['names'] == [u'None']: self._options['names'] = [] self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1 self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked() def _refresh_rospackgraph(self, force_update=False): if not self.initialized: return self._update_thread.kill() self._update_options() # avoid update if options did not change and force_update is not set new_options_serialized = pickle.dumps(self._options) if new_options_serialized == self._options_serialized and not force_update: return self._options_serialized = pickle.dumps(self._options) self._scene.setBackgroundBrush(Qt.lightGray) self._update_thread.start() # this runs in a non-gui thread, so don't access widgets here directly def _update_thread_run(self): self._update_graph(self._generate_dotcode()) @Slot() def _update_finished(self): self._scene.setBackgroundBrush(Qt.white) self._redraw_graph_scene() # this runs in a non-gui thread, so don't access widgets here directly def _generate_dotcode(self): includes = [] excludes = [] for name in self._options['names']: if name.strip().startswith('-'): excludes.append(name.strip()[1:]) else: includes.append(name.strip()) # orientation = 'LR' descendants = True ancestors = True if self._options['directions'] == 1: descendants = False if self._options['directions'] == 0: ancestors = False return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory, selected_names=includes, excludes=excludes, depth=self._options['depth'], with_stacks=self._options['with_stacks'], descendants=descendants, ancestors=ancestors, mark_selected=self._options['mark_selected'], colortheme=self._options['colortheme'], hide_transitives=self._options['hide_transitives'], hide_wet=self._options['package_types'] == 1, hide_dry=self._options['package_types'] == 2) # this runs in a non-gui thread, so don't access widgets here directly def _update_graph(self, dotcode): self._current_dotcode = dotcode self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level']) def _generate_tool_tip(self, url): if url is not None and ':' in url: item_type, item_path = url.split(':', 1) if item_type == 'node': tool_tip = 'Node:\n %s' % (item_path) service_names = rosservice.get_service_list(node=item_path) if service_names: tool_tip += '\nServices:' for service_name in service_names: try: service_type = rosservice.get_service_type(service_name) tool_tip += '\n %s [%s]' % (service_name, service_type) except rosservice.ROSServiceIOException as e: tool_tip += '\n %s' % (e) return tool_tip elif item_type == 'topic': topic_type, topic_name, _ = rostopic.get_topic_type(item_path) return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) return url def _redraw_graph_scene(self): # remove items in order to not garbage nodes which will be continued to be used for item in self._scene.items(): self._scene.removeItem(item) self._scene.clear() for node_item in self._nodes.values(): self._scene.addItem(node_item) for edge_items in self._edges.values(): for edge_item in edge_items: edge_item.add_to_scene(self._scene) self._scene.setSceneRect(self._scene.itemsBoundingRect()) if self._options['auto_fit']: self._fit_in_view() def _load_dot(self, file_name=None): if file_name is None: file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return try: fh = open(file_name, 'rb') dotcode = fh.read() fh.close() except IOError: return # disable controls customizing fetched ROS graph self._widget.depth_combo_box.setEnabled(False) self._widget.directions_combo_box.setEnabled(False) self._widget.package_type_combo_box.setEnabled(False) self._widget.filter_line_edit.setEnabled(False) self._widget.with_stacks_check_box.setEnabled(False) self._widget.mark_check_box.setEnabled(False) self._widget.colorize_check_box.setEnabled(False) self._widget.hide_transitives_check_box.setEnabled(False) self._update_graph(dotcode) self._redraw_graph_scene() @Slot() def _fit_in_view(self): self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) def _save_dot(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rospackgraph.dot', self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return handle = QFile(file_name) if not handle.open(QIODevice.WriteOnly | QIODevice.Text): return handle.write(self._current_dotcode) handle.close() def _save_svg(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rospackgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) if file_name is None or file_name == '': return generator = QSvgGenerator() generator.setFileName(file_name) generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) painter = QPainter(generator) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() def _save_image(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rospackgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) if file_name is None or file_name == '': return img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) painter = QPainter(img) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() img.save(file_name) def _clear_filter(self): if not self._filtering_started: self._widget.filter_line_edit.setText('') self._filtering_started = True
class Overview(Plugin): def __init__(self, context): super(Overview, self).__init__(context) self.__tfListener = tf.TransformListener() self.__copterFrame = "base_link" self.__worldFrame = "world" # Give QObjects reasonable names self.setObjectName('simple_position_control_gui') # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-c", "--copterFrame", dest="copterFrame", help="Specify the copter frame") parser.add_argument("-w", "--worldFrame", dest="worldFrame", help="Specify the world frame") args, unknowns = parser.parse_known_args(context.argv()) if args.copterFrame: self.__copterFrame = args.copterFrame rospy.loginfo("using %s as copter frame", self.__copterFrame) if args.worldFrame: self.__worldFrame = args.worldFrame rospy.loginfo("using %s as world frame", self.__worldFrame) # Create QWidget self._widget = QWidget() # Get path to UI file which should be in the "resource" folder of this node ui_file = os.path.join(rospkg.RosPack().get_path('position_controller'), 'src', 'rqt_control_gui', 'resource', 'overview.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('OverviewUI') # 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._widget.forwardButton.clicked.connect(self.goForward) self._widget.backwardButton.clicked.connect(self.goBackward) self._widget.leftButton.clicked.connect(self.goLeft) self._widget.rightButton.clicked.connect(self.goRight) self._widget.rotateButton.clicked.connect(self.rotate) self._widget.landButton.clicked.connect(self.land) self._widget.startButton.clicked.connect(self.start) self._widget.absoluteControlButton.clicked.connect(self.absoluteControl) self._widget.tfButton.clicked.connect(self.goToTF) self.__statusLabel = self._widget.statusLabel def setRelativePose(self, x_delta, y_delta, yaw_delta): """ set new target position relative to current position :param: x_delta: change in x :param: y_delta: change in y :param: yaw_delta: change in yaw """ try: (trans, rot) = self.__tfListener.lookupTransform(self.__worldFrame, self.__copterFrame, rospy.Time(0)) quad_delta = tf.transformations.quaternion_from_euler(0, 0, yaw_delta) translation_delta = tf.transformations.translation_matrix((x_delta, y_delta, 0)) m_current = tf.transformations.translation_matrix(trans).dot(tf.transformations.quaternion_matrix(rot)) m_delta = translation_delta.dot(tf.transformations.quaternion_matrix(quad_delta)) m_target = m_current.dot(m_delta) _, _, target_yaw = tf.transformations.euler_from_matrix(m_target) target_x, target_y, _ = tf.transformations.translation_from_matrix(m_target) self.__statusLabel.setText("going to x: {0} y: {1} yaw: {2}".format(target_x, target_y, target_yaw)) rospy.wait_for_service('/position_controller/set_target_pos', 1) setPose = rospy.ServiceProxy('/position_controller/set_target_pos', SetPos) setPose(target_x, target_y, target_yaw) except Exception as e: self.__statusLabel.setText(str(e)) def goForward(self): distance = float(self._widget.distanceEdit.text()) self.setRelativePose(distance, 0, 0) def goBackward(self): distance = float(self._widget.distanceEdit.text()) self.setRelativePose(-distance, 0, 0) def goLeft(self): distance = float(self._widget.distanceEdit.text()) self.setRelativePose(0, distance, 0) def goRight(self): distance = float(self._widget.distanceEdit.text()) self.setRelativePose(0, -distance, 0) def rotate(self): angle = math.pi * float(self._widget.rotationEdit.text()) / 180.0 self.setRelativePose(0, 0, angle) def land(self): try: self.setAltitude(0.0) except ValueError: pass def start(self): try: altitude = float(self._widget.zEdit.text()) self.setAltitude(altitude) self.setAltitude(altitude) statusMessage = "starting to altitude {0}".format(altitude) self.__statusLabel.setText(statusMessage) except ValueError: pass def setAltitude(self, altitude): rospy.wait_for_service('/position_controller/set_altitude', 1) setAltitudeService = rospy.ServiceProxy('/position_controller/set_altitude', SetAltitude) setAltitudeService(altitude) def absoluteControl(self): statusMessage = "" try: ((x, y, _z), quaternion) = self.__tfListener.lookupTransform(self.__worldFrame, self.__copterFrame, rospy.Time(0)) (_roll, _pitch, yaw) = euler_from_quaternion(quaternion) try: yaw = math.pi * float(self._widget.angleEdit.text()) / 180.0 except ValueError: pass try: x = float(self._widget.xEdit.text()) except ValueError: pass try: y = float(self._widget.yEdit.text()) except ValueError: pass statusMessage += "going to x: {0} y: {1} yaw: {2} ".format(x, y, yaw) rospy.wait_for_service('/position_controller/set_target_pos', 1) setPose = rospy.ServiceProxy('/position_controller/set_target_pos', SetPos) setPose(x, y, yaw) self.__statusLabel.setText(statusMessage) try: altitude = float(self._widget.zEdit.text()) statusMessage += "setting altitude to {0}".format(altitude) self.setAltitude(altitude) self.__statusLabel.setText(statusMessage) except ValueError: pass except Exception as e: self.__statusLabel.setText(str(e)) def goToTF(self): pass def shutdown_plugin(self): pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: rospy.loginfo("saving simple position controller gui setting") instance_settings.set_value("worldFrame", self.__worldFrame) instance_settings.set_value("copterFrame", self.__copterFrame) def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: rospy.loginfo("restoring simple position controller gui setting") storedWorldFrame = instance_settings.value("worldFrame") if type(storedWorldFrame) == unicode: storedWorldFrame = storedWorldFrame.encode('ascii', 'ignore') if storedWorldFrame: self.__worldFrame = storedWorldFrame storedCopterFrame = instance_settings.value("copterFrame") if type(storedCopterFrame) == unicode: storedCopterFrame = storedCopterFrame.encode('ascii', 'ignore') if storedCopterFrame: self.__copterFrame = storedCopterFrame
class RobotSteering(Plugin): slider_factor = 1000.0 def __init__(self, context): super(RobotSteering, self).__init__(context) self.setObjectName('RobotSteering') self._publisher = None self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.topic_line_edit.textChanged.connect( self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect( self._on_x_linear_slider_changed) self._widget.z_angular_slider.valueChanged.connect( self._on_z_angular_slider_changed) self._widget.increase_x_linear_push_button.pressed.connect( self._on_strong_increase_x_linear_pressed) self._widget.reset_x_linear_push_button.pressed.connect( self._on_reset_x_linear_pressed) self._widget.decrease_x_linear_push_button.pressed.connect( self._on_strong_decrease_x_linear_pressed) self._widget.increase_z_angular_push_button.pressed.connect( self._on_strong_increase_z_angular_pressed) self._widget.reset_z_angular_push_button.pressed.connect( self._on_reset_z_angular_pressed) self._widget.decrease_z_angular_push_button.pressed.connect( self._on_strong_decrease_z_angular_pressed) self._widget.max_x_linear_double_spin_box.valueChanged.connect( self._on_max_x_linear_changed) self._widget.min_x_linear_double_spin_box.valueChanged.connect( self._on_min_x_linear_changed) self._widget.max_z_angular_double_spin_box.valueChanged.connect( self._on_max_z_angular_changed) self._widget.min_z_angular_double_spin_box.valueChanged.connect( self._on_min_z_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect( self._on_strong_increase_x_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect( self._on_reset_x_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect( self._on_strong_decrease_x_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect( self._on_strong_increase_z_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect( self._on_reset_z_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect( self._on_strong_decrease_z_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip( self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_x_linear_push_button.setToolTip( self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_x_linear_push_button.setToolTip( self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_x_linear_push_button.setToolTip( self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_z_angular_push_button.setToolTip( self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_z_angular_push_button.setToolTip( self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_z_angular_push_button.setToolTip( self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect( self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() if topic == '': return try: self._publisher = rospy.Publisher(topic, Twist, queue_size=10) except TypeError: self._publisher = rospy.Publisher(topic, Twist) def _on_stop_pressed(self): self._widget.x_linear_slider.setValue(0) self._widget.z_angular_slider.setValue(0) def _on_x_linear_slider_changed(self): self._widget.current_x_linear_label.setText( '%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_z_angular_slider_changed(self): self._widget.current_z_angular_label.setText( '%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep()) def _on_reset_x_linear_pressed(self): self._widget.x_linear_slider.setValue(0) def _on_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep()) def _on_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep()) def _on_reset_z_angular_pressed(self): self._widget.z_angular_slider.setValue(0) def _on_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep()) def _on_max_x_linear_changed(self, value): self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_x_linear_changed(self, value): self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor) def _on_max_z_angular_changed(self, value): self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_z_angular_changed(self, value): self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor) def _on_strong_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep()) def _on_strong_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue( self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep()) def _on_strong_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep()) def _on_strong_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue( self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep()) def _on_parameter_changed(self): self._send_twist( self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): if self._publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = z_angular # Only send the zero command once so other devices can take control if x_linear == 0 and z_angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) else: self.zero_cmd_sent = False self._publisher.publish(twist) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic', self._widget.topic_line_edit.text()) instance_settings.set_value( 'vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value( 'vx_min', self._widget.min_x_linear_double_spin_box.value()) instance_settings.set_value( 'vw_max', self._widget.max_z_angular_double_spin_box.value()) instance_settings.set_value( 'vw_min', self._widget.min_z_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', "/cmd_vel") value = rospy.get_param("~default_topic", value) self._widget.topic_line_edit.setText(value) value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value('vx_max', value) value = rospy.get_param("~default_vx_max", value) self._widget.max_x_linear_double_spin_box.setValue(float(value)) value = self._widget.min_x_linear_double_spin_box.value() value = instance_settings.value('vx_min', value) value = rospy.get_param("~default_vx_min", value) self._widget.min_x_linear_double_spin_box.setValue(float(value)) value = self._widget.max_z_angular_double_spin_box.value() value = instance_settings.value('vw_max', value) value = rospy.get_param("~default_vw_max", value) self._widget.max_z_angular_double_spin_box.setValue(float(value)) value = self._widget.min_z_angular_double_spin_box.value() value = instance_settings.value('vw_min', value) value = rospy.get_param("~default_vw_min", value) self._widget.min_z_angular_double_spin_box.setValue(float(value))
class MyPlugin(Plugin): def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') rospy.loginfo("Remora: ECU Console") # 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 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__)), 'ecu_console.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('MyPluginUi') # 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. self._widget.setWindowTitle('ECU') 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.firstStatusMsg = False # Register all subsribers here self.intTempSub_ = rospy.Subscriber('/sunfish/ecu/int_temp', Temperature, self.callback_IntTemp) self.extTempSub_ = rospy.Subscriber('/sunfish/ecu/ext_temp', Temperature, self.callback_ExtTemp) self.depthSub_ = rospy.Subscriber('/sunfish/ecu/depth', Depth, self.callback_Depth) self.insSub_ = rospy.Subscriber('/sunfish/ecu/INS', INS, self.callback_INS) self.magFieldSub_ = rospy.Subscriber('/sunfish/ecu/MST', MagField, self.callback_MagField) self.statusSub_ = rospy.Subscriber('/sunfish/ecu/status', Status, self.callback_Status) return def shutdown_plugin(self): self.intTempSub_.unregister() self.extTempSub_.unregister() self.depthSub_.unregister() self.insSub_.unregister() self.magFieldSub_.unregister() self.statusSub_.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 #def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget title bar # Usually used to open a modal configuration dialog def formatFloat(self, f): return ("%2.2f" % f) def callback_IntTemp(self, value): self._widget.intTempData.setText(self.formatFloat(value.temperature)) return def callback_ExtTemp(self, value): self._widget.extTempData.setText(self.formatFloat(value.temperature)) return def callback_Depth(self, value): self._widget.pressureData.setText(self.formatFloat(value.Pressure)) self._widget.tempData.setText(self.formatFloat(value.Temperature)) return def callback_INS(self, value): self._widget.ab_data_0.setText(self.formatFloat(value.AbsOrientation[0])) self._widget.ab_data_1.setText(self.formatFloat(value.AbsOrientation[1])) self._widget.ab_data_2.setText(self.formatFloat(value.AbsOrientation[2])) self._widget.av_Data_0.setText(self.formatFloat(value.AngularVelocity[0])) self._widget.av_Data_1.setText(self.formatFloat(value.AngularVelocity[1])) self._widget.av_Data_2.setText(self.formatFloat(value.AngularVelocity[2])) self._widget.af_data_0.setText(self.formatFloat(value.AccelerationVector[0])) self._widget.af_data_1.setText(self.formatFloat(value.AccelerationVector[1])) self._widget.af_data_2.setText(self.formatFloat(value.AccelerationVector[2])) self._widget.la_Data_0.setText(self.formatFloat(value.LinearAcceleration[0])) self._widget.la_Data_1.setText(self.formatFloat(value.LinearAcceleration[1])) self._widget.la_Data_2.setText(self.formatFloat(value.LinearAcceleration[2])) self._widget.gv_data_0.setText(self.formatFloat(value.GravityVector[0])) self._widget.gv_data_1.setText(self.formatFloat(value.GravityVector[1])) self._widget.gv_data_2.setText(self.formatFloat(value.GravityVector[2])) return def callback_MagField(self, value): self._widget.mf_data_0.setText(self.formatFloat(value.MagneticField[0])) self._widget.mf_data_1.setText(self.formatFloat(value.MagneticField[1])) self._widget.mf_data_2.setText(self.formatFloat(value.MagneticField[2])) return def callback_Status(self, value): if self.firstStatusMsg == False: self._widget.hwdData.setText("Hwd: " + value.Hardware) self._widget.firmwareData.setText("Firmware:" + value.Firmware) self.firstStatusMsg = True self._widget.voltData.setText(("%2.1f" % value.Voltage)) self._widget.currentData.setText(("%2.2f" % value.Current)) self._widget.pwm_lcd_0.display(10) self._widget.pwm_lcd_1.display(10) self._widget.pwm_lcd_2.display(10) self._widget.pwm_lcd_3.display(10) self._widget.pwm_lcd_4.display(50) self._widget.pwm_lcd_5.display(50) self._widget.pwm_lcd_6.display(50) self._widget.pwm_lcd_7.display(50) self._widget.pwm_lcd_8.display(100) self._widget.pwm_lcd_9.display(100) self._widget.pwm_lcd_10.display(100) self._widget.pwm_lcd_11.display(100) return
class KillCounterPlugin(Plugin): def __init__(self, context): super(KillCounterPlugin, self).__init__(context) #return # Give QObjects reasonable names self.setObjectName('KillCounterPlugin') rp = rospkg.RosPack() # 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() self._container = QWidget() self._layout = QVBoxLayout() self._container.setLayout(self._layout) self._num_killed = 0 self._kill_label = QLabel('Martians Killed: 0') #Killing Martians is Bad Tho, We are a dying species and killing us is mean and will destroy an awesome society with culture and life and happiness. I'm so disappointed in you Kimberly self._layout.addWidget(self._kill_label) MIN = 540 MAX = 660 self._chance = randint(MIN, MAX) context.add_widget(self._container) # 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(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelection.ui') #ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelectionSimple.ui') # Extend the widget with all attributes and children from UI file #loadUi(ui_file, self._widget, {}) # Give QObjects reasonable names self._widget.setObjectName('Kill Counter') # 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())) self.running = True rate = rospy.Rate(1) def run(): while self.running: num = randint(0, self._chance) if num == self._chance: self._num_killed += 1 self._kill_label.setText('Martians Killed: ' + str(self._num_killed)) rate.sleep() self.run_thread = threading.Thread(target=run) self.run_thread.start() def shutdown_plugin(self): # TODO unregister all publishers here` self.running = False
class DriverStationSim(Plugin): def __init__(self, context): super(DriverStationSim, self).__init__(context) # Give QObjects reasonable names self.setObjectName('DriverStationSim') # 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_driver_station_sim'), 'resource', 'driverStationSim.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('DriverStationSim') # 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. auto_pub = rospy.Publisher("/frcrobot/autonomous_mode", AutoMode, queue_size=3) match_pub = rospy.Publisher("/frcrobot/match_data", MatchSpecificData, queue_size=3) def pub_data(self): r = rospy.Rate(100) auto_msg = AutoMode() auto_msg.mode = [0, 0, 0, 0] auto_msg.delays = [0, 0, 0, 0] match_msg = MatchSpecificData() modes = [0, 0, 0, 0] match_msg = MatchSpecificData() start_time = rospy.get_time() enable_last = False auto_last = False practice_last = False auto_duration = 0 while (not rospy.is_shutdown()): #Robot State Values enable = self._widget.enable_button_2.isChecked() disable = self._widget.disable_button_2.isChecked() auto = self._widget.auto_mode.isChecked() practice = self._widget.practice_button.isChecked() #Time Start and Restart Handling if (not enable_last and enable): rospy.logwarn("enableLast") start_time = rospy.get_time() if (not auto_last and auto and not practice): rospy.logwarn("autoLast") start_time = rospy.get_time() if (not practice_last and practice): rospy.logwarn("practiceLast") start_time = rospy.get_time() auto_duration = 15 #TODO read from DS if (enable and practice): if (rospy.get_time() < start_time + auto_duration): auto = True enable = True disable = False elif (rospy.get_time() >= start_time + auto_duration and rospy.get_time < start_time + 150): auto = False enable = True disable = False elif (rospy.get_time() >= start_time + 150): auto = False enable = False disable = True if (enable): time_diff = int(rospy.get_time() - start_time) minutes = ((150 - time_diff) / 60) seconds = ((149 - time_diff) % 60 + (1.0 - (rospy.get_time() - start_time) % 1)) time = str(minutes) + ":" + str(seconds) if (not self._widget.timer_pause.isChecked()): self._widget.time.setText(time[:7]) match_msg.matchTimeRemaining = 150 - time_diff match_msg.Disabled = False match_msg.Enabled = True else: match_msg.matchTimeRemaining = 0 match_msg.Disabled = True match_msg.Enabled = False #Publish Data # match_msg.allianceData = self._widget.match_data.text() match_msg.allianceColor = 1 match_msg.driverStationLocation = 1 match_msg.matchNumber = 1 match_msg.Autonomous = auto enable_last = match_msg.Enabled auto_last = auto practice_last = practice auto_msg.header.stamp = rospy.Time.now() auto_msg.position = self._widget.start_pos.value() auto_msg.mode[0] = self._widget.mode_0.value() auto_msg.mode[1] = self._widget.mode_1.value() auto_msg.mode[2] = self._widget.mode_2.value() auto_msg.mode[3] = self._widget.mode_3.value() auto_msg.delays[0] = self._widget.delay_0.value() auto_msg.delays[1] = self._widget.delay_1.value() auto_msg.delays[2] = self._widget.delay_2.value() auto_msg.delays[3] = self._widget.delay_3.value() match_pub.publish(match_msg) auto_pub.publish(auto_msg) r.sleep() load_thread = threading.Thread(target=pub_data, args=(self, )) load_thread.start() 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) 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 DynTuneUI(Plugin): loggerUpdate = Signal(str, name='loggerUpdate') def __init__(self, context): super(DynTuneUI, self).__init__(context) # Give QObjects reasonable names self.setObjectName('dyn_tune_plugin') # 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() self._bag_widget = MyBagWidget(context) # 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_dyn_tune'), 'resource', 'DynTune.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('DynTuneUI') self._widget.vLayout.insertWidget(1, self._bag_widget, 0) self._widget.vLayout.setStretchFactor(self._widget.hLayout, 1) self._widget.vLayout.setStretchFactor(self._widget._widget_func, 1) # 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) index = self._widget._widget_values._column_names.index("checkbox") self._widget._widget_values.topics_tree_widget.hideColumn(index) self._widget._widget_values.topics_tree_widget.model().setHeaderData( 0, Qt.Horizontal, "value") self._widget._widget_func.start() self._widget._widget_values.start() self._widget._widget_param.start() rospy.Subscriber("/rosout", Log, self.logger_update) self.loggerUpdate.connect(self.logger_update_slot) css_file = os.path.join(rospkg.RosPack().get_path('rqt_dyn_tune'), 'src', 'rqt_dyn_tune', 'css', 'selector.css') with open(css_file, "r") as fh: csstext = fh.read() self._widget._widget_func.setStyleSheet(csstext) self._widget._widget_values.setStyleSheet(csstext) self._widget._widget_param.setStyleSheet(csstext) css_file = os.path.join(rospkg.RosPack().get_path('rqt_dyn_tune'), 'src', 'rqt_dyn_tune', 'css', 'bag.css') with open(css_file, "r") as fh: csstext = fh.read() self._bag_widget.setStyleSheet(csstext) css_file = os.path.join(rospkg.RosPack().get_path('rqt_dyn_tune'), 'src', 'rqt_dyn_tune', 'css', 'plugin.css') with open(css_file, "r") as fh: csstext = fh.read() self._widget.setStyleSheet(csstext) self._widget.tune_btn.clicked.connect(self.tune_clicked) @Slot() def topics_refreshed(): topic_widget = self._widget._widget_values values = topic_widget.get_selected_values() self._widget._widget_func.topics_refreshed(values) self._widget._widget_values.topicsRefreshed.connect(topics_refreshed) def logger_update(self, msg): if msg.name == "/dyn_tune_backbone": self.loggerUpdate.emit(msg.msg) @Slot(str) def logger_update_slot(self, msg): self._widget.logger.append(msg) def tune_clicked(self): srv = self.creat_optimize_srv() print srv self.optimize = rospy.ServiceProxy('/optimize', Optimize) result = self.optimize(srv) print result pass def create_experiment_msg(self, name="experiment_0"): exp = Experiment() params_dict = self._widget._widget_param.get_selected() exp.parameters = json.dumps(params_dict) exp.objective = self._widget._widget_func.create_function_msg() return exp def creat_optimize_srv(self): opt = OptimizeRequest() opt.observation_values = self._widget._widget_values.get_selected() opt.start_signals = "{}" opt.end_signals = "{}" opt.src_bag = self._bag_widget.filename opt.dst_bag = "_simulation.bag" opt.src_topic = "/.*" opt.dst_topic = "" exp = self.create_experiment_msg() opt.experiments.append(exp) return opt # opt.src. 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 RosGraph(Plugin): _deferred_fit_in_view = Signal() def __init__(self, context): super(RosGraph, self).__init__(context) self.initialized = False self.setObjectName('RosGraph') self._graph = None self._current_dotcode = None self._widget = QWidget() # factory builds generic dotcode items self.dotcode_factory = PydotFactory() # self.dotcode_factory = PygraphvizFactory() # generator builds rosgraph self.dotcode_generator = RosGraphDotcodeGenerator() # dot_to_qt transforms into Qt elements using dot layout self.dot_to_qt = DotToQtGenerator() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui') loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) self._widget.setObjectName('RosGraphUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._scene = QGraphicsScene() self._scene.setBackgroundBrush(Qt.white) self._widget.graphics_view.setScene(self._scene) self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH) self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH) self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH) self._widget.graph_type_combo_box.setCurrentIndex(0) self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph) self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False) completer = RepeatedWordCompleter(self.node_completionmodel, self) completer.setCompletionMode(QCompleter.PopupCompletion) completer.setWrapAround(True) completer.setCaseSensitivity(Qt.CaseInsensitive) self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph) self._widget.filter_line_edit.setCompleter(completer) self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False) topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self) topic_completer.setCompletionMode(QCompleter.PopupCompletion) topic_completer.setWrapAround(True) topic_completer.setCaseSensitivity(Qt.CaseInsensitive) self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph) self._widget.topic_filter_line_edit.setCompleter(topic_completer) self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph) self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph) self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph) self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph) self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph) self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph) self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view) self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view) self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) self._widget.load_dot_push_button.pressed.connect(self._load_dot) self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_dot_push_button.pressed.connect(self._save_dot) self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) self._widget.save_as_image_push_button.pressed.connect(self._save_image) self._update_rosgraph() self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) self._deferred_fit_in_view.emit() context.add_widget(self._widget) def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex()) instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text()) instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked()) instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked()) instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked()) instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked()) instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked()) instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) def restore_settings(self, plugin_settings, instance_settings): self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0))) self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/')) self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/')) self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true']) self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true']) self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true']) self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true']) self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true']) self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) self.initialized = True self._refresh_rosgraph() def _update_rosgraph(self): # re-enable controls customizing fetched ROS graph self._widget.graph_type_combo_box.setEnabled(True) self._widget.filter_line_edit.setEnabled(True) self._widget.topic_filter_line_edit.setEnabled(True) self._widget.namespace_cluster_check_box.setEnabled(True) self._widget.actionlib_check_box.setEnabled(True) self._widget.dead_sinks_check_box.setEnabled(True) self._widget.leaf_topics_check_box.setEnabled(True) self._widget.quiet_check_box.setEnabled(True) self._graph = rosgraph.impl.graph.Graph() self._graph.set_master_stale(5.0) self._graph.set_node_stale(5.0) self._graph.update() self.node_completionmodel.refresh(self._graph.nn_nodes) self.topic_completionmodel.refresh(self._graph.nt_nodes) self._refresh_rosgraph() def _refresh_rosgraph(self): if not self.initialized: return self._update_graph_view(self._generate_dotcode()) def _generate_dotcode(self): ns_filter = self._widget.filter_line_edit.text() topic_filter = self._widget.topic_filter_line_edit.text() graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex()) orientation = 'LR' if self._widget.namespace_cluster_check_box.isChecked(): namespace_cluster = 1 else: namespace_cluster = 0 accumulate_actions = self._widget.actionlib_check_box.isChecked() hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked() hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked() quiet = self._widget.quiet_check_box.isChecked() return self.dotcode_generator.generate_dotcode( rosgraphinst=self._graph, ns_filter=ns_filter, topic_filter=topic_filter, graph_mode=graph_mode, hide_single_connection_topics=hide_single_connection_topics, hide_dead_end_topics=hide_dead_end_topics, cluster_namespaces_level=namespace_cluster, accumulate_actions=accumulate_actions, dotcode_factory=self.dotcode_factory, orientation=orientation, quiet=quiet) def _update_graph_view(self, dotcode): if dotcode == self._current_dotcode: return self._current_dotcode = dotcode self._redraw_graph_view() def _generate_tool_tip(self, url): if url is not None and ':' in url: item_type, item_path = url.split(':', 1) if item_type == 'node': tool_tip = 'Node:\n %s' % (item_path) service_names = rosservice.get_service_list(node=item_path) if service_names: tool_tip += '\nServices:' for service_name in service_names: try: service_type = rosservice.get_service_type(service_name) tool_tip += '\n %s [%s]' % (service_name, service_type) except rosservice.ROSServiceIOException as e: tool_tip += '\n %s' % (e) return tool_tip elif item_type == 'topic': topic_type, topic_name, _ = rostopic.get_topic_type(item_path) return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) return url def _redraw_graph_view(self): self._scene.clear() if self._widget.highlight_connections_check_box.isChecked(): highlight_level = 3 else: highlight_level = 1 # layout graph and create qt items (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, highlight_level=highlight_level, same_label_siblings=True) for node_item in nodes.itervalues(): self._scene.addItem(node_item) for edge_items in edges.itervalues(): for edge_item in edge_items: edge_item.add_to_scene(self._scene) self._scene.setSceneRect(self._scene.itemsBoundingRect()) if self._widget.auto_fit_graph_check_box.isChecked(): self._fit_in_view() def _load_dot(self, file_name=None): if file_name is None: file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return try: fh = open(file_name, 'rb') dotcode = fh.read() fh.close() except IOError: return # disable controls customizing fetched ROS graph self._widget.graph_type_combo_box.setEnabled(False) self._widget.filter_line_edit.setEnabled(False) self._widget.topic_filter_line_edit.setEnabled(False) self._widget.namespace_cluster_check_box.setEnabled(False) self._widget.actionlib_check_box.setEnabled(False) self._widget.dead_sinks_check_box.setEnabled(False) self._widget.leaf_topics_check_box.setEnabled(False) self._widget.quiet_check_box.setEnabled(False) self._update_graph_view(dotcode) def _fit_in_view(self): self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) def _save_dot(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return handle = QFile(file_name) if not handle.open(QIODevice.WriteOnly | QIODevice.Text): return handle.write(self._current_dotcode) handle.close() def _save_svg(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) if file_name is None or file_name == '': return generator = QSvgGenerator() generator.setFileName(file_name) generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) painter = QPainter(generator) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() def _save_image(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) if file_name is None or file_name == '': return img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) painter = QPainter(img) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() img.save(file_name)
class RqtLgsvlSimulatorConfiguratorPlugin(Plugin): def __init__(self, context): super(RqtLgsvlSimulatorConfiguratorPlugin, self).__init__(context) rospack = rospkg.RosPack() self.settings = QSettings(rospack.get_path('lgsvl_simulator_bridge')+"/config/setting.dat",QSettings.IniFormat) # Give QObjects reasonable names self.setObjectName('RqtLgsvlSimulatorConfiguratorPlugin') # 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('lgsvl_simulator_bridge'), 'resource', 'LgsvlSimulatorConfigratorPlugin.ui') # Extend the widget with all atrributes and children from UI File loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('RqtLgsvlSimulatorConfiguratorPluginUi') # Show _widget.windowTitle on left-top of each plugin(when it's set in _widget). # This is useful when you open multiple plugins aat 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 self.settings.beginGroup("simulator_params") context.add_widget(self._widget) ip = self.settings.value('ip') if ip != None: self._widget.ip_box.setText(ip) port = self.settings.value('port') if port != None: self._widget.port_box.setText(port) config_path = self.settings.value('config_path') if config_path != None: self._widget.configpath_box.setText(config_path) self.settings.endGroup() self.json_dict = {} self.instance_id = "" self.is_remote = False self._widget.button_config_ref.clicked[bool].connect(self._handle_config_ref_button_clicked) self._widget.button_config.clicked[bool].connect(self._handle_config_button_clicked) self._widget.launch_button.clicked[bool].connect(self._handle_launch_button_clicked) self._widget.terminate_button.clicked[bool].connect(self._handle_terminate_button_clicked) 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.get_value(k, v) pass def restore_settings(self, pluign_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass def _handle_launch_button_clicked(self): if str(self._widget.ip_box.text()) == "localhost" or str(self._widget.ip_box.text()) == "0.0.0.0" or str(self._widget.ip_box.text()) == "127.0.0.1": cmd = ["roslaunch","lgsvl_simulator_bridge","lgsvl_simulator.launch"] self.proc = subprocess.Popen(cmd, stdout=subprocess.PIPE) self._widget.launch_button.setStyleSheet("background-color: #8fb8e0") self._widget.terminate_button.setStyleSheet("background-color: #FFFFFF") self.is_remote = False return else: cmd = ["roslaunch","lgsvl_simulator_bridge","bridge.launch"] self.proc = subprocess.Popen(cmd, stdout=subprocess.PIPE) address = "http://" + self._widget.ip_box.text() + ":" + self._widget.port_box.text()+"/simulator/launch" self.settings.beginGroup("simulator_params") self.settings.setValue("ip", self._widget.ip_box.text()) self.settings.setValue("port", self._widget.port_box.text()) self.settings.endGroup() try: response = requests.post(address,json=self.json_dict) self.instance_id = response.json()[u'instance_id'] except: self._widget.launch_button.setStyleSheet("background-color: #F5A9A9") return self.is_remote = False self._widget.launch_button.setStyleSheet("background-color: #8fb8e0") self._widget.terminate_button.setStyleSheet("background-color: #FFFFFF") def _handle_config_ref_button_clicked(self): rospack = rospkg.RosPack() fname = QFileDialog.getOpenFileName(self._widget, 'Open file', rospack.get_path('lgsvl_simulator_bridge')+'/config') self._widget.configpath_box.setText(fname[0]) def _handle_config_button_clicked(self): path = self._widget.configpath_box.text() try: f = open(path, "r+") self.json_dict = json.load(f) self.settings.beginGroup("simulator_params") self.settings.setValue("config_path", path) self.settings.endGroup() except: self._widget.button_config.setStyleSheet("background-color: #F5A9A9") return self._widget.button_config.setStyleSheet("background-color: #8fb8e0") def _handle_terminate_button_clicked(self): address = "http://" + self._widget.ip_box.text() + ":" + self._widget.port_box.text()+"/simulator/terminate" self.settings.beginGroup("simulator_params") self.settings.setValue("ip", self._widget.ip_box.text()) self.settings.setValue("port", self._widget.port_box.text()) self.settings.endGroup() if self.instance_id == "" and self.is_remote == True: return if self.is_remote == True: try: response = requests.post(address,json={u'instance_id':self.instance_id}) except: self._widget.terminate_button.setStyleSheet("background-color: #F5A9A9") return try: self.proc.terminate() except: self._widget.terminate_button.setStyleSheet("background-color: #F5A9A9") return self._widget.terminate_button.setStyleSheet("background-color: #8fb8e0") self._widget.launch_button.setStyleSheet("background-color: #FFFFFF")
class GroundRobotTeleop(Plugin): def __init__(self, context): super(GroundRobotTeleop, self).__init__(context) # Give QObjects reasonable names self.setObjectName('GroundRobotTeleop') # 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_ground_robot_teleop'), 'resource', 'GroundRobotTeleop.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('GroundRobotTeleopUi') # 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 logo pixmap = QPixmap(os.path.join(rospkg.RosPack().get_path( 'rqt_ground_robot_teleop'), 'resource', 'jderobot.png')) self._widget.img_logo.setPixmap(pixmap.scaled(121, 121)) # Set Variables self.linear_velocity_scaling_factor = 1 self.angular_velocity_scaling_factor = 0.5 # Set functions for each GUI Item self._widget.stop_button.clicked.connect(self.stop_robot) # Add Publishers self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) #Add global variables self.twist_msg = Twist() self.stop_icon = QIcon() self.stop_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path( 'rqt_ground_robot_teleop'), 'resource', 'stop.png')), QIcon.Normal, QIcon.Off) self.play_icon = QIcon() self.play_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path( 'rqt_ground_robot_teleop'), 'resource', 'play.png')), QIcon.Normal, QIcon.Off) self.bridge = CvBridge() self.teleop = TeleopWidget(self, 'set_twist', 311) self._widget.teleop_layout.addWidget(self.teleop) self.teleop.setVisible(True) # Add widget to the user interface context.add_widget(self._widget) # Add Subscibers rospy.Subscriber('camera/rgb/image_raw', Image, self.cam_cb) def msg_to_pixmap(self, msg): cv_img = self.bridge.imgmsg_to_cv2(msg) h, w, _ = cv_img.shape bytesPerLine = 3 * w q_img = QImage(cv_img.data, w, h, bytesPerLine, QImage.Format_RGB888) return QPixmap.fromImage(q_img).scaled(320, 240) def cam_cb(self, msg): self._widget.img_camera.setPixmap(self.msg_to_pixmap(msg)) def stop_robot(self): rospy.loginfo('Stopping Robot') self.teleop.stop() for i in range(5): self.twist_msg = Twist() self.twist_pub.publish(self.twist_msg) rospy.sleep(0.05) def set_twist(self, u, v): az = -self.linear_velocity_scaling_factor * u x = -self.angular_velocity_scaling_factor * v self._widget.rot_value.setText('%.2f' % az) self._widget.x_value.setText('%.2f' % x) self.twist_msg.linear.x = x self.twist_msg.angular.z = az self.twist_pub.publish(self.twist_msg) 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 VacuumGripper(Plugin): def __init__(self, context): super(VacuumGripper, self).__init__(context) # Give QObjects reasonable names self.setObjectName('VacuumGripper') # 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) rospy.loginfo("Opening GUI") # 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_industrial_robot'), 'resources', 'VacuumGripper.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('VacuumGripper') # 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) rospy.Subscriber("/joint_states", JointState, self.jointstate_callback) self.jointstate_pub = rospy.Publisher("/joint_states", JointState, queue_size=0) self.robot = RobotWrapper() filename = os.path.join(rospkg.RosPack().get_path('rqt_industrial_robot'), 'src','rqt_vacuum_gripper', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) jointslimit = joints_setup["joints_limit"] self._widget.xEdit.editingFinished.connect(self.set_x) self._widget.yEdit.editingFinished.connect(self.set_y) self._widget.zEdit.editingFinished.connect(self.set_z) self._widget.rollEdit.editingFinished.connect(self.set_roll) self._widget.pitchEdit.editingFinished.connect(self.set_pitch) self._widget.yawEdit.editingFinished.connect(self.set_yaw) self._widget.planButton.clicked.connect(self.plan) self._widget.executeButton.clicked.connect(self.execute) self._widget.planexeButton.clicked.connect(self.planexe) self._widget.stopexeButton.clicked.connect(self.stopexe) self._widget.homeButton.clicked.connect(self.backtohome) self._widget.jointSlider_1.sliderReleased.connect(self.setjoint1) self._widget.jointSlider_1.valueChanged.connect(self.viewjoint1) self._widget.jointSlider_1.setMinimum(jointslimit["joint_1"]["low"]) self._widget.jointSlider_1.setMaximum(jointslimit["joint_1"]["high"]) self._widget.joint1Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(1)),2))+DEG) self._widget.joint1_low.setText(str(jointslimit["joint_1"]["low"])+DEG) self._widget.joint1_high.setText(str(jointslimit["joint_1"]["high"])+DEG) self._widget.jointSlider_2.sliderReleased.connect(self.setjoint2) self._widget.jointSlider_2.valueChanged.connect(self.viewjoint2) self._widget.jointSlider_2.setMinimum(jointslimit["joint_2"]["low"]) self._widget.jointSlider_2.setMaximum(jointslimit["joint_2"]["high"]) self._widget.joint2Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(2)),2))+DEG) self._widget.joint2_low.setText(str(jointslimit["joint_2"]["low"])+DEG) self._widget.joint2_high.setText(str(jointslimit["joint_2"]["high"])+DEG) self._widget.jointSlider_3.sliderReleased.connect(self.setjoint3) self._widget.jointSlider_3.valueChanged.connect(self.viewjoint3) self._widget.jointSlider_3.setMinimum(jointslimit["joint_3"]["low"]) self._widget.jointSlider_3.setMaximum(jointslimit["joint_3"]["high"]) self._widget.joint3Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(3)),2))+DEG) self._widget.joint3_low.setText(str(jointslimit["joint_3"]["low"])+DEG) self._widget.joint3_high.setText(str(jointslimit["joint_3"]["high"])+DEG) self._widget.jointSlider_4.sliderReleased.connect(self.setjoint4) self._widget.jointSlider_4.valueChanged.connect(self.viewjoint4) self._widget.jointSlider_4.setMinimum(jointslimit["joint_4"]["low"]) self._widget.jointSlider_4.setMaximum(jointslimit["joint_4"]["high"]) self._widget.joint4Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(4)),2))+DEG) self._widget.joint4_low.setText(str(jointslimit["joint_4"]["low"])+DEG) self._widget.joint4_high.setText(str(jointslimit["joint_4"]["high"])+DEG) self._widget.jointSlider_5.sliderReleased.connect(self.setjoint5) self._widget.jointSlider_5.valueChanged.connect(self.viewjoint5) self._widget.jointSlider_5.setMinimum(jointslimit["joint_5"]["low"]) self._widget.jointSlider_5.setMaximum(jointslimit["joint_5"]["high"]) self._widget.joint5Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(5)),2))+DEG) self._widget.joint5_low.setText(str(jointslimit["joint_5"]["low"])+DEG) self._widget.joint5_high.setText(str(jointslimit["joint_5"]["high"])+DEG) self._widget.jointSlider_6.sliderReleased.connect(self.setjoint6) self._widget.jointSlider_6.valueChanged.connect(self.viewjoint6) self._widget.jointSlider_6.setMinimum(jointslimit["joint_6"]["low"]) self._widget.jointSlider_6.setMaximum(jointslimit["joint_6"]["high"]) self._widget.joint6Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(6)),2))+DEG) self._widget.joint6_low.setText(str(jointslimit["joint_6"]["low"])+DEG) self._widget.joint6_high.setText(str(jointslimit["joint_6"]["high"])+DEG) self._widget.GraspButton.clicked.connect(self.gripper_grasp) self._widget.ReleaseButton.clicked.connect(self.gripper_release) self._widget.rvizButton.clicked.connect(self.launchrviz) self._widget.start_button.clicked.connect(self.playClicked) self._widget.stop_button.clicked.connect(self.stopClicked) self._widget.pause_button.clicked.connect(self.pauseClicked) self._widget.restart_button.clicked.connect(self.restartClicked) self._widget.updatefkButton.clicked.connect(self.updatefk) self._widget.updateikButton.clicked.connect(self.updateik) self.updatefk() self.updateik() self._widget.respawnButton.clicked.connect(self.respawn_all_objects) rospy.Subscriber("/updatepose", Bool, self.updatepose) rospy.Subscriber("/gui_message", String, self.browser_callback) self.message_pub = rospy.Publisher("/gui_message", String, queue_size=0) self.updatepose_pub = rospy.Publisher("/updatepose", Bool, queue_size=0) self.startalgorithm_pub = rospy.Publisher("/start_algorithm", Bool, queue_size=0) self.stopalgorithm_pub = rospy.Publisher("/stop_algorithm", Bool, queue_size=0) self.pausealgorithm_pub = rospy.Publisher("/pause_algorithm", Bool, queue_size=0) self.startalgorithm_sub = rospy.Subscriber("/start_algorithm", Bool, self.startalgorithm_callback) self.stopalgorithm_sub = rospy.Subscriber("/stop_algorithm", Bool, self.stopalgorithm_callback) self.algorithm_is_on = False def startalgorithm_callback(self, msg): self.updatepose_trigger(True) if msg.data == True: self.algorithm_is_on = True last_time = rospy.Time.now().to_sec() while self.algorithm_is_on: if rospy.Time.now().to_sec()-last_time > 0.1: self.updatepose_trigger(True) last_time = rospy.Time.now().to_sec() def stopalgorithm_callback(self, msg): self.algorithm_is_on = False def browser_callback(self, msg): self._widget.browser.append(msg.data) def send_message(self, msg): message = String() message.data = msg self.message_pub.publish(message) def updatepose_trigger(self, value): msg = Bool() msg.data = value self.updatepose_pub.publish(msg) def updatepose(self, msg): if msg.data == True: self.updateik() def jointstate_callback(self, msg): if len(msg.name)==6: msg.name.extend(["gripper_joint", "gripper_joint1", "gripper_joint2", "gripper_joint3", "gripper_joint4", "gripper_joint5", "gripper_joint6", "gripper_joint7", "gripper_joint8"]) position = list(msg.position) position.extend([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) msg.position = position self.jointstate_pub.publish(msg) def respawn_all_objects(self): self.robot.modelmanager.respawn_all_objects() def updatefk(self): self._widget.joint1Browser.clear() self._widget.joint1Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(1)),2))+DEG) self._widget.jointSlider_1.setValue(numpy.rad2deg(self.robot.get_joint_value(1))) self._widget.joint2Browser.clear() self._widget.joint2Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(2)),2))+DEG) self._widget.jointSlider_2.setValue(numpy.rad2deg(self.robot.get_joint_value(2))) self._widget.joint3Browser.clear() self._widget.joint3Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(3)),2))+DEG) self._widget.jointSlider_3.setValue(numpy.rad2deg(self.robot.get_joint_value(3))) self._widget.joint4Browser.clear() self._widget.joint4Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(4)),2))+DEG) self._widget.jointSlider_4.setValue(numpy.rad2deg(self.robot.get_joint_value(4))) self._widget.joint5Browser.clear() self._widget.joint5Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(5)),2))+DEG) self._widget.jointSlider_5.setValue(numpy.rad2deg(self.robot.get_joint_value(5))) self._widget.joint6Browser.clear() self._widget.joint6Browser.append(str(round(numpy.rad2deg(self.robot.get_joint_value(6)),2))+DEG) self._widget.jointSlider_6.setValue(numpy.rad2deg(self.robot.get_joint_value(6))) def updateik(self): roll, pitch, yaw, x, y, z = self.robot.get_arm_pose() self._widget.xEdit.setText(str(round(x,4))) self._widget.yEdit.setText(str(round(y,4))) self._widget.zEdit.setText(str(round(z,4))) self._widget.rollEdit.setText(str(round(numpy.rad2deg(roll),2))) self._widget.pitchEdit.setText(str(round(numpy.rad2deg(pitch),2))) self._widget.yawEdit.setText(str(round(numpy.rad2deg(yaw),2))) def launchrviz(self): os.system("gnome-terminal -x sh -c \"roslaunch rqt_industrial_robot rviz.launch\"") def algorithm_trigger(self, pub, value): msg = Bool() msg.data = value pub.publish(msg) def playClicked(self): self.send_message("Start Algorithm") self.algorithm_trigger(self.startalgorithm_pub, True) def stopClicked(self): self.send_message("Stopping Algorithm") self.algorithm_trigger(self.stopalgorithm_pub, True) self.algorithm_trigger(self.startalgorithm_pub, False) def pauseClicked(self): self.send_message("Pausing Algorithm") self.algorithm_trigger(self.pausealgorithm_pub, True) def restartClicked(self): self.send_message("Retart Algorithm") self.algorithm_trigger(self.pausealgorithm_pub, False) def update(self): while True: self.updatefk() self.updateik() print("updating") time.sleep(1) def plan(self): self.set_x() self.set_y() self.set_z() self.set_roll() self.set_pitch() self.set_yaw() self.robot.plan() def execute(self): last_joints = self.robot.get_joints_value() self.robot.execute() # print("start moving") # update after robot stops moving while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10): last_joints = self.robot.get_joints_value() #print(last_joints) time.sleep(0.5) self.updatefk() self.updateik() time.sleep(1.5) # print("double check movement") # update after robot stops moving while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10): last_joints = self.robot.get_joints_value() #print(last_joints) time.sleep(0.5) self.updatefk() self.updateik() def planexe(self): last_joints = self.robot.get_joints_value() self.set_x() self.set_y() self.set_z() self.set_roll() self.set_pitch() self.set_yaw() self.robot.plan() self.robot.execute() # print("start moving") # update after robot stops moving while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10): last_joints = self.robot.get_joints_value() #print(last_joints) time.sleep(0.5) self.updatefk() self.updateik() time.sleep(1.5) # print("double check movement") # update after robot stops moving while abs(sum(last_joints)-sum(self.robot.get_joints_value()) > 1e-10): last_joints = self.robot.get_joints_value() #print(last_joints) time.sleep(0.5) self.updatefk() self.updateik() def stopexe(self): self.robot.stop_execution() self.updatefk() self.updateik() def backtohome(self): self.robot.back_to_home() self.updatefk() self.updateik() def set_x(self): self.robot.set_x(float(self._widget.xEdit.text())) def set_y(self): self.robot.set_y(float(self._widget.yEdit.text())) def set_z(self): self.robot.set_z(float(self._widget.zEdit.text())) def set_roll(self): self.robot.set_roll(numpy.deg2rad(float(self._widget.rollEdit.text()))) def set_pitch(self): self.robot.set_pitch(numpy.deg2rad(float(self._widget.pitchEdit.text()))) def set_yaw(self): self.robot.set_yaw(numpy.deg2rad(float(self._widget.yawEdit.text()))) def viewjoint1(self): self._widget.joint1Browser.clear() self._widget.joint1Browser.append(str(self._widget.jointSlider_1.value())+DEG) def viewjoint2(self): self._widget.joint2Browser.clear() self._widget.joint2Browser.append(str(self._widget.jointSlider_2.value())+DEG) def viewjoint3(self): self._widget.joint3Browser.clear() self._widget.joint3Browser.append(str(self._widget.jointSlider_3.value())+DEG) def viewjoint4(self): self._widget.joint4Browser.clear() self._widget.joint4Browser.append(str(self._widget.jointSlider_4.value())+DEG) def viewjoint5(self): self._widget.joint5Browser.clear() self._widget.joint5Browser.append(str(self._widget.jointSlider_5.value())+DEG) def viewjoint6(self): self._widget.joint6Browser.clear() self._widget.joint6Browser.append(str(self._widget.jointSlider_6.value())+DEG) def setjoint1(self): angle = numpy.deg2rad(self._widget.jointSlider_1.value()) self.robot.set_arm_joint(1, angle) self.updateik() def setjoint2(self): angle = numpy.deg2rad(self._widget.jointSlider_2.value()) self.robot.set_arm_joint(2, angle) self.updateik() def setjoint3(self): angle = numpy.deg2rad(self._widget.jointSlider_3.value()) self.robot.set_arm_joint(3, angle) self.updateik() def setjoint4(self): angle = numpy.deg2rad(self._widget.jointSlider_4.value()) self.robot.set_arm_joint(4, angle) self.updateik() def setjoint5(self): angle = numpy.deg2rad(self._widget.jointSlider_5.value()) self.robot.set_arm_joint(5, angle) self.updateik() def setjoint6(self): angle = numpy.deg2rad(self._widget.jointSlider_6.value()) self.robot.set_arm_joint(6, angle) self.updateik() def setRobotWrapper(self, robot): self.robot = robot def gripper_grasp(self): self.robot.gripper_grasp() def gripper_release(self): self.robot.gripper_release()
class ExperimentGUI(Plugin): def __init__(self, context): super(ExperimentGUI, self).__init__(context) # Give QObjects reasonable names self.plans = [ 'Starting Position', 'Above Panel', 'Panel Grabbed', 'Above Placement Nest', 'Panel Placed' ] #state_dict ties each state to planlistindex values #self.state_dict={'reset_position':0,'pickup_prepare':1,'pickup_lower':2,'pickup_grab_first_step':2,'pickup_grab_second_step':2,'pickup_raise':2,'transport_panel':3,'place_lower':4,'place_set_first_step':4,'place_set_second_step':4,'place_raise':4} self.gui_execute_states = [ "reset", "panel_pickup", "pickup_grab", "transport_panel", "place_panel" ] self.execute_states = [ ['plan_to_reset_position', 'move_to_reset_position'], ['plan_pickup_prepare', 'move_pickup_prepare'], [ 'plan_pickup_lower', 'move_pickup_lower', 'plan_pickup_grab_first_step', 'move_pickup_grab_first_step', 'plan_pickup_grab_second_step', 'move_pickup_grab_second_step', 'plan_pickup_raise', 'move_pickup_raise' ], ['plan_transport_payload', 'move_transport_payload'], ['plan_place_set_second_step'] ] self.teleop_modes = [ 'Error', 'Off', 'Joint', 'Cartesian', 'Cylindrical', 'Spherical' ] self.current_teleop_mode = 1 self.teleop_button_string = "Tele-op\nMode:\n" self.setObjectName('MyPlugin') self._lock = threading.Lock() #self._send_event=threading.Event() self.controller_commander = controller_commander_pkg.ControllerCommander( ) # 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.in_process = None self.recover_from_pause = False #rospy.get_param("rosbag_name") #<param name="start_time" command="date +'%d-%m-%Y_%Ih%Mm%S'"/> #rosbag record args="record -O arg('start_time') self._mainwidget = QWidget() self.layout = QGridLayout() self._mainwidget.setLayout(self.layout) self.disconnectreturnoption = False self.stackedWidget = QStackedWidget() #self._mainwidget) self.layout.addWidget(self.stackedWidget, 0, 0) self._welcomescreen = QWidget() self._runscreen = QWidget() self._errordiagnosticscreen = QWidget() self.stackedWidget.addWidget(self._welcomescreen) self.stackedWidget.addWidget(self._runscreen) self.stackedWidget.addWidget(self._errordiagnosticscreen) #self._data_array=collections.deque(maxlen=500) self._proxy_model = message_proxy_model.MessageProxyModel() self._rospack = rospkg.RosPack() #self.console=console_widget.ConsoleWidget(self._proxy_model,self._rospack) self.robotconnectionled = LEDIndicator() self.robotconnectionled.setDisabled(True) # Make the led non clickable self.forcetorqueled = LEDIndicator() self.forcetorqueled.setDisabled(True) # Make the led non clickable self.overheadcameraled = LEDIndicator() self.overheadcameraled.setDisabled(True) # Make the led non clickable self.grippercameraled = LEDIndicator() self.grippercameraled.setDisabled(True) # Make the led non clickable self.runscreenstatusled = LEDIndicator() self.runscreenstatusled.setDisabled(True) self.step_executor = GUI_Step_Executor() self.step_executor.error_signal.connect(self._feedback_receive) #self.step_executor.error_function=self._feedback_receive #Need this to pause motions self.process_client = actionlib.ActionClient('process_step', ProcessStepAction) self.process_client.wait_for_server() self.placement_targets = { 'leeward_mid_panel': 'panel_nest_leeward_mid_panel_target', 'leeward_tip_panel': 'panel_nest_leeward_tip_panel_target' } self.placement_target = 'panel_nest_leeward_mid_panel_target' self.panel_type = 'leeward_mid_panel' self.client_handle = None service_list = rosservice.get_service_list() if ('/overhead_camera/trigger' in service_list): self.led_change(self.overheadcameraled, True) else: self.led_change(self.overheadcameraled, False) if ('/gripper_camera_2/trigger' in service_list): self.led_change(self.grippercameraled, True) else: self.led_change(self.grippercameraled, False) self.mode = 0 #self.rewound=False self.count = 0 self.data_count = 0 self.force_torque_data = np.zeros((6, 1)) self.joint_angle_data = np.zeros((6, 1)) # Get path to UI file which should be in the "resource" folder of this package self.welcomescreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'welcomeconnectionscreen.ui') self.runscreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'Runscreen.ui') self.errorscreenui = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'resource', 'errordiagnosticscreen.ui') self.rewound = False # Extend the widget with all attributes and children from UI file loadUi(self.welcomescreenui, self._welcomescreen) loadUi(self.runscreenui, self._runscreen) loadUi(self.errorscreenui, self._errordiagnosticscreen) # Give QObjects reasonable names self._mainwidget.setObjectName('MyPluginUi') # 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._mainwidget.setWindowTitle(self._mainwidget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._mainwidget) self.context = context self.plugin_settings = None self.instance_settings = None #self._errordiagnosticscreen.consoleWidget=console_widget.ConsoleWidget(self._proxy_model,self._rospack) #####consoleiagnosticscreen.backToRun.pressed.connect(self._to_run_screen) #self._runscThread=ConsoleThread(self._widget.State_info) # self._welcomescreen.statusFormLayout.takeAt(0) self._welcomescreen.statusFormLayout.addWidget(self.robotconnectionled, 0, 0) self._welcomescreen.statusFormLayout.addWidget(self.forcetorqueled, 2, 0) self._welcomescreen.statusFormLayout.addWidget(self.overheadcameraled, 4, 0) self._welcomescreen.statusFormLayout.addWidget(self.grippercameraled, 6, 0) self._runscreen.connectionLayout.addWidget(self.runscreenstatusled, 0, 1) #self._welcomescreen.robotConnectionWidget.addWidget(self.led) #consoleThread.finished.connect(app.exit) #####consoleThread.start() self.rviz_starter = os.path.join( rospkg.RosPack().get_path('rpi_arm_composites_manufacturing_gui'), 'src', 'rpi_arm_composites_manufacturing_gui', 'rviz_starter.py') # Add widget to the user interface #context.add_widget(console)==QDialog.Accepted #context.add_widget(rqt_console) for entry in self.plans: listentry = QListWidgetItem(entry) listentry.setFlags(Qt.ItemIsSelectable) self._runscreen.planList.addItem(listentry) self._runscreen.planList.item(0).setSelected(True) self.planListIndex = 0 self._runscreen.panelType.setText(self.panel_type) self._runscreen.placementNestTarget.setText("Leeward Mid Panel Nest") self._runscreen.panelType.setReadOnly(True) self._runscreen.placementNestTarget.setReadOnly(True) self.commands_sent = False rospy.Subscriber("controller_state", controllerstate, self.callback) self._set_controller_mode = rospy.ServiceProxy("set_controller_mode", SetControllerMode) rospy.Subscriber("GUI_state", ProcessState, self.process_state_set) #rospy.Subscriber('gui_error', String, self._feedback_receive()) self.force_torque_plot_widget = QWidget() self.joint_angle_plot_widget = QWidget() self._welcomescreen.openConfig.clicked.connect( self._open_config_options) #self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_login_prompt) self._welcomescreen.toRunScreen.pressed.connect(self._to_run_screen) self._runscreen.backToWelcome.pressed.connect(self._to_welcome_screen) #self._runscreen.toErrorScreen.pressed.connect(self._to_error_screen) self._runscreen.nextPlan.pressed.connect(self._next_plan) self._runscreen.previousPlan.pressed.connect(self._previousPlan) self._runscreen.resetToHome.pressed.connect(self._reset_position) self._runscreen.stopPlan.pressed.connect(self._stopPlan) self._runscreen.accessTeleop.pressed.connect(self.change_teleop_modes) #self._errordiagnosticscreen.openOverheadCameraView.pressed.connect(self._open_overhead_camera_view) #self._errordiagnosticscreen.openGripperCameraViews.pressed.connect(self._open_gripper_camera_views) self._errordiagnosticscreen.openForceTorqueDataPlot.pressed.connect( self._open_force_torque_data_plot) self._errordiagnosticscreen.openJointAngleDataPlot.pressed.connect( self._open_joint_angle_data_plot) self._errordiagnosticscreen.backToRun.pressed.connect( self._to_run_screen) #self._runscreen.widget.frame=rviz.VisualizationFrame() #self._runscreen.widget.frame.setSplashPath( "" ) ## VisualizationFrame.initialize() must be called before ## VisualizationFrame.load(). In fact it must be called ## before most interactions with RViz classes because it ## instantiates and initializes the VisualizationManager, ## which is the central class of RViz. #self._runscreen.widget.frame.initialize() #self.manager = self._runscreen.widget.frame.getManager() # self._welcomescreen.openAdvancedOptions.pressed.connect(self._open_advanced_options) def led_change(self, led, state): led.setChecked(state) def _to_welcome_screen(self): self.stackedWidget.setCurrentIndex(0) def _to_run_screen(self): self.controller_commander.set_controller_mode( self.controller_commander.MODE_HALT, 1, [], []) if (self.stackedWidget.currentIndex() == 0): self.messagewindow = PanelSelectorWindow() self.messagewindow.show() self.messagewindow.setFixedSize(self.messagewindow.size()) if self.messagewindow.exec_(): next_selected_panel = self.messagewindow.get_panel_selected() if (next_selected_panel != None): self.panel_type = next_selected_panel self.placement_target = self.placement_targets[ self.panel_type] self.stackedWidget.setCurrentIndex(1) self._runscreen.panelType.setText(self.panel_type) if (self.panel_type == 'leeward_mid_panel'): self._runscreen.placementNestTarget.setText( "Leeward Mid Panel Nest") elif (self.panel_type == 'leeward_tip_panel'): self._runscreen.placementNestTarget.setText( "Leeward Tip Panel Nest") else: raise Exception('Unknown panel type selected') def _to_error_screen(self): self.stackedWidget.setCurrentIndex(2) def _login_prompt(self): self.loginprompt = UserAuthenticationWindow() if self.loginprompt.exec_(): #self.loginprompt.show() #while(not self.loginprompt.returned): #pass return True else: return False def _open_config_options(self): if (self._login_prompt()): self.led_change(self.robotconnectionled, True) #def _open_overhead_camera_view(self): #def _open_gripper_camera_views(self): def _open_force_torque_data_plot(self): self.plot_container = [] self.x_data = np.arange(1) self.force_torque_app = QApplication([]) self.force_torque_plot_widget = pg.plot() self.force_torque_plot_widget.addLegend() #self.layout.addWidget(self.force_torque_plot_widget,0,1) self.force_torque_plot_widget.showGrid(x=True, y=True) self.force_torque_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m') self.force_torque_plot_widget.setLabel('bottom', 'Sample Number', 'n') self.plot_container.append( self.force_torque_plot_widget.plot(pen=(255, 0, 0), name="Torque X")) self.plot_container.append( self.force_torque_plot_widget.plot(pen=(0, 255, 0), name="Torque Y")) self.plot_container.append( self.force_torque_plot_widget.plot(pen=(0, 0, 255), name="Torque Z")) self.plot_container.append( self.force_torque_plot_widget.plot(pen=(255, 255, 0), name="Force X")) self.plot_container.append( self.force_torque_plot_widget.plot(pen=(0, 255, 255), name="Force Y")) self.plot_container.append( self.force_torque_plot_widget.plot(pen=(255, 0, 255), name="Force Z")) #self.force_torque_plotter=PlotManager(title='Force Torque Data',nline=3,widget=self.force_torque_plot_widget) #self.force_torque_plot_widget.show() #self.force_torque_plotter.add("Hello", np.arange(10)) #self.force_torque_plotter.update() #self.rosGraph.show() #self.rosGraph.exec_() def _open_joint_angle_data_plot(self): self.plot_container = [] self.x_data = np.arange(1) self.joint_angle_app = QApplication([]) self.joint_angle_plot_widget = pg.plot() self.joint_angle_plot_widget.addLegend() #self.layout.addWidget(self.joint_angle_plot_widget,0,1) self.joint_angle_plot_widget.showGrid(x=True, y=True) self.joint_angle_plot_widget.setLabel('left', 'Force/Torque', 'N/N*m') self.joint_angle_plot_widget.setLabel('bottom', 'Sample Number', 'n') self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(255, 0, 0), name="Joint 1")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(0, 255, 0), name="Joint 2")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(0, 0, 255), name="Joint 3")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(255, 255, 0), name="Joint 4")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(0, 255, 255), name="Joint 5")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(255, 0, 255), name="Joint 6")) self.plot_container.append( self.joint_angle_plot_widget.plot(pen=(255, 255, 255), name="Joint 7")) def _open_rviz_prompt(self): subprocess.Popen(['python', self.rviz_starter]) # def _open_advanced_options(self): # main = Main() # sys.exit(main.main(sys.argv, standalone='rqt_rviz/RViz', plugin_argument_provider=add_arguments)) def _raise_rviz_window(self): subprocess.call(["xdotool", "search", "--name", "rviz", "windowraise"]) def _next_plan(self): #TODO: Disable previous plan if planListIndex==2 or 4 self._runscreen.nextPlan.setDisabled(True) self._runscreen.previousPlan.setDisabled(True) self._runscreen.resetToHome.setDisabled(True) self.reset_teleop_button() #TODO Make it change color when in motion if (self.planListIndex + 1 == self._runscreen.planList.count()): self.planListIndex = 0 elif (self.recover_from_pause): self.recover_from_pause = False else: self.planListIndex += 1 #g=GUIStepGoal(self.gui_execute_states[self.planListIndex], self.panel_type) #self.client_handle=self.client.send_goal(g,done_cb=self._process_done,feedback_cb=self._feedback_receive) self.step_executor._nextPlan(self.panel_type, self.planListIndex, self.placement_target) self._runscreen.planList.item(self.planListIndex).setSelected(True) if (self.rewound): self.rewound = False self._runscreen.previousPlan.setDisabled(False) """ self._runscreen.vacuum.setText("OFF") self._runscreen.panel.setText("Detached") self._runscreen.panelTag.setText("Not Localized") self._runscreen.nestTag.setText("Not Localized") self._runscreen.overheadCamera.setText("OFF") self._runscreen.gripperCamera.setText("OFF") self._runscreen.forceSensor.setText("Biased to 0") self._runscreen.pressureSensor.setText("[0,0,0]") """ ''' elif(self.planListIndex==1): self.send_thread=threading.Thread(target=self._execute_steps,args=(1,self.last_step, self.panel_type,0)) rospy.loginfo("thread_started") self.send_thread.setDaemon(True) self.send_thread.start() self._send_event.set() #self._execute_step('plan_pickup_prepare',self.panel_type) #self._execute_step('move_pickup_prepare')''' """ self._runscreen.vacuum.setText("OFF") self._runscreen.panel.setText("Detached") self._runscreen.panelTag.setText("Localized") self._runscreen.nestTag.setText("Not Localized") self._runscreen.overheadCamera.setText("ON") self._runscreen.gripperCamera.setText("OFF") self._runscreen.forceSensor.setText("ON") self._runscreen.pressureSensor.setText("[0,0,0]") """ """ elif(self.planListIndex==2): self.send_thread=threading.Thread(target=self._execute_steps,args=(2,self.last_step)) self.send_thread.setDaemon(True) self.send_thread.start() self._send_event.set()""" """ self._execute_step('plan_pickup_lower') self._execute_step('move_pickup_lower') self._execute_step('plan_pickup_grab_first_step') self._execute_step('move_pickup_grab_first_step') self._execute_step('plan_pickup_grab_second_step') self._execute_step('move_pickup_grab_second_step') self._execute_step('plan_pickup_raise') self._execute_step('move_pickup_raise') self._runscreen.vacuum.setText("OFF") self._runscreen.panel.setText("Detached") self._runscreen.panelTag.setText("Localized")self.controller_commander=controller_commander_pkg.arm_composites_manufacturing_controller_commander() self._runscreen.nestTag.setText("Not Localized") self._runscreen.overheadCamera.setText("OFF") self._runscreen.gripperCamera.setText("OFF") self._runscreen.forceSensor.setText("ON") self._runscreen.pressureSensor.setText("[0,0,0]") """ """ elif(self.planListIndex==3): if(self.panel_type=="leeward_mid_panel"): subprocess.Popen(['python', self.YC_transport_code, 'leeward_mid_panel']) elif(self.panel_type=="leeward_tip_panel"): subprocess.Popen(['python', self.YC_transport_code, 'leeward_tip_panel']) self.commands_sent=True """ #self.send_thread=threading.Thread(target=self._execute_steps,args=(3,self.last_step,self.placement_target,0)) #self.send_thread.setDaemon(True) #self.send_thread.start() #self._send_event.set()""" """ self._execute_step('plan_transport_payload',self.placement_target) self._execute_step('move_transport_payload') self._runscreen.vacuum.setText("ON") self._runscreen.panel.setText("Attached") self._runscreen.panelTag.setText("Localized") self._runscreen.nestTag.setText("Not Localized") self._runscreen.overheadCamera.setText("OFF") self._runscreen.gripperCamera.setText("OFF") self._runscreen.forceSensor.setText("ON") self._runscreen.pressureSensor.setText("[1,1,1]") """ """ elif(self.planListIndex==4): if(self.panel_type=="leeward_mid_panel"): subprocess.Popen(['python', self.YC_place_code]) elif(self.panel_type=="leeward_tip_panel"): subprocess.Popen(['python', self.YC_place_code2]) self.commands_sent=True """ """ self._runscreen.vacuum.setText("ON") self._runscreen.panel.setText("Attached") self._runscreen.panelTag.setText("Localized") self._runscreen.nestTag.setText("Not Localized") self._runscreen.overheadCamera.setText("OFF") self._runscreen.gripperCamera.setText("OFF") self._runscreen.forceSensor.setText("OFF") self._runscreen.pressureSensor.setText("[1,1,1]") """ def _stopPlan(self): #self.client.cancel_all_goals() #self.process_client.cancel_all_goals() #g=GUIStepGoal("stop_plan", self.panel_type) #self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive) self.step_executor._stopPlan() self.recover_from_pause = True self._runscreen.nextPlan.setDisabled(False) self._runscreen.previousPlan.setDisabled(False) self._runscreen.resetToHome.setDisabled(False) self.reset_teleop_button() def _previousPlan(self): if (self.planListIndex == 0): self.planListIndex = self._runscreen.planList.count() - 1 else: self.planListIndex -= 1 self.reset_teleop_button() self._runscreen.planList.item(self.planListIndex).setSelected(True) # self.rewound=True #self._runscreen.previousPlan.setDisabled(True) #g=GUIStepGoal("previous_plan", self.panel_type) #self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive,done_cb=self._process_done) self.step_executor._previousPlan() @pyqtSlot() def _feedback_receive(self): with self._lock: messagewindow = VacuumConfirm() error_msg = self.step_executor.error confirm = QMessageBox.warning( messagewindow, 'Error', 'Operation failed with error:\n' + error_msg, QMessageBox.Ok, QMessageBox.Ok) self._runscreen.nextPlan.setDisabled(False) self._runscreen.previousPlan.setDisabled(False) self._runscreen.resetToHome.setDisabled(False) def process_state_set(self, data): #if(data.state!="moving"): self._runscreen.nextPlan.setDisabled(False) self._runscreen.previousPlan.setDisabled(False) self._runscreen.resetToHome.setDisabled(False) def _reset_position(self): messagewindow = VacuumConfirm() reply = QMessageBox.question(messagewindow, 'Path Verification', 'Proceed to Reset Position', QMessageBox.Yes | QMessageBox.No, QMessageBox.No) if reply == QMessageBox.Yes: self.planListIndex = 0 #g=GUIStepGoal("reset", self.panel_type) #self.client_handle=self.client.send_goal(g,feedback_cb=self._feedback_receive) self.reset_teleop_button() self.step_executor._nextPlan(None, self.planListIndex) self._runscreen.planList.item(self.planListIndex).setSelected(True) #subprocess.Popen(['python', self.reset_code]) else: rospy.loginfo("Reset Rejected") def start_shared_control(self): if (self.planListIndex + 1 == self._runscreen.planList.count()): self.planListIndex = 0 elif self.recover_from_pause: self.recover_from_pause = False else: self.planListIndex += 1 if (self.planListIndex == 1): self._execute_step('plan_pickup_prepare', self.panel_type) elif (self.planListIndex == 2): self._execute_step('plan_pickup_lower') elif (self.planListIndex == 3): self._execute_step('plan_pickup_grab_first_step') #TODO: How to handle what happens after threshold exceeded to generate next plan step elif (self.planListIndex == 4): self._execute_step('plan_pickup_raise') elif (self.planListIndex == 5): self._execute_step('plan_transport_payload', self.placement_target) #self.set_controller_mode(self.controller_commander.MODE_SHARED_TRAJECTORY, 1, [],[]) def change_teleop_modes(self): with self._lock: self.current_teleop_mode += 1 try: if (self.current_teleop_mode == len(self.teleop_modes)): self.current_teleop_mode = 1 self.controller_commander.set_controller_mode( self.controller_commander.MODE_HALT, 1, [], []) elif (self.current_teleop_mode == 1): self.reset_teleop_button() elif (self.current_teleop_mode == 2): self.controller_commander.set_controller_mode( self.controller_commander.MODE_JOINT_TELEOP, 1, [], []) elif (self.current_teleop_mode == 3): self.controller_commander.set_controller_mode( self.controller_commander.MODE_CARTESIAN_TELEOP, 1, [], []) elif (self.current_teleop_mode == 4): self.controller_commander.set_controller_mode( self.controller_commander.MODE_CYLINDRICAL_TELEOP, 1, [], []) elif (self.current_teleop_mode == 5): self.controller_commander.set_controller_mode( self.controller_commander.MODE_SPHERICAL_TELEOP, 1, [], []) rospy.loginfo("Entering teleop mode:" + self.teleop_modes[self.current_teleop_mode]) button_string = self.teleop_button_string + self.teleop_modes[ self.current_teleop_mode] self._runscreen.accessTeleop.setText(button_string) except Exception as err: rospy.loginfo(str(err)) self.step_executor.error = "Controller failed to set teleop mode" self.step_executor.error_signal.emit() def set_controller_mode(self, mode, speed_scalar=1.0, ft_bias=[], ft_threshold=[]): req = SetControllerModeRequest() req.mode.mode = mode req.speed_scalar = speed_scalar req.ft_bias = ft_bias req.ft_stop_threshold = ft_threshold res = self._set_controller_mode(req) if (res.error_code.mode != ControllerMode.MODE_SUCCESS): raise Exception("Could not set controller mode") def error_recovery_button(self): self.current_teleop_mode = 0 self._runscreen.accessTeleop.setText("Recover from Error") def reset_teleop_button(self): self.current_teleop_mode = 1 self.controller_commander.set_controller_mode( self.controller_commander.MODE_HALT, 1, [], []) button_string = self.teleop_button_string + self.teleop_modes[ self.current_teleop_mode] self._runscreen.accessTeleop.setText(button_string) def callback(self, data): #self._widget.State_info.append(data.mode) with self._lock: if (self.stackedWidget.currentIndex() == 2): if (self.count > 10): #stringdata=str(data.mode) #freeBytes.acquire() #####consoleData.append(str(data.mode)) self._errordiagnosticscreen.consoleWidget_2.addItem( str(data.joint_position)) self.count = 0 #print data.joint_position self.count += 1 #self._widget.State_info.scrollToBottom() #usedBytes.release() #self._data_array.append(stringdata) #print self._widget.State_info.count() if (self._errordiagnosticscreen.consoleWidget_2.count() > 200): item = self._errordiagnosticscreen.consoleWidget_2.takeItem( 0) #print "Hello Im maxed out" del item ''' if self.in_process: if self.client.get_state() == actionlib.GoalStatus.PENDING or self.client.get_state() == actionlib.GoalStatus.ACTIVE: self._runscreen.nextPlan.setDisabled(True) self._runscreen.previousPlan.setDisabled(True) self._runscreen.resetToHome.setDisabled(True) rospy.loginfo("Pending") elif self.client.get_state() == actionlib.GoalStatus.SUCCEEDED: self._runscreen.nextPlan.setDisabled(False) self._runscreen.previousPlan.setDisabled(False) self._runscreen.resetToHome.setDisabled(False) self.in_process=False rospy.loginfo("Succeeded") elif self.client.get_state() == actionlib.GoalStatus.ABORTED: self.in_process=False if(not self.recover_from_pause): raise Exception("Process step failed and aborted") elif self.client.get_state() == actionlib.GoalStatus.REJECTED: self.in_process=False raise Exception("Process step failed and Rejected") elif self.client.get_state() == actionlib.GoalStatus.LOST: self.in_process=False raise Exception("Process step failed and lost") ''' if (self.count > 10): self.count = 0 if (data.mode.mode < 0): ''' #self.stackedWidget.setCurrentIndex(2) if(data.mode.mode==-5 or data.mode.mode==-6): error_msg="Error mode %d : Controller is not synched or is in Invalid State" %data.mode.mode self._errordiagnosticscreen.errorLog.setPlainText(error_msg) if(data.mode.mode==-3 or data.mode.mode==-2): error_msg="Error mode %d : Controller operation or argument is invalid" %data.mode.mode self._errordiagnosticscreen.errorLog.setPlainText(error_msg) if(data.mode.mode==-13 or data.mode.mode==-14): error_msg="Error mode %d : Sensor fault or communication Error" %data.mode.mode self._errordiagnosticscreen.errorLog.setPlainText(error_msg) if(data.mode.mode==-1): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -1: Internal system error detected") if(data.mode.mode==-4): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -4: Robot Fault detected") if(data.mode.mode==-7): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -7: Robot singularity detected, controller cannot perform movement") if(data.mode.mode==-8): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -8: Robot Setpoint could not be tracked, robot location uncertain") if(data.mode.mode==-9): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -9: Commanded Trajectory is invalid and cannot be executed. Please replan") if(data.mode.mode==-10): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -10: Trajectory Tracking Error detected, robot position uncertain, consider lowering speed of operation to improve tracking") if(data.mode.mode==-11): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -11: Robot trajectory aborted.") if(data.mode.mode==-12): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -12: Robot Collision Imminent, operation stopped to prevent damage") if(data.mode.mode==-15): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -15: Sensor state is invalid") if(data.mode.mode==-16): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -16: Force Torque Threshold Violation detected, stopping motion to prevent potential collisions/damage") if(data.mode.mode==-17): self._errordiagnosticscreen.errorLog.setPlainText("Error mode -17: Invalid External Setpoint given") ''' #messagewindow=VacuumConfirm() #reply = QMessageBox.question(messagewindow, 'Connection Lost', # 'Robot Connection Lost, Return to Welcome Screen?', QMessageBox.Yes | QMessageBox.No, QMessageBox.No) #if reply==QMessageBox.Yes: # # self.disconnectreturnoption=False #else: # self.disconnectreturnoption=True self.led_change(self.robotconnectionled, False) self.led_change(self.runscreenstatusled, False) self.error_recovery_button() else: self.led_change(self.robotconnectionled, True) self.led_change(self.runscreenstatusled, True) if (data.ft_wrench_valid == "False"): self.stackedWidget.setCurrentIndex(0) self.led_change(self.forcetorqueled, False) else: self.led_change(self.forcetorqueled, True) #self.service_list=rosservice.get_service_list() #if(self.disconnectreturnoption and data.error_msg==""): # self.disconnectreturnoption=False self.count += 1 if (not self.force_torque_plot_widget.isHidden()): self.x_data = np.concatenate((self.x_data, [data.header.seq])) incoming = np.array([ data.ft_wrench.torque.x, data.ft_wrench.torque.y, data.ft_wrench.torque.z, data.ft_wrench.force.x, data.ft_wrench.force.y, data.ft_wrench.force.z ]).reshape(6, 1) self.force_torque_data = np.concatenate( (self.force_torque_data, incoming), axis=1) if (self.data_count > 500): self.force_torque_data = self.force_torque_data[..., 1:] self.x_data = self.x_data[1:] self.force_torque_plot_widget.setRange( xRange=(self.x_data[1], self.x_data[-1])) else: self.data_count += 1 for i in range(6): self.plot_container[i].setData( self.x_data, self.force_torque_data[i, ...]) self.force_torque_app.processEvents() if (not self.joint_angle_plot_widget.isHidden()): self.x_data = np.concatenate((self.x_data, [data.header.seq])) incoming = np.array([ data.ft_wrench.torque.x, data.ft_wrench.torque.y, data.ft_wrench.torque.z, data.ft_wrench.force.x, data.ft_wrench.force.y, data.ft_wrench.force.z ]).reshape(7, 1) self.joint_angle_data = np.concatenate( (self.joint_angle_data, incoming), axis=1) if (self.data_count > 500): self.joint_angle_data = self.joint_angle_data[..., 1:] self.x_data = self.x_data[1:] self.joint_angle_plot_widget.setRange( xRange=(self.x_data[1], self.x_data[-1])) else: self.data_count += 1 for i in range(7): self.plot_container[i].setData( self.x_data, self.joint_angle_data[i, ...]) self.joint_angle_app.processEvents() #if(len(self._data_array)>10): # for x in self._data_array: # self._widget.State_info.append(x) 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 RobotSteering(Plugin): slider_factor = 1000.0 def __init__(self, context): super(RobotSteering, self).__init__(context) self.setObjectName('RobotSteering') self._publisher = None self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource', 'RobotSteering.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('RobotSteeringUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed) self._widget.stop_push_button.pressed.connect(self._on_stop_pressed) self._widget.x_linear_slider.valueChanged.connect(self._on_x_linear_slider_changed) self._widget.z_angular_slider.valueChanged.connect(self._on_z_angular_slider_changed) self._widget.increase_x_linear_push_button.pressed.connect(self._on_strong_increase_x_linear_pressed) self._widget.reset_x_linear_push_button.pressed.connect(self._on_reset_x_linear_pressed) self._widget.decrease_x_linear_push_button.pressed.connect(self._on_strong_decrease_x_linear_pressed) self._widget.increase_z_angular_push_button.pressed.connect(self._on_strong_increase_z_angular_pressed) self._widget.reset_z_angular_push_button.pressed.connect(self._on_reset_z_angular_pressed) self._widget.decrease_z_angular_push_button.pressed.connect(self._on_strong_decrease_z_angular_pressed) self._widget.max_x_linear_double_spin_box.valueChanged.connect(self._on_max_x_linear_changed) self._widget.min_x_linear_double_spin_box.valueChanged.connect(self._on_min_x_linear_changed) self._widget.max_z_angular_double_spin_box.valueChanged.connect(self._on_max_z_angular_changed) self._widget.min_z_angular_double_spin_box.valueChanged.connect(self._on_min_z_angular_changed) self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget) self.shortcut_w.setContext(Qt.ApplicationShortcut) self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed) self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget) self.shortcut_x.setContext(Qt.ApplicationShortcut) self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget) self.shortcut_s.setContext(Qt.ApplicationShortcut) self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed) self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget) self.shortcut_a.setContext(Qt.ApplicationShortcut) self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed) self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget) self.shortcut_z.setContext(Qt.ApplicationShortcut) self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget) self.shortcut_d.setContext(Qt.ApplicationShortcut) self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed) self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W), self._widget) self.shortcut_shift_w.setContext(Qt.ApplicationShortcut) self.shortcut_shift_w.activated.connect(self._on_strong_increase_x_linear_pressed) self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X), self._widget) self.shortcut_shift_x.setContext(Qt.ApplicationShortcut) self.shortcut_shift_x.activated.connect(self._on_reset_x_linear_pressed) self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S), self._widget) self.shortcut_shift_s.setContext(Qt.ApplicationShortcut) self.shortcut_shift_s.activated.connect(self._on_strong_decrease_x_linear_pressed) self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A), self._widget) self.shortcut_shift_a.setContext(Qt.ApplicationShortcut) self.shortcut_shift_a.activated.connect(self._on_strong_increase_z_angular_pressed) self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z), self._widget) self.shortcut_shift_z.setContext(Qt.ApplicationShortcut) self.shortcut_shift_z.activated.connect(self._on_reset_z_angular_pressed) self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D), self._widget) self.shortcut_shift_d.setContext(Qt.ApplicationShortcut) self.shortcut_shift_d.activated.connect(self._on_strong_decrease_z_angular_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space), self._widget) self.shortcut_space.setContext(Qt.ApplicationShortcut) self.shortcut_space.activated.connect(self._on_stop_pressed) self._widget.stop_push_button.setToolTip(self._widget.stop_push_button.toolTip() + ' ' + self.tr('([Shift +] Space)')) self._widget.increase_x_linear_push_button.setToolTip(self._widget.increase_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] W)')) self._widget.reset_x_linear_push_button.setToolTip(self._widget.reset_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] X)')) self._widget.decrease_x_linear_push_button.setToolTip(self._widget.decrease_x_linear_push_button.toolTip() + ' ' + self.tr('([Shift +] S)')) self._widget.increase_z_angular_push_button.setToolTip(self._widget.increase_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] A)')) self._widget.reset_z_angular_push_button.setToolTip(self._widget.reset_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] Z)')) self._widget.decrease_z_angular_push_button.setToolTip(self._widget.decrease_z_angular_push_button.toolTip() + ' ' + self.tr('([Shift +] D)')) # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) self._update_parameter_timer.timeout.connect(self._on_parameter_changed) self._update_parameter_timer.start(100) self.zero_cmd_sent = False @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() try: self._publisher = rospy.Publisher(topic, Twist, queue_size=10) except TypeError: self._publisher = rospy.Publisher(topic, Twist) def _on_stop_pressed(self): self._widget.x_linear_slider.setValue(0) self._widget.z_angular_slider.setValue(0) def _on_x_linear_slider_changed(self): self._widget.current_x_linear_label.setText('%0.2f m/s' % (self._widget.x_linear_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_z_angular_slider_changed(self): self._widget.current_z_angular_label.setText('%0.2f rad/s' % (self._widget.z_angular_slider.value() / RobotSteering.slider_factor)) self._on_parameter_changed() def _on_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.singleStep()) def _on_reset_x_linear_pressed(self): self._widget.x_linear_slider.setValue(0) def _on_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.singleStep()) def _on_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.singleStep()) def _on_reset_z_angular_pressed(self): self._widget.z_angular_slider.setValue(0) def _on_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.singleStep()) def _on_max_x_linear_changed(self, value): self._widget.x_linear_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_x_linear_changed(self, value): self._widget.x_linear_slider.setMinimum(value * RobotSteering.slider_factor) def _on_max_z_angular_changed(self, value): self._widget.z_angular_slider.setMaximum(value * RobotSteering.slider_factor) def _on_min_z_angular_changed(self, value): self._widget.z_angular_slider.setMinimum(value * RobotSteering.slider_factor) def _on_strong_increase_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() + self._widget.x_linear_slider.pageStep()) def _on_strong_decrease_x_linear_pressed(self): self._widget.x_linear_slider.setValue(self._widget.x_linear_slider.value() - self._widget.x_linear_slider.pageStep()) def _on_strong_increase_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() + self._widget.z_angular_slider.pageStep()) def _on_strong_decrease_z_angular_pressed(self): self._widget.z_angular_slider.setValue(self._widget.z_angular_slider.value() - self._widget.z_angular_slider.pageStep()) def _on_parameter_changed(self): self._send_twist(self._widget.x_linear_slider.value() / RobotSteering.slider_factor, self._widget.z_angular_slider.value() / RobotSteering.slider_factor) def _send_twist(self, x_linear, z_angular): if self._publisher is None: return twist = Twist() twist.linear.x = x_linear twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = z_angular # Only send the zero command once so other devices can take control if x_linear == 0 and z_angular == 0: if not self.zero_cmd_sent: self.zero_cmd_sent = True self._publisher.publish(twist) else: self.zero_cmd_sent = False self._publisher.publish(twist) def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None def shutdown_plugin(self): self._update_parameter_timer.stop() self._unregister_publisher() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('topic' , self._widget.topic_line_edit.text()) instance_settings.set_value('vx_max', self._widget.max_x_linear_double_spin_box.value()) instance_settings.set_value('vx_min', self._widget.min_x_linear_double_spin_box.value()) instance_settings.set_value('vw_max', self._widget.max_z_angular_double_spin_box.value()) instance_settings.set_value('vw_min', self._widget.min_z_angular_double_spin_box.value()) def restore_settings(self, plugin_settings, instance_settings): value = instance_settings.value('topic', "/cmd_vel") value = rospy.get_param("~default_topic", value) self._widget.topic_line_edit.setText(value) value = self._widget.max_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_max', value) value = rospy.get_param("~default_vx_max", value) self._widget.max_x_linear_double_spin_box.setValue(float(value)) value = self._widget.min_x_linear_double_spin_box.value() value = instance_settings.value( 'vx_min', value) value = rospy.get_param("~default_vx_min", value) self._widget.min_x_linear_double_spin_box.setValue(float(value)) value = self._widget.max_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_max', value) value = rospy.get_param("~default_vw_max", value) self._widget.max_z_angular_double_spin_box.setValue(float(value)) value = self._widget.min_z_angular_double_spin_box.value() value = instance_settings.value( 'vw_min', value) value = rospy.get_param("~default_vw_min", value) self._widget.min_z_angular_double_spin_box.setValue(float(value))
class StanUiPlugin(Plugin): def __init__(self, context): super(StanUiPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('StanUiPlugin') # 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('stan_ui'), 'resource', 'stan_ui_widget.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('StanUiWidget') # 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._widget.StartMotorButton.clicked[bool].connect( self._handle_start_motor_button_clicked) self.motors_started = False print("UpdatedUI3") def _handle_start_motor_button_clicked(self, event): if self.motors_started: print("Stopping motors") self._widget.StartMotorButton.setText("Start Motors") self.motors_started = False else: print("Starting motors") self._widget.StartMotorButton.setText("Stop Motors") self.motors_started = True def shutdown_plugin(self): # TODO unregister all publishers and stop timers 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 ControllerManager(Plugin): """ Graphical frontend for managing ros_control controllers. """ _cm_update_freq = 1 # Hz def __init__(self, context): super(ControllerManager, self).__init__(context) self.setObjectName('ControllerManager') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_manager.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('ControllerManagerUi') # Pop-up that displays controller information self._popup_widget = QWidget() ui_file = os.path.join(rp.get_path('rqt_controller_manager'), 'resource', 'controller_info.ui') loadUi(ui_file, self._popup_widget) self._popup_widget.setObjectName('ControllerInfoUi') # 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) # Initialize members self._cm_ns = [] # Namespace of the selected controller manager self._controllers = [] # State of each controller self._table_model = None self._controller_lister = None # Controller manager service proxies self._load_srv = None self._unload_srv = None self._switch_srv = None # Controller state icons rospack = rospkg.RosPack() path = rospack.get_path('rqt_controller_manager') self._icons = {'running': QIcon(path + '/resource/led_green.png'), 'stopped': QIcon(path + '/resource/led_red.png'), 'uninitialized': QIcon(path + '/resource/led_off.png')} # Controllers display table_view = self._widget.table_view table_view.setContextMenuPolicy(Qt.CustomContextMenu) table_view.customContextMenuRequested.connect(self._on_ctrl_menu) table_view.doubleClicked.connect(self._on_ctrl_info) header = table_view.horizontalHeader() header.setSectionResizeMode(QHeaderView.ResizeToContents) header.setContextMenuPolicy(Qt.CustomContextMenu) header.customContextMenuRequested.connect(self._on_header_menu) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_ctrl_list_timer = QTimer(self) self._update_ctrl_list_timer.setInterval(1000.0 / self._cm_update_freq) self._update_ctrl_list_timer.timeout.connect(self._update_controllers) self._update_ctrl_list_timer.start() # Signal connections w = self._widget w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) def shutdown_plugin(self): self._update_cm_list_timer.stop() self._update_ctrl_list_timer.stop() self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns # Setup services for communicating with the selected controller manager self._set_cm_services(cm_ns) # Controller lister for the selected controller manager if cm_ns: self._controller_lister = ControllerLister(cm_ns) self._update_controllers() else: self._controller_lister = None def _set_cm_services(self, cm_ns): if cm_ns: # NOTE: Persistent services are used for performance reasons. # If the controller manager dies, we detect it and disconnect from # it anyway load_srv_name = _append_ns(cm_ns, 'load_controller') self._load_srv = rospy.ServiceProxy(load_srv_name, LoadController, persistent=True) unload_srv_name = _append_ns(cm_ns, 'unload_controller') self._unload_srv = rospy.ServiceProxy(unload_srv_name, UnloadController, persistent=True) switch_srv_name = _append_ns(cm_ns, 'switch_controller') self._switch_srv = rospy.ServiceProxy(switch_srv_name, SwitchController, persistent=True) else: self._load_srv = None self._unload_srv = None self._switch_srv = None def _update_controllers(self): # Find controllers associated to the selected controller manager controllers = self._list_controllers() # Update controller display, if necessary if self._controllers != controllers: self._controllers = controllers self._show_controllers() # NOTE: Model is recomputed from scratch def _list_controllers(self): """ @return List of controllers associated to a controller manager namespace. Contains both stopped/running controllers, as returned by the C{list_controllers} service, plus uninitialized controllers with configurations loaded in the parameter server. @rtype [str] """ if not self._cm_ns: return [] # Add loaded controllers first controllers = self._controller_lister() # Append potential controller configs found in the parameter server all_ctrls_ns = _resolve_controllers_ns(self._cm_ns) for name in get_rosparam_controller_names(all_ctrls_ns): add_ctrl = not any(name == ctrl.name for ctrl in controllers) if add_ctrl: type_str = _rosparam_controller_type(all_ctrls_ns, name) uninit_ctrl = ControllerState(name=name, type=type_str, state='uninitialized') controllers.append(uninit_ctrl) return controllers def _show_controllers(self): table_view = self._widget.table_view self._table_model = ControllerTable(self._controllers, self._icons) table_view.setModel(self._table_model) def _on_ctrl_menu(self, pos): # Get data of selected controller row = self._widget.table_view.rowAt(pos.y()) if row < 0: return # Cursor is not under a valid item ctrl = self._controllers[row] # Show context menu menu = QMenu(self._widget.table_view) if ctrl.state == 'running': action_stop = menu.addAction(self._icons['stopped'], 'Stop') action_kill = menu.addAction(self._icons['uninitialized'], 'Stop and Unload') elif ctrl.state == 'stopped': action_start = menu.addAction(self._icons['running'], 'Start') action_unload = menu.addAction(self._icons['uninitialized'], 'Unload') elif ctrl.state == 'uninitialized': action_load = menu.addAction(self._icons['stopped'], 'Load') action_spawn = menu.addAction(self._icons['running'], 'Load and Start') action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action if ctrl.state == 'running': if action is action_stop: self._stop_controller(ctrl.name) elif action is action_kill: self._stop_controller(ctrl.name) self._unload_controller(ctrl.name) elif ctrl.state == 'stopped': if action is action_start: self._start_controller(ctrl.name) elif action is action_unload: self._unload_controller(ctrl.name) elif ctrl.state == 'uninitialized': if action is action_load: self._load_controller(ctrl.name) if action is action_spawn: self._load_controller(ctrl.name) self._start_controller(ctrl.name) def _on_ctrl_info(self, index): popup = self._popup_widget ctrl = self._controllers[index.row()] popup.ctrl_name.setText(ctrl.name) popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() model_root = QStandardItem('Claimed Resources') res_model.appendRow(model_root) for hw_res in ctrl.claimed_resources: hw_iface_item = QStandardItem(hw_res.hardware_interface) model_root.appendRow(hw_iface_item) for res in hw_res.resources: res_item = QStandardItem(res) hw_iface_item.appendRow(res_item) popup.resource_tree.setModel(res_model) popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree)) popup.resource_tree.expandAll() popup.move(QCursor.pos()) popup.show() def _on_header_menu(self, pos): header = self._widget.table_view.horizontalHeader() # Show context menu menu = QMenu(self._widget.table_view) action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') action = menu.exec_(header.mapToGlobal(pos)) # Evaluate user action if action is action_toggle_auto_resize: if header.resizeMode(0) == QHeaderView.ResizeToContents: header.setSectionResizeMode(QHeaderView.Interactive) else: header.setSectionResizeMode(QHeaderView.ResizeToContents) def _load_controller(self, name): self._load_srv.call(LoadControllerRequest(name=name)) def _unload_controller(self, name): self._unload_srv.call(UnloadControllerRequest(name=name)) def _start_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[name], stop_controllers=[], strictness=strict) self._switch_srv.call(req) def _stop_controller(self, name): strict = SwitchControllerRequest.STRICT req = SwitchControllerRequest(start_controllers=[], stop_controllers=[name], strictness=strict) self._switch_srv.call(req)
class JointTrajectoryController(Plugin): """ Graphical frontend for a C{JointTrajectoryController}. There are two modes for interacting with a controller: 1. B{Monitor mode} Joint displays are updated with the state reported by the controller. This is a read-only mode and does I{not} send control commands. Every time a new controller is selected, it starts in monitor mode for safety reasons. 2. B{Control mode} Joint displays update the control command that is sent to the controller. Commands are sent periodically evan if the displays are not being updated by the user. To control the aggressiveness of the motions, the maximum speed of the sent commands can be scaled down using the C{Speed scaling control} This plugin is able to detect and keep track of all active controller managers, as well as the JointTrajectoryControllers that are I{running} in each controller manager. For a controller to be compatible with this plugin, it must comply with the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - The controller exposes the C{command} and C{state} topics in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. A reference implementation of the C{JointTrajectoryController} is available in the C{joint_trajectory_controller} ROS package. """ _cmd_pub_freq = 10.0 # Hz _widget_update_freq = 30.0 # Hz _ctrlrs_update_freq = 1 # Hz _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration jointStateChanged = Signal([dict]) def __init__(self, context): super(JointTrajectoryController, self).__init__(context) self.setObjectName('JointTrajectoryController') # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'), 'resource', 'joint_trajectory_controller.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('JointTrajectoryControllerUi') # Setup speed scaler speed_scaling = DoubleEditor(1.0, 100.0) speed_scaling.spin_box.setSuffix('%') speed_scaling.spin_box.setValue(50.0) speed_scaling.spin_box.setDecimals(0) speed_scaling.setEnabled(False) self._widget.speed_scaling_layout.addWidget(speed_scaling) self._speed_scaling_widget = speed_scaling speed_scaling.valueChanged.connect(self._on_speed_scaling_change) self._on_speed_scaling_change(speed_scaling.value()) # 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) # Initialize members self._jtc_name = [] # Name of selected joint trajectory controller self._cm_ns = [] # Namespace of the selected controller manager self._joint_pos = {} # name->pos map for joints of selected controller self._joint_names = [] # Ordered list of selected controller joints self._robot_joint_limits = {} # Lazily evaluated on first use # Timer for sending commands to active controller self._update_cmd_timer = QTimer(self) self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq) self._update_cmd_timer.timeout.connect(self._update_cmd_cb) # Timer for updating the joint widgets from the controller state self._update_act_pos_timer = QTimer(self) self._update_act_pos_timer.setInterval(1000.0 / self._widget_update_freq) self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_jtc_list_timer = QTimer(self) self._update_jtc_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) self._cmd_pub = None # Controller command publisher self._state_sub = None # Controller state subscriber self._list_controllers = None def shutdown_plugin(self): self._update_cmd_timer.stop() self._update_act_pos_timer.stop() self._update_cm_list_timer.stop() self._update_jtc_list_timer.stop() self._unregister_state_sub() self._unregister_cmd_pub() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) instance_settings.set_value('jtc_name', self._jtc_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) # Resore last session's controller, if running self._update_jtc_list() jtc_name = instance_settings.value('jtc_name') jtc_combo = self._widget.jtc_combo jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) except (ValueError): pass except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) def _update_jtc_list(self): # Clear controller list if no controller information is available if not self._list_controllers: self._widget.jtc_combo.clear() return # List of running controllers with a valid joint limits specification # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: self._robot_joint_limits = get_joint_limits() # Lazy evaluation valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits for name in _jtc_joint_names(jtc_info)) if has_limits: valid_jtc.append(jtc_info); valid_jtc_names = [data.name for data in valid_jtc] # Update widget update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() def _on_joint_state_change(self, actual_pos): assert(len(actual_pos) == len(self._joint_pos)) for name in actual_pos.keys(): self._joint_pos[name]['position'] = actual_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns if cm_ns: self._list_controllers = ControllerLister(cm_ns) # NOTE: Clear below is important, as different controller managers # might have controllers with the same name but different # configurations. Clearing forces controller re-discovery self._widget.jtc_combo.clear() self._update_jtc_list() else: self._list_controllers = None def _on_jtc_change(self, jtc_name): self._unload_jtc() self._jtc_name = jtc_name if self._jtc_name: self._load_jtc() def _on_jtc_enabled(self, val): # Don't allow enabling if there are no controllers selected if not self._jtc_name: self._widget.enable_button.setChecked(False) return # Enable/disable joint displays for joint_widget in self._joint_widgets(): joint_widget.setEnabled(val) # Enable/disable speed scaling self._speed_scaling_widget.setEnabled(val) if val: # Widgets send desired position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: # Controller updates widgets with actual position self._update_cmd_timer.stop() self._update_act_pos_timer.start() def _load_jtc(self): # Initialize joint data corresponding to selected controller running_jtc = self._running_jtc_info() self._joint_names = next(_jtc_joint_names(x) for x in running_jtc if x.name == self._jtc_name) for name in self._joint_names: self._joint_pos[name] = {} # Update joint display try: layout = self._widget.joint_group.layout() for name in self._joint_names: limits = self._robot_joint_limits[name] joint_widget = DoubleEditor(limits['min_position'], limits['max_position']) layout.addRow(name, joint_widget) # NOTE: Using partial instead of a lambda because lambdas # "will not evaluate/look up the argument values before it is # effectively called, breaking situations like using a loop # variable inside it" from functools import partial par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) except: # TODO: Can we do better than swallow the exception? from sys import exc_info print 'Unexpected error:', exc_info()[0] # Enter monitor mode (sending commands disabled) self._on_jtc_enabled(False) # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) state_topic = jtc_ns + '/state' cmd_topic = jtc_ns + '/command' self._state_sub = rospy.Subscriber(state_topic, JointTrajectoryControllerState, self._state_cb, queue_size=1) self._cmd_pub = rospy.Publisher(cmd_topic, JointTrajectory, queue_size=1) # Start updating the joint positions self.jointStateChanged.connect(self._on_joint_state_change) def _unload_jtc(self): # Stop updating the joint positions try: self.jointStateChanged.disconnect(self._on_joint_state_change) except: pass # Reset ROS interfaces self._unregister_state_sub() self._unregister_cmd_pub() # Clear joint widgets # NOTE: Implementation is a workaround for: # https://bugreports.qt-project.org/browse/QTBUG-15990 :( layout = self._widget.joint_group.layout() if layout is not None: while layout.count(): layout.takeAt(0).widget().deleteLater() # Delete existing layout by reparenting to temporary QWidget().setLayout(layout) self._widget.joint_group.setLayout(QFormLayout()) # Reset joint data self._joint_names = [] self._joint_pos = {} # Enforce monitor mode (sending commands disabled) self._widget.enable_button.setChecked(False) def _running_jtc_info(self): from controller_manager_msgs.utils\ import filter_by_type, filter_by_state controller_list = self._list_controllers() jtc_list = filter_by_type(controller_list, 'JointTrajectoryController', match_substring=True) running_jtc_list = filter_by_state(jtc_list, 'running') return running_jtc_list def _unregister_cmd_pub(self): if self._cmd_pub is not None: self._cmd_pub.unregister() self._cmd_pub = None def _unregister_state_sub(self): if self._state_sub is not None: self._state_sub.unregister() self._state_sub = None def _state_cb(self, msg): actual_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] joint_pos = msg.actual.positions[i] actual_pos[joint_name] = joint_pos self.jointStateChanged.emit(actual_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]['command'] = val def _update_cmd_cb(self): dur = [] traj = JointTrajectory() traj.joint_names = self._joint_names point = JointTrajectoryPoint() for name in traj.joint_names: pos = self._joint_pos[name]['position'] cmd = pos try: cmd = self._joint_pos[name]['command'] except (KeyError): pass max_vel = self._robot_joint_limits[name]['max_velocity'] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) point.positions.append(cmd) point.time_from_start = rospy.Duration(max(dur) / self._speed_scale) traj.points.append(point) self._cmd_pub.publish(traj) def _update_joint_widgets(self): joint_widgets = self._joint_widgets() for id in range(len(joint_widgets)): joint_name = self._joint_names[id] try: joint_pos = self._joint_pos[joint_name]['position'] joint_widgets[id].setValue(joint_pos) except (KeyError): pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? widgets = [] layout = self._widget.joint_group.layout() for row_id in range(layout.rowCount()): widgets.append(layout.itemAt(row_id, QFormLayout.FieldRole).widget()) return widgets
class RQTDuckpit(Plugin): def __init__(self, context): super(RQTDuckpit, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Duckpit') # 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_duckpit'), 'resource', 'rqt_duckpit.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('rqt_duckpit') # 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.veh = rospy.get_param('/veh', "megabot08") # Subscribers # self.camera_topic = '/' + self.veh + '/camera_node/image/compressed' # self.sub_camera = rospy.Subscriber(self.camera_topic, sensor_msgs.msg.CompressedImage, self.cbImage, queue_size=1) self.steps_ahead = 5 self.cmd_topics = [] self.cmd_topics.append('/' + self.veh + '/inverse_kinematics_node/car_cmd') self.cmd_node_name = 'cnn_node' for i in range(1, self.steps_ahead): self.cmd_topics.append('/' + self.veh + '/' + self.cmd_node_name + '/car_cmd_' + str(i + 1)) self.sub_cmds = dict() for i in range(0, self.steps_ahead): self.sub_cmds[i] = rospy.Subscriber( self.cmd_topics[i], duckietown_msgs.msg.Twist2DStamped, self.cbCarCmds, callback_args=i + 1) self._widget.h_slider.setValue(0) self._widget.h_slider_2.setValue(0) self._widget.h_slider_3.setValue(0) self._widget.h_slider_4.setValue(0) self._widget.h_slider_5.setValue(0) self._widget.label_val.setText("0") self._widget.label_val_2.setText("0") self._widget.label_val_3.setText("0") self._widget.label_val_4.setText("0") self._widget.label_val_5.setText("0") def cbCarCmds(self, msg, num): if num == 1: self._widget.h_slider.setValue(msg.omega) self._widget.label_val.setText(str(msg.omega)) elif num == 2: self._widget.h_slider_2.setValue(msg.omega) self._widget.label_val_2.setText(str(msg.omega)) elif num == 3: self._widget.h_slider_3.setValue(msg.omega) self._widget.label_val_3.setText(str(msg.omega)) elif num == 4: self._widget.h_slider_4.setValue(msg.omega) self._widget.label_val_4.setText(str(msg.omega)) elif num == 5: self._widget.h_slider_5.setValue(msg.omega) self._widget.label_val_5.setText(str(msg.omega)) 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 PlexilPlanSelectionGUI(Plugin): #sets up our signal for the sub callback monitor_signal = pyqtSignal(['QString']) def __init__(self, context): '''init initializes the widget and sets up our subscriber, publisher and event handlers''' super(PlexilPlanSelectionGUI, self).__init__(context) self.setObjectName('PlexilPlanSelectionGUI') # Process standalone plugin command-line arguments parser = ArgumentParser() parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) # Find resources and Create QWidget self._widget = QWidget() ui_file = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'plexil_plan_selection.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('PlexilPlanSelectionUI') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) #set window icon icon = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'icon.png') self._widget.setWindowIcon(QIcon(icon)) #pub and sub setup self.status_publisher = rospy.Publisher('plexil_plan_selection_status', String, queue_size=20) self.status_subscriber = rospy.Subscriber('plexil_plan_selection_status', String, self.status_callback) #client placeholder self.plan_select_client = None #Qt signal to modify GUI from callback self.monitor_signal[str].connect(self.monitor_status) #populates the plan list, shows different plans based off of what launch file is running if rospy.get_param('owlat_flag', False): owlat_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans', 'owlat_plans') self.populate_plan_list(owlat_plan_dir) else: ow_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans') self.populate_plan_list(ow_plan_dir) #sets up tables self._widget.sentPlansTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents) self._widget.planQueueTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents) self._widget.sentPlansTable.insertColumn(0) self._widget.sentPlansTable.insertColumn(1) self._widget.planQueueTable.insertColumn(0) #sets up event listeners self._widget.addButton.clicked[bool].connect(self.handle_add_button_clicked) self._widget.removeButton.clicked[bool].connect(self.handle_remove_button_clicked) self._widget.upButton.clicked[bool].connect(self.handle_up_button_clicked) self._widget.downButton.clicked[bool].connect(self.handle_down_button_clicked) self._widget.sendButton.clicked[bool].connect(self.handle_send_button_clicked) self._widget.resetButton.clicked[bool].connect(self.handle_reset_button_clicked) def populate_plan_list(self, plan_dir): '''Finds all .ple plexil plans in the plans directory and stores them in the widget list.''' plan_list = [] #get all plans with extension .ple for p in os.listdir(plan_dir): if p.endswith(".ple"): plan_list.append(p.rsplit(".")[0]) #add to list self._widget.planList.addItems(plan_list) self._widget.planList.sortItems() def monitor_status(self, feedback): '''Signal from callback calls this function to do the work to avoid threading issued with the GUI, changes the status on the sent plans table to match the current plan status. Plan can be Pending, Running, Finished or Failed.''' num_rows = self._widget.sentPlansTable.rowCount() #if completed and previously running we set status as finished if feedback == "COMPLETE": for i in range(num_rows): current_status = self._widget.sentPlansTable.item(i,1).text() if current_status == "Running...": self._widget.sentPlansTable.item(i, 1).setText("Finished") self._widget.sentPlansTable.item(i, 1).setBackground(QColor(0,128,0)) break else: #splitting into plan name and status status = feedback.rsplit(":")[0] running_plan = feedback.rsplit(":")[1].rsplit(".")[0] #we set status to running or Failed depending on status for i in range(num_rows): plan_name = self._widget.sentPlansTable.item(i,0).text() current_status = self._widget.sentPlansTable.item(i,1).text() if plan_name == running_plan and current_status == "Pending...": if status == "SUCCESS": self._widget.sentPlansTable.item(i, 1).setText("Running...") break else: self._widget.sentPlansTable.item(i, 1).setText("Failed") self._widget.sentPlansTable.item(i, 1).setBackground(QColor(230,38,0)) break return def status_callback(self, msg): '''Callback from status subscriber. Sends the msg to the function monitor_signal for further processing in order to prevent threading issues in the GUI.''' #have to use a signal here or else GUI wont update feedback_string = str(msg.data) self.monitor_signal.emit(feedback_string) def handle_add_button_clicked(self, checked): '''When the add button is clicked this function moves any selected items to the plan queue table.''' #get selected items selected_plans = self._widget.planList.selectedItems() #create a new row in the queue table for each selection and insert it for i in selected_plans: row_position = self._widget.planQueueTable.rowCount() self._widget.planQueueTable.insertRow(row_position) self._widget.planQueueTable.setItem(row_position-1, 1, QTableWidgetItem(i.text())) self._widget.planList.selectionModel().clear() self._widget.planQueueTable.resizeColumnsToContents() return def handle_remove_button_clicked(self, checked): '''When the remove button is clicked this function deletes any selected items from the plan queue table.''' selected_rows = self._widget.planQueueTable.selectedItems() for i in selected_rows: self._widget.planQueueTable.removeRow(self._widget.planQueueTable.row(i)) self._widget.planQueueTable.selectionModel().clear() return def handle_up_button_clicked(self, checked): '''When up button is clicked this function moves the selected item up in the queue table.''' selected_row = self._widget.planQueueTable.currentRow() #checks we are not at top of list if selected_row <= 0: return #switches the two rows and puts selection on previously selected row else: belowTxt = self._widget.planQueueTable.item(selected_row,0).text() aboveTxt = self._widget.planQueueTable.item(selected_row-1,0).text() self._widget.planQueueTable.item(selected_row, 0).setText(aboveTxt) self._widget.planQueueTable.item(selected_row-1, 0).setText(belowTxt) self._widget.planQueueTable.selectionModel().clear() self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row-1,0)) return def handle_down_button_clicked(self, checked): '''When down button is clicked this function moves the selected item down in the queue table.''' selected_row = self._widget.planQueueTable.currentRow() num_rows = self._widget.planQueueTable.rowCount() #checks we are not at bottom of list if selected_row >= num_rows-1 or selected_row < 0: return #switches the two rows and puts selection on previously selected row else: belowTxt = self._widget.planQueueTable.item(selected_row+1,0).text() aboveTxt = self._widget.planQueueTable.item(selected_row,0).text() self._widget.planQueueTable.item(selected_row+1, 0).setText(aboveTxt) self._widget.planQueueTable.item(selected_row, 0).setText(belowTxt) self._widget.planQueueTable.selectionModel().clear() self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row+1,0)) return def handle_send_button_clicked(self, checked): '''When send button is clicked any items in the plan queue table are sent (in order) to the sent plans table. It then publishes a string array of these plans so that the ow_plexil_plan_selection node can run them sequentially. If the subscriber is not connected a popup box reminding the user to run the ow_plexil node will show up.''' if self.check_client_set_up() == False: return num_rows = self._widget.planQueueTable.rowCount() plans_sent = [] #creates a new row in the sent table for each item in the queue table and inserts it for i in range(num_rows): plan_name = self._widget.planQueueTable.item(0,0).text() plans_sent.append(str(plan_name + ".plx")) self._widget.planQueueTable.removeRow(0) row_position = self._widget.sentPlansTable.rowCount() self._widget.sentPlansTable.insertRow(row_position) self._widget.sentPlansTable.setItem(row_position, 0, QTableWidgetItem(plan_name)) self._widget.sentPlansTable.setItem(row_position, 1, QTableWidgetItem("Pending...")) self._widget.sentPlansTable.resizeColumnsToContents() # Create msg and send to subscriber for plan execution self.plan_select_client("ADD", plans_sent) return def handle_reset_button_clicked(self, checked): '''When reset button is clicked all plans in the sent plans table are deleted. It also publishes a string command RESET letting the ow_plexil_plan_selection node know that any plans in its queue should also be deleted.''' if self.check_client_set_up() == False: return num_rows = self._widget.sentPlansTable.rowCount() #deletes each row pressent in sentplanstable for i in range(num_rows): self._widget.sentPlansTable.removeRow(0) self.plan_select_client("RESET", []) return def check_client_set_up(self): '''Checks to see if the client is initialized, if not we check if the server is running before initializing the client. If server is not yet running we send a popup informing the user and return False.''' #checks to see if we have connected to service yet if self.plan_select_client == None: #we get list of services and see if any match plexil_plan_selection services = rosservice.get_service_list() service_running = [i for i in services if "plexil_plan_selection" in i] #if none exist we send a popup and return if len(service_running) == 0: popup = QMessageBox() popup.setWindowTitle("ow_plexil service not yet connected") popup.setText("ow_plexil plan selection service not connected yet, please make sure the ow_plexil launch file is running.") popup.exec_() return False else: #client setup rospy.wait_for_service('plexil_plan_selection') self.plan_select_client = rospy.ServiceProxy('plexil_plan_selection', PlanSelection) return True else: return True def shutdown_plugin(self): '''handles shutdown procedures for the plugin, unregisters the publisher and subscriber.''' self.status_subscriber.unregister() pass
class TfEcho(Plugin): # do_update_label = QtCore.pyqtSignal(String) def __init__(self, context): super(TfEcho, self).__init__(context) # Give QObjects reasonable names self.setObjectName('TfEcho') rp = rospkg.RosPack() # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # TODO(lucasw) these aren't working # parser.add_argument("source_frame", default='') # parent # parser.add_argument("target_frame", default='') # child 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.source_frame = '' self.target_frame = '' # self.source_frame = self.args.source_frame # self.target_frame = self.args.target_frame # 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(rp.get_path('rqt_tf_echo'), 'resource', 'tf_echo.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('TfEchoUi') # 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) cache_time = 10.0 self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(cache_time)) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # TODO(lucasw) ros param self.source_frame = "" self.target_frame = "" self.label = {} self.label['current_time'] = self._widget.findChild(QLabel, 'current_time_label') self.label['transform_time'] = self._widget.findChild(QLabel, 'transform_time_label') for axis in ['x', 'y', 'z', 'label']: self.label['trans_' + axis] = self._widget.findChild(QLabel, 'trans_' + axis + '_label') self.label['trans_' + axis + '_2'] = self._widget.findChild(QLabel, 'trans_' + axis + '_label_2') for axis in ['x', 'y', 'z', 'w']: self.label['quat_' + axis] = self._widget.findChild(QLabel, 'quat_' + axis + '_label') self.label['quat_' + axis + '_2'] = self._widget.findChild(QLabel, 'quat_' + axis + '_label_2') for unit in ['rad', 'deg']: for axis in ['roll', 'pitch', 'yaw', 'label']: name = 'rot_' + axis + '_' + unit self.label[name] = self._widget.findChild(QLabel, name + '_label') self.label[name + '_2'] = self._widget.findChild(QLabel, name + '_label_2') self.label['source'] = self._widget.findChild(QLineEdit, 'source_line_edit') self.label['target'] = self._widget.findChild(QLineEdit, 'target_line_edit') self._widget.setContextMenuPolicy(QtCore.Qt.ActionsContextMenu) self.actions = {} self.setup_menu() self.qt_timer = QTimer() self.qt_timer.start(200) self.qt_timer.timeout.connect(self.qt_update) def add_menu_item(self, menu_name, labels): menu_text = menu_name if self.label[labels[0]].isHidden(): menu_text = "Show " + menu_text else: menu_text = "Hide " + menu_text action = QAction(menu_text, self._widget) action.triggered.connect(partial(self.toggle_labels, labels)) self._widget.addAction(action) self.actions[menu_name] = action def setup_menu(self): for key in self.actions.keys(): self._widget.removeAction(self.actions[key]) self.add_menu_item("transform time", ["transform_time"]) self.add_menu_item("current time", ["current_time"]) self.add_menu_item("source/parent frame", ["source"]) self.add_menu_item("target/child frame", ["target"]) self.add_menu_item("translation", ["trans_x", "trans_y", "trans_z", "trans_label"]) self.add_menu_item("rotation quaternion", ["quat_x", "quat_y", "quat_z", "quat_w"]) self.add_menu_item("rotation (radians)", ["rot_roll_rad", "rot_pitch_rad", "rot_yaw_rad", "rot_label_rad"]) self.add_menu_item("rotation (degrees)", ["rot_roll_deg", "rot_pitch_deg", "rot_yaw_deg", "rot_label_deg"]) def toggle(self, name): if self.label[name].isHidden(): self.label[name].show() if name + '_2' in self.label.keys(): self.label[name + '_2'].show() else: self.label[name].hide() if name + '_2' in self.label.keys(): self.label[name + '_2'].hide() def toggle_labels(self, labels): for label in labels: self.toggle(label) self.setup_menu() def qt_update(self): lookup_time = rospy.Time() cur_time = rospy.Time.now().to_sec() self.source_frame = self.label['source'].text() self.target_frame = self.label['target'].text() # TODO(lucasw) add a text box that can be disabled, # put any error messages into it. msg = '' ts = None if self.source_frame != "" and self.target_frame != "": try: ts = self.tf_buffer.lookup_transform(self.source_frame, self.target_frame, lookup_time, rospy.Duration(0.005)) except tf2.TransformException as ex: msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time) rospy.logdebug(msg + str(ex)) except rospy.exceptions.ROSTimeMovedBackwardsException as ex: msg = str(ex) rospy.logdebug(str(ex)) # update the cur time in case lookup_transform was slow cur_time = rospy.Time.now().to_sec() self.label['current_time'].setText("{:1.2f}".format(cur_time)) if ts is None: # TODO(lucasw) need a dedicated status label in case the user # hides this? for label in ['source', 'target']: self.label[label].setStyleSheet('background-color: yellow') return for label in ['source', 'target']: self.label[label].setStyleSheet('background-color: white') self.label['transform_time'].setStyleSheet('') self.label['transform_time'].setText("{:1.2f}".format(ts.header.stamp.to_sec())) self.label['trans_x'].setText("{:1.3f}".format(ts.transform.translation.x)) self.label['trans_y'].setText("{:1.3f}".format(ts.transform.translation.y)) self.label['trans_z'].setText("{:1.3f}".format(ts.transform.translation.z)) quat = ts.transform.rotation self.label['quat_x'].setText("{:1.3f}".format(quat.x)) self.label['quat_y'].setText("{:1.3f}".format(quat.y)) self.label['quat_z'].setText("{:1.3f}".format(quat.z)) self.label['quat_w'].setText("{:1.3f}".format(quat.w)) euler = _euler_from_quaternion_msg(quat) axes = ['roll', 'pitch', 'yaw'] for i in range(3): self.label['rot_' + axes[i] + '_rad'].setText("{:1.3f}".format(euler[i])) self.label['rot_' + axes[i] + '_deg'].setText("{:1.3f}".format(math.degrees(euler[i]))) def shutdown_plugin(self): # TODO unregister all publishers here pass def get_param_setting(self, name, label_name, val, instance_settings): if val == '': val = rospy.get_param('~' + name, '') # load from instance_settings only if args/params aren't set if val == '' and instance_settings.contains(name): val = instance_settings.value(name) self.label[label_name].setText(val) return val def restore_settings(self, plugin_settings, instance_settings): self.source_frame = self.get_param_setting("source_frame", "source", self.source_frame, instance_settings) self.target_frame = self.get_param_setting("target_frame", "target", self.target_frame, instance_settings) for key in self.label.keys(): name = key + '_hidden' if instance_settings.contains(name): if instance_settings.value(name) == str(True): self.label[key].hide() self.setup_menu() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('source_frame', self.source_frame) instance_settings.set_value('target_frame', self.target_frame) for key in self.label.keys(): is_hidden = self.label[key].isHidden() name = key + '_hidden' instance_settings.set_value(name, str(is_hidden))
class MessageGUI(Plugin): def reply_msg_callback(self, msg_in): self._widget.reply.setText(msg_in.message) def arithmetic_reply_msg_callback(self, msg_in): display_text = 'Operation Type: '+msg_in.oper_type+'\n'+ \ 'Answer: '+str(msg_in.answer)+'\n'+ \ 'Time Received: '+str(msg_in.time_received)+'\n'+ \ 'Time Answered: '+str(msg_in.time_answered)+'\n'+ \ 'Process Time: '+str(msg_in.process_time) self._widget.reply.setText(display_text) def message_count_display(self, counter_val): display_text = '' if self.counter_req_id == 0: display_text = display_text + 'Total messages:' elif self.counter_req_id == 1: display_text = display_text + 'Total replied messages:' elif self.counter_req_id == 2: display_text = display_text + 'Total sent messages:' elif self.counter_req_id == 3: display_text = display_text + 'Time since last replied message:' elif self.counter_req_id == 4: display_text = display_text + 'Time since last sent message:' print(display_text) self._widget.counter_reply.setText(display_text + str(counter_val.reply)) def __init__(self, context): super(MessageGUI, self).__init__(context) self.setObjectName('MessageGUI') self.message_pub = rospy.Publisher("sent_msg", sent_msg, queue_size=1000) rospy.Subscriber("reply_msg", reply_msg, self.reply_msg_callback) rospy.Subscriber("arithmetic_reply", arithmetic_reply, self.arithmetic_reply_msg_callback) self.msg_to_send = sent_msg() self.counter_req_id = -1 self._widget = QWidget() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('message_ui'), 'resource', 'MessageGUI.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('MessageGUIui') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.message_to_send.textChanged.connect( self._on_msg_to_send_changed) self._widget.counter_val_to_get.textChanged.connect( self._on_counter_val_to_get_changed) self._widget.counter_val_to_get.setInputMask('9') self._widget.send_message.pressed.connect( self._on_send_message_pressed) self._widget.send_request.pressed.connect( self._on_send_request_pressed) def _on_msg_to_send_changed(self, msg): msg = str(msg) self.msg_to_send.message = msg def _on_send_message_pressed(self): self.msg_to_send.header.stamp = rospy.Time.now() self.message_pub.publish(self.msg_to_send) def _on_counter_val_to_get_changed(self, id): try: self.counter_req_id = int(id) except ValueError: print('String input is not an integer') def _on_send_request_pressed(self): rospy.wait_for_service('message_counter') try: counter_serv = rospy.ServiceProxy('message_counter', counter) response = counter_serv(self.counter_req_id) self.message_count_display(response) return response except rospy.ServiceException, ex: print "Service call to get message counter failed. %s" % e
class MAVROSGUI(Plugin): def __init__(self, context): super(MAVROSGUI, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MAVROSGUI') rp = rospkg.RosPack() # 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 is a sibling of this file # in this example the .ui and .py file are in the same folder ui_file = os.path.join(rp.get_path('rqt_mavros_gui'), 'resource', 'MAVROSGUI.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('MAVROSGUIUi') # 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.mavros_namespace = "/mavros" self._widget.button_safety_arm.clicked.connect( self.button_safety_arm_pressed) self._widget.button_safety_disarm.clicked.connect( self.button_safety_disarm_pressed) self._widget.button_param_refresh.clicked.connect( self.button_param_refresh_pressed) self._widget.button_param_set.clicked.connect( self.button_param_set_pressed) self._widget.button_flight_mode_set.clicked.connect( self.button_flight_mode_set_pressed) self._widget.combo_param_list.currentIndexChanged.connect( self.combo_param_list_pressed) self.flight_modes = sorted([ "MANUAL", "ACRO", "ALTCTL", "POSCTL", "OFFBOARD", "STABILIZED", "RATTITUDE", "AUTO.MISSION", "AUTO.LOITER", "AUTO.RTL", "AUTO.LAND", "AUTO.RTGS", "AUTO.READY", "AUTO.TAKEOFF" ]) for fm in self.flight_modes: self._widget.combo_flight_mode_list.addItem(fm) # Handled in the restore settings callback #self.refresh_mavros_backend() self.sub_state = None self.timer_state = None self.msg_state_last = rospy.Time(0) def shutdown_plugin(self): if self.sub_state: self.sub_state.unregister() if self.timer_state: self.timer_state.shutdown() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('namespace', self.mavros_namespace) instance_settings.set_value( 'mode_selection', self._widget.combo_flight_mode_list.currentText()) def restore_settings(self, plugin_settings, instance_settings): ns = instance_settings.value('namespace') if ns: self.mavros_namespace = str(ns) mode = str(instance_settings.value('mode_selection')) if mode in self.flight_modes: self._widget.combo_flight_mode_list.setCurrentIndex( self.flight_modes.index(mode)) self._refresh_mavros_backend() def trigger_configuration(self): """Present the user with a dialog for choosing the topic to view, the data type, and other settings used to generate the HUD. This displays a SimpleSettingsDialog asking the user to choose the settings as desired. This method is blocking""" dialog = SimpleSettingsDialog(title='Quaternion View Options') dialog.add_lineedit("namespace", str(self.mavros_namespace), "Namespace") settings = dialog.get_settings() if settings is not None: for s in settings: if s[0] == "namespace": self.mavros_namespace = str(s[1]) self._refresh_mavros_backend() def button_safety_arm_pressed(self): self._arm(True) rospy.logdebug("Safety arm button pressed!") def button_safety_disarm_pressed(self): self._arm(False) rospy.logdebug("Safety disarm button pressed!") def button_param_refresh_pressed(self): param_received = 0 try: param_received, param_list = param_get_all(False) rospy.logdebug("Parameters received: %s" % str(param_received)) except IOError as e: rospy.logerr(e) self._widget.combo_param_list.clear() param_id_list = list() if param_received > 0: self._widget.textbox_param_value.setEnabled(True) self._widget.button_param_set.setEnabled(True) for p in param_list: param_id_list.append(p.param_id) for p in sorted(param_id_list): self._widget.combo_param_list.addItem(p) def button_param_set_pressed(self): param_id = str(self._widget.combo_param_list.currentText()) val_str = str(self._widget.textbox_param_value.text()) if '.' in val_str: val = float(val_str) else: val = int(val_str) try: rospy.loginfo(param_set(param_id, val)) except IOError as e: rospy.logerr(e) rospy.logdebug("Set param button pressed!") def button_flight_mode_set_pressed(self): mode_req = str(self._widget.combo_flight_mode_list.currentText()) try: set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) ret = set_mode(base_mode=0, custom_mode=mode_req) if not ret.mode_sent: rospy.logerr("Request failed. Check mavros logs") except (rospy.ServiceException, IOError) as e: rospy.logerr(e) rospy.logdebug("Set param button pressed!") def combo_param_list_pressed(self, ): param_id = self._widget.combo_param_list.currentText() try: if param_id: self._widget.textbox_param_value.setText( str(param_get(param_id))) else: self._widget.textbox_param_value.setText("") except IOError as e: rospy.logerr(e) def _arm(self, state): try: ret = command.arming(value=state) if not ret.success: rospy.logerr("Request failed. Check mavros logs") rospy.loginfo("Command result: %s" % str(ret.result)) except rospy.ServiceException as ex: rospy.logerr(ex) def _cb_state(self, msg_in): self.msg_state_last = rospy.Time.now() if msg_in.connected: self._widget.label_status_live_conn.setText("Connected") else: self._widget.label_status_live_conn.setText("Disconnected") if msg_in.armed: self._widget.label_status_live_armed.setText("Armed") self._widget.label_status_live_armed.setStyleSheet( "QLabel{color: rgb(255, 255, 255);background-color: rgb(25, 116, 2);}" ) else: self._widget.label_status_live_armed.setText("Disarmed") self._widget.label_status_live_armed.setStyleSheet( "QLabel{color: rgb(255, 255, 255);background-color: rgb(116,2,25);}" ) if msg_in.mode == "CMODE(0)": self._widget.label_status_live_mode.setText("UNKNOWN") else: self._widget.label_status_live_mode.setText(msg_in.mode) def _state_monitor(self, event): if self.msg_state_last != rospy.Time(0): if (event.current_real - self.msg_state_last) > rospy.Duration(5): self.msg_state_last = rospy.Time(0) self._widget.label_status_live_conn.setText("Disconnected") self._widget.label_status_live_armed.setText("Disconnected") self._widget.label_status_live_armed.setStyleSheet("") self._widget.label_status_live_mode.setText("Disconnected") def _refresh_mavros_backend(self): mavros.set_namespace(self.mavros_namespace) if self.sub_state: self.sub_state.unregister() if self.timer_state: self.timer_state.shutdown() #Timer should trigger immidiately self.timer_state = rospy.Timer(rospy.Duration(1), self._state_monitor) self.sub_state = rospy.Subscriber(self.mavros_namespace + "/state", State, self._cb_state)
class Smach(Plugin): update_graph_sig = Signal(str) def __init__(self, context): super(Smach, self).__init__(context) self.initialized = 0 self._dotcode_sub = None self._topic_dict = {} self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) # Give QObjects reasonable names self.setObjectName('Smach') # 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 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__)), 'rqt_smach.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.setObjectName('SmachPluginUi') # 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) palette = QPalette() palette.setColor(QPalette.Background, Qt.white) self._widget.setPalette(palette) #Connect widgets with corresponding methods self._widget.namespace_input.currentIndexChanged.connect( self._handle_ns_changed) self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box) self._widget.restrict_ns.clicked.connect(self.refresh_combo_box) self._widget.ud_path_input.currentIndexChanged.connect( self._handle_ud_path) self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path) self._widget.ud_text_browser.setReadOnly(1) self._widget.show_implicit_button.clicked.connect( self._handle_show_implicit) self._widget.help_button.setIcon(QIcon.fromTheme('help-contents')) self._widget.help_button.clicked.connect(self._handle_help) self._widget.tree.clicked.connect(self._handle_tree_clicked) #Depth and width spinners: self._widget.depth_input.setRange(-1, 1337) self._widget.depth_input.setValue(-1) self._widget.depth_input.valueChanged.connect(self._set_depth) self._widget.label_width_input.setRange(1, 1337) self._widget.label_width_input.setValue(40) self._widget.label_width_input.valueChanged.connect(self._set_width) self._widget.tree.setColumnCount(1) self._widget.tree.setHeaderLabels(["Containers"]) self._widget.tree.show() self._ns = "" self.refresh_combo_box() # Bind path list self._widget.path_input.currentIndexChanged.connect( self._handle_path_changed) #Keep Combo Boxes sorted self._widget.namespace_input.setInsertPolicy(6) self._widget.path_input.setInsertPolicy(6) self._widget.ud_path_input.setInsertPolicy(6) #Set up mouse actions for xdot widget self._widget.xdot_widget.register_select_callback(self.select_cb) # Create graph data structures # Containers is a map of (container path) -> container proxy self._containers = {} self._top_containers = {} # smach introspection client self._client = smach_ros.IntrospectionClient() self._selected_paths = [] # Message subscribers self._structure_subs = {} self._status_subs = {} # Initialize xdot display state self._path = '/' self._needs_zoom = True self._structure_changed = True self._max_depth = -1 self._show_all_transitions = False self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True) self._graph_needs_refresh = True self._tree_needs_refresh = True self._keep_running = True self._update_server_list() self._update_graph() self._update_tree() # Start a timer to update the server list self._server_timer = QTimer(self) self._server_timer.timeout.connect(self._update_server_list) self._server_timer.start(1000) # Start a timer to update the graph display self._graph_timer = QTimer(self) self._graph_timer.timeout.connect(self._update_graph) self._graph_timer.start(1093) # Start a timer to update the._widget.tree display self._tree_timer = QTimer(self) self._tree_timer.timeout.connect(self._update_tree) self._tree_timer.start(1217) def _handle_tree_clicked(self): selected = self._widget.tree.selectedItems()[0] path = "/" + str(selected.text(0)) parent = selected.parent() while parent: path = "/" + str(parent.text(0)) + path parent = parent.parent() self._widget.ud_path_input.setCurrentIndex( self._widget.ud_path_input.findText(path)) def _handle_help(self): """Event: Help button pressed""" helpMsg = QMessageBox() helpMsg.setWindowTitle("Keyboard Controls") helpMsg.setIcon(QMessageBox.Information) helpMsg.setText( "Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R" ) ret = helpMsg.exec_() def select_cb(self, event): """Event: Click to select a graph node to display user data and update the graph.""" # Only set string status x = event.x() y = event.y() url = self._widget.xdot_widget.get_url(x, y) if url is None: return item = self._widget.xdot_widget.items_by_url.get(url.url, None) if item: self._widget.status_bar.showMessage(item.url) # Left button-up if item.url not in self._containers: if ':' in item.url: container_url = '/'.join(item.url.split(':')[:-1]) else: container_url = '/'.join(item.url.split('/')[:-1]) else: container_url = item.url if event.button() == Qt.LeftButton: self._selected_paths = [item.url] self._widget.ud_path_input.setCurrentIndex( self._widget.ud_path_input.findText(item.url)) self._graph_needs_refresh = True def _handle_show_implicit(self): """Event: Show Implicit button checked""" self._show_all_transitions = not self._show_all_transitions self._graph_needs_refresh = True self._structure_changed = True if self._widget.zoom_checkbox.isChecked(): self._needs_zoom = True def _set_depth(self): """Event: Depth value changed""" self._max_depth = self._widget.depth_input.value() self._graph_needs_refresh = True self._structure_changed = True if self._widget.zoom_checkbox.isChecked(): self._needs_zoom = True def _set_width(self): """Event: Label Width value changed""" self._label_wrapper.width = self._widget.label_width_input.value() self._graph_needs_refresh = True def _handle_ud_set_path(self): """Event: Set as Initial State button pressed""" state_path = self._widget.ud_path_input.currentText() parent_path = get_parent_path(state_path) if len(parent_path) > 0: state = get_label(state_path) server_name = self._containers[parent_path]._server_name self._client.set_initial_state(server_name, parent_path, [state], timeout=rospy.Duration(60.0)) self._structure_changed = True self._graph_needs_refresh = True if self._widget.zoom_checkbox.isChecked(): self._needs_zoom = True def _handle_ud_path(self): """Event: User Data selection combo box changed""" path = self._widget.ud_path_input.currentText() #Check the path is non-zero length if len(path) > 0: # Get the container corresponding to this path, since userdata is # stored in the containers if path not in self._containers: parent_path = get_parent_path(path) else: parent_path = path if parent_path not in self._containers: parent_path = get_parent_path(parent_path) if parent_path in self._containers: # Get the container container = self._containers[parent_path] # Generate the userdata string ud_str = '' for (k, v) in container._local_data._data.iteritems(): ud_str += str(k) + ": " vstr = str(v) # Add a line break if this is a multiline value if vstr.find('\n') != -1: ud_str += '\n' ud_str += vstr + '\n\n' #Display the user data self._widget.ud_text_browser.setPlainText(ud_str) self._widget.ud_text_browser.show() def _update_server_list(self): """Update the list of known SMACH introspection servers.""" # Discover new servers if self._widget.restrict_ns.isChecked(): server_names = [self._widget.namespace_input.currentText()[0:-1]] #self._status_subs = {} else: server_names = self._client.get_servers() new_server_names = [ sn for sn in server_names if sn not in self._status_subs ] # Create subscribers for newly discovered servers for server_name in new_server_names: # Create a subscriber for the plan structure (topology) published by this server self._structure_subs[server_name] = rospy.Subscriber( server_name + smach_ros.introspection.STRUCTURE_TOPIC, SmachContainerStructure, callback=self._structure_msg_update, callback_args=server_name, queue_size=50) # Create a subscriber for the active states in the plan published by this server self._status_subs[server_name] = rospy.Subscriber( server_name + smach_ros.introspection.STATUS_TOPIC, SmachContainerStatus, callback=self._status_msg_update, queue_size=50) def _structure_msg_update(self, msg, server_name): """Update the structure of the SMACH plan (re-generate the dotcode).""" # Just return if we're shutting down if not self._keep_running: return # Get the node path path = msg.path pathsplit = path.split('/') parent_path = '/'.join(pathsplit[0:-1]) rospy.logdebug("RECEIVED: " + path) rospy.logdebug("CONTAINERS: " + str(self._containers.keys())) # Initialize redraw flag needs_redraw = False # Determine if we need to update one of the proxies or create a new one if path in self._containers: rospy.logdebug("UPDATING: " + path) # Update the structure of this known container needs_redraw = self._containers[path].update_structure(msg) else: rospy.logdebug("CONSTRUCTING: " + path) # Create a new container container = ContainerNode(server_name, msg) self._containers[path] = container #Add items to ud path combo box if self._widget.ud_path_input.findText(path) == -1: self._widget.ud_path_input.addItem(path) for item in container._children: if self._widget.ud_path_input.findText(path + "/" + item) == -1: self._widget.ud_path_input.addItem(path + "/" + item) # Store this as a top container if it has no parent if parent_path == '': self._top_containers[path] = container # Append the path to the appropriate widgets self._append_new_path(path) # We need to redraw the graph if this container's parent is being viewed if path.find(self._widget.path_input.currentText()) == 0: needs_redraw = True if needs_redraw: self._structure_changed = True if self._widget.zoom_checkbox.isChecked(): self._needs_zoom = True self._tree_needs_refresh = True self._graph_needs_refresh = True def _status_msg_update(self, msg): """Process status messages.""" # Check if we're in the process of shutting down if not self._keep_running: return # Get the path to the updating conainer path = msg.path rospy.logdebug("STATUS MSG: " + path) # Check if this is a known container if path in self._containers: # Get the container and check if the status update requires regeneration container = self._containers[path] if container.update_status(msg): self._graph_needs_refresh = True self._tree_needs_refresh = True def _append_new_path(self, path): """Append a new path to the path selection widgets""" if ((not self._widget.restrict_ns.isChecked()) or ((self._widget.restrict_ns.isChecked()) and (self._widget.namespace_input.currentText() in path)) or (path == self._widget.namespace_input.currentText()[0:-1])): self._widget.path_input.addItem(path) def _update_graph(self): """This thread continuously updates the graph when it changes. The graph gets updated in one of two ways: 1: The structure of the SMACH plans has changed, or the display settings have been changed. In this case, the dotcode needs to be regenerated. 2: The status of the SMACH plans has changed. In this case, we only need to change the styles of the graph. """ if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown( ): # Get the containers to update containers_to_update = {} # Check if the path that's currently being viewed is in the # list of existing containers if self._path in self._containers: # Some non-root path containers_to_update = { self._path: self._containers[self._path] } elif self._path == '/': # Root path containers_to_update = self._top_containers # Check if we need to re-generate the dotcode (if the structure changed) # TODO: needs_zoom is a misnomer if self._structure_changed or self._needs_zoom: dotstr = "digraph {\n\t" dotstr += ';'.join([ "compound=true", "outputmode=nodesfirst", "labeljust=l", "nodesep=0.5", "minlen=2", "mclimit=5", "clusterrank=local", "ranksep=0.75", # "remincross=true", # "rank=sink", "ordering=\"\"", ]) dotstr += ";\n" # Generate the rest of the graph # TODO: Only re-generate dotcode for containers that have changed for path, container in containers_to_update.items(): dotstr += container.get_dotcode(self._selected_paths, [], 0, self._max_depth, self._containers, self._show_all_transitions, self._label_wrapper) # The given path isn't available if len(containers_to_update) == 0: dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]' dotstr += '\n}\n' # Set the dotcode to the new dotcode, reset the flags self.set_dotcode(dotstr, zoom=False) self._structure_changed = False self._graph_needs_refresh = False # Update the styles for the graph if there are any updates for path, container in containers_to_update.items(): container.set_styles(self._selected_paths, 0, self._max_depth, self._widget.xdot_widget.items_by_url, self._widget.xdot_widget.subgraph_shapes, self._containers) self._widget.xdot_widget.repaint() def set_dotcode(self, dotcode, zoom=True): """Set the xdot view's dotcode and refresh the display.""" # Set the new dotcode if self._widget.xdot_widget.set_dotcode(dotcode, False): # Re-zoom if necessary if zoom or self._needs_zoom: self._widget.xdot_widget.zoom_to_fit() self._needs_zoom = False def _update_tree(self): """Update the tree view.""" if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown( ): self._tree_nodes = {} self._widget.tree.clear() for path, tc in self._top_containers.iteritems(): if ((not self._widget.restrict_ns.isChecked()) or ((self._widget.restrict_ns.isChecked()) and (self._widget.namespace_input.currentText() in path)) or (path == self._widget.namespace_input.currentText()[0:-1])): self.add_to_tree(path, None) self._tree_needs_refresh = False self._widget.tree.sortItems(0, 0) def add_to_tree(self, path, parent): """Add a path to the tree view.""" if parent is None: container = QTreeWidgetItem() container.setText(0, self._containers[path]._label) self._widget.tree.addTopLevelItem(container) else: container = QTreeWidgetItem(parent) container.setText(0, self._containers[path]._label) # Add children to_tree for label in self._containers[path]._children: child_path = '/'.join([path, label]) if child_path in self._containers.keys(): self.add_to_tree(child_path, container) else: child = QTreeWidgetItem(container) child.setText(0, label) def append_tree(self, container, parent=None): """Append an item to the tree view.""" if not parent: node = QTreeWidgetItem() node.setText(0, container._label) self._widget.tree.addTopLevelItem(node) for child_label in container._children: child = QTreeWidgetItem(node) child.setText(0, child_label) def _update_thread_run(self): """Update the list of namespaces.""" _, _, topic_types = rospy.get_master().getTopicTypes() self._topic_dict = dict(topic_types) keys = list(self._topic_dict.keys()) namespaces = list() for i in keys: print i if i.endswith("smach/container_status"): namespaces.append(i[0:i.index("smach/container_status")]) self._widget.namespace_input.setItems.emit(namespaces) @Slot() def _update_finished(self): """Enable namespace combo box.""" self._widget.namespace_input.setEnabled(True) def _handle_ns_changed(self): """If namespace selection is changed then reinitialize everything.""" ns = self._widget.namespace_input.currentText() if len(ns) > 0: if self._ns != ns: self._actions_connected = False self._ns = ns self.enable_widgets(False) rospy.loginfo("Creating action clients on namespace '%s'..." % ns) rospy.loginfo("Action clients created.") self._actions_connected = True self.enable_widgets(True) self._containers = {} self._top_containers = {} self._selected_paths = [] self._structure_subs = {} self._status_subs = {} self._needs_zoom = True self._structure_changed = True self._max_depth = -1 self._show_all_transitions = False self._graph_needs_refresh = True self._tree_needs_refresh = True #self._widget.namespace_input.clear() self._widget.path_input.clear() self._widget.ud_path_input.clear() self._widget.tree.clear() @Slot() def refresh_combo_box(self): """Refresh namespace combo box.""" self._update_thread.kill() self._containers = {} self._top_containers = {} self._selected_paths = [] self._structure_subs = {} self._status_subs = {} self._structure_changed = True self._tree_needs_refresh = True #self._graph_needs_refresh = True #self._widget.namespace_input.clear() self._widget.path_input.clear() self._widget.ud_path_input.clear() self._widget.tree.clear() if self._widget.restrict_ns.isChecked(): self._widget.namespace_input.setEnabled(False) self._widget.namespace_input.setEditText('updating...') self._widget.ns_refresh_button.setEnabled(True) self._update_thread.start() else: self._widget.namespace_input.setEnabled(False) self._widget.namespace_input.setEditText('Unrestricted') self._widget.ns_refresh_button.setEnabled(False) self._graph_needs_refresh = True self._tree_needs_refresh = True self._widget.path_input.addItem('/') def _handle_path_changed(self, checked): """If the path input is changed, update graph.""" self._path = self._widget.path_input.currentText() self._graph_needs_refresh = True self._structure_changed = True if self._widget.zoom_checkbox.isChecked(): self._needs_zoom = True def enable_widgets(self, enable): """Enable all widgets.""" self._widget.xdot_widget.setEnabled(enable) self._widget.path_input.setEnabled(enable) self._widget.depth_input.setEnabled(enable) self._widget.label_width_input.setEnabled(enable) self._widget.ud_path_input.setEnabled(enable) self._widget.ud_text_browser.setEnabled(enable)
class MyPlugin(Plugin): sig_sysmsg = Signal(str) l_location = pyqtSignal(float, float) r_location = pyqtSignal(float, float) LOS_optimize_chal=pyqtSignal(str,str) NLOS_optimize_chal=pyqtSignal(str) com_timer = QTimer() rssi_timer = QTimer() baro_timer = QTimer() ConnInd_timer = QTimer() _pid = 0000000 # save_file_processing def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') rp = rospkg.RosPack() self._lrssivalue=0.0 self._rrssivalue=0.0 self._lcomvalue=0.0 self._rcomvalue=0.0 self._lbaroaltvalue = 0.0 self._rbaroaltvalue = 0.0 self._enable_gps=0 self._LOS_NLOS=1 self.imu_done=4 self.imu_run_stop_value=1 self.initial_heading=500 self.ros_run_stop_value=1 self.run_initial_scan=1 self.initial_scan_state=1 self.localimu=0 self.remoteimu=0 self.camera_run_save_stop_state=1 self.initial_map=1 self.rssi_test_gps_rssi=1 # 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 ## def goCoords(): ## def resetError(): ## # coordsEdit.setStyleSheet('') ## ## try: ## print("work") ## #latitude, longitude = coordsEdit.text().split(",") ## ## except ValueError: ## #coordsEdit.setStyleSheet("color: red;") ## QTimer.singleShot(500, resetError) ## else: ## map.centerAt(latitude, longitude) ## # map.moveMarker("MyDragableMark", latitude, longitude) def onMarkerMoved(key, latitude, longitude): print("Moved!!", key, latitude, longitude) if key == 'local GPS': l_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude)) else: r_coordsEdit.setText("{0:.8}, {1:.8}".format(latitude, longitude)) #coordsEdit.setText("{}, {}".format(latitude, longitude)) #self.map.moveMarker("local GPS",latitude+0.01, longitude+0.01) def onMarkerRClick(key): print("RClick on ", key) # map.setMarkerOptions(key, draggable=False) def onMarkerLClick(key): print("LClick on ", key) def onMarkerDClick(key): print("DClick on ", key) # map.setMarkerOptions(key, draggable=True) def onMapMoved(latitude, longitude): print("Moved to ", latitude, longitude) def onMapRClick(latitude, longitude): print("RClick on ", latitude, longitude) def onMapLClick(latitude, longitude): print("LClick on ", latitude, longitude) def onMapDClick(latitude, longitude): print("DClick on ", latitude, longitude) def move_l_mark(lat_l,lon_l): #onMarkerMoved("local gps",lat_,lon_) #print("get location",lat_,lon_) self.map.moveMarker("local GPS",lat_l,lon_l) l_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_l,lon_l)) if self.initial_map==1: self.map.centerAt(lat_l,lon_l) self.map.setZoom(15) self.initial_map=0 #time.sleep(0.01) def local_callback(llocation): llat=llocation.data[0] llon=llocation.data[1] #move_mark(self,lat,lon) self.l_location.emit(llat,llon) def move_r_mark(lat_r,lon_r): #onMarkerMoved("local gps",lat_,lon_) #print("get location",lat_,lon_) self.map.moveMarker("remote GPS",lat_r,lon_r) r_coordsEdit.setText("{0:.9}, {1:.9}".format(lat_r,lon_r)) #time.sleep(0.01) def remote_callback(rlocation): rlat=rlocation.data[0] rlon=rlocation.data[1] #move_mark(self,lat,lon) self.r_location.emit(rlat,rlon) @pyqtSlot() def change_los_nos_value(): if self._LOS_NLOS==1: self._LOS_NLOS=0 initial_heading_calculation_button.setText("Run") else: self._LOS_NLOS=1 initial_heading_calculation_button.setText("Scan") print(self._LOS_NLOS) def optimize_channel_fun(): os.system( "sudo /home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/GetRSSI.sh") if self._LOS_NLOS == 1: a = localchangechannel.local_change_channel() print("channel a", a) time.sleep(2) b = remotechangechannel.remote_change_channel() print("channel b", b) self.LOS_optimize_chal.emit(a,b) # channel_textedit.setText("L:%s, R:%s" % (a, b)) else: a = localchangechannel.local_change_channel() self.NLOS_optimize_chal.emit(a) # channel_textedit.setText("L:%s, R:N" % a) @pyqtSlot() def LOS_channel_testedit(vala,valb): channel_textedit.setText("L:%s, R:%s" % (vala, valb)) @pyqtSlot() def NLOS_channel_testedit(vala): channel_textedit.setText("L:%s, R:N" % vala) @pyqtSlot() def channel_on_click(): optimize_channel_process= threading.Thread(target=optimize_channel_fun) optimize_channel_process.start() # optimize_channel_process.start() # imu calibration ******************************************* def run_calibration_fun(): print("calibration") if (self._LOS_NLOS == 1): # print("calibrate two IMUs") os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1") time.sleep(7) os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 1") else: # print("calibrate local IMU") os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 0") time.sleep(7) os.system( "/home/dnc2/Documents/catkin_ws/devel/sshnode/start_IMU_calibration.sh 0") @pyqtSlot() def imu_run_stop_click(): # imu_threading = threading.Thread(target=run_calibration_fun) imu_threading= multiprocessing.Process(target=run_calibration_fun) if self.imu_run_stop_value==1: print("stat threading") IMU_run_stop_button.setText("Stop") IMU_state_state_label.setText("Working") time.sleep(1) self.imu_state_timer.start(60000) imu_threading.start() self.imu_run_stop_value = 0 else: IMU_run_stop_button.setText("Run") self.imu_run_stop_value = 1 IMU_state_state_label.setText("State") os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_IMU_calibration.sh 1") # if imu_threading.isAlive(): # self.imu_threading = threading.Thread(target=run_calibration_fun(selqf, self._LOS_NLOS)) # self.imu_threading.start() # parent_conn, child_conn = multiprocessing.Pipe() # self.imu_processing = multiprocessing.Process(target=run_calibration_fun) # self.imu_processing.start() def stat_ros_nodes(): print("stat ROS node ...") # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_rosbag.sh") # if self._LOS_NLOS==1: if self.initial_heading==500: os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS)) time.sleep(7) os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_no_initial.sh %d " % (self._LOS_NLOS)) else: os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS)) time.sleep(7) os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_ros_with_initial.sh %d %d" %(self._LOS_NLOS,self.initial_heading)) # else: # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_local.sh %d %f" % self._LOS_NLOS,self.initial_heading) @pyqtSlot() def stat_ros_click(): # self.imu_timer.stop(); start_ros_node_threading = threading.Thread(target=stat_ros_nodes) # if not self.monitor_ccq.isRunning(): # self.monitor_ccq.start() # start_save_threading = threading.Thread(target=start_save_data) if self.ros_run_stop_value==1: self.ros_run_stop_value=0 Ros_node_run_button.setText("Stop") Ros_node_state_label.setText("Working") IMU_run_stop_button.setEnabled(False) self.distance_timer.start(10000) IMU_run_stop_button.setStyleSheet( "background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left") start_ros_node_threading.start() else: self.ros_run_stop_value=1 # self.monitor_ccq.terminate() Ros_node_run_button.setText("Run") Ros_node_state_label.setText("Done") IMU_run_stop_button.setEnabled(True) self.distance_timer.stop() IMU_run_stop_button.setStyleSheet( "background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left") os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_ROS_nodes.sh %d" % (self._LOS_NLOS)) # if start_ros_node_threading.isAlive(): # start_save_threading.start() def run_camera_nodes(): if self.camera_run_save_stop_state==1: if infrared_camera.isChecked(): camera_comman=10 elif not infrared_camera.isChecked(): camera_comman=0 if optical_camera.isChecked(): opt_camera_comman=10 elif not optical_camera.isChecked(): opt_camera_comman=0 camera_run_button.setText("Confirm") self.camera_run_save_stop_state = 2 os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh %d %d" %(camera_comman,opt_camera_comman)) elif self.camera_run_save_stop_state==2: if save_infrared.isChecked(): save_inf_var=1 else: save_inf_var=0 if save_optical.isChecked(): save_opt_var=1 else: save_opt_var=0 camera_run_button.setText("Stop") self.camera_run_save_stop_state = 3 os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/save_camera_nodes.sh %d %d" % (save_inf_var, save_opt_var)) elif self.camera_run_save_stop_state==3: camera_run_button.setText("Confirm") self.camera_run_save_stop_state = 1 os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_camera_nodes.sh") # if optical_camera.isChecked() and infrared_camera.isChecked(): # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 1") # elif optical_camera.isChecked() and infrared_camera.isChecked()==0: # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 1 0") # elif optical_camera.isChecked()==0 and infrared_camera.isChecked()==1: # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_camera_nodes.sh 0 1") @pyqtSlot() def start_camera_click(): start_camera_threading=threading.Thread(target=run_camera_nodes) if optical_camera.isChecked() or infrared_camera.isChecked(): start_camera_threading.start() def initial_scan_run(): os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/start_initial_scan.sh") @pyqtSlot() def calculate_Azimuth(): if self._LOS_NLOS==1 and self.run_initial_scan==1: initial_heading_calculation_button.setText("Stop") self.run_initial_scan = 0 IMU_run_stop_button.setEnabled(False) IMU_run_stop_button.setStyleSheet("background-color: rgb(192,197,197);padding: 6px; border-radius:5px;text_align:left") initial_scan_threading =threading.Thread(target=initial_scan_run) initial_scan_threading.start() elif self._LOS_NLOS ==1 and self.run_initial_scan==0: initial_heading_calculation_button.setText("Run") self.run_initial_scan = 1 IMU_run_stop_button.setEnabled(True) IMU_run_stop_button.setStyleSheet( "background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left") # os.system("/home/dnc2/Documents/catkin_ws/devel/sshnode/stop_initial_scan.sh") if self._LOS_NLOS==0: if l_coordsEdit.text() and l_coordsEdit.text().find(",") !=-1: llocation_string = l_coordsEdit.text() print(llocation_string) l_lat_lon = llocation_string.split(",") try: l_latitiude = float(l_lat_lon[0]) l_longitude = float(l_lat_lon[1]) except: Azimuth_warning() return if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1: rlocation_string = r_coordsEdit.text() print(rlocation_string) r_lat_lon = rlocation_string.split(",") try: r_latitiude = float(r_lat_lon[0]) r_longitude = float(r_lat_lon[1]) temp_remote_gps_msg.data= [r_latitiude, r_longitude] temp_remote_gps.publish(temp_remote_gps_msg) # print("temp_gps_test1") # temp_remote_gps.publish(temp_remote_gps_msg) # print("temp_gps_test") except: Azimuth_warning() return try: LON1 = l_longitude*0.0174532925199433 LAT1 = l_latitiude*0.0174532925199433 LON2 = r_longitude*0.0174532925199433 LAT2 = r_latitiude*0.0174532925199433 az = math.atan2(math.sin(LON2-LON1) * math.cos(LAT2),math.cos(LAT1) * math.sin(LAT2) - math.sin(LAT1) * math.cos(LAT2) * math.cos(LON2-LON1)) az = math.fmod(az, 9.118906528) az = math.fmod(az, 6.283185307179586)*(57.2957795131) if az<0 : az = az+360 Azimuth =az except: Azimuth_warning() return initial_heading_display_label.setText(" %0.1f"%Azimuth) self.initial_heading=int(Azimuth) @pyqtSlot() def calculate_distance(): if l_coordsEdit.text() and l_coordsEdit.text().find(",") != -1: llocation_string = l_coordsEdit.text() l_lat_lon = llocation_string.split(",") try: l_latitiude = float(l_lat_lon[0]) l_longitude = float(l_lat_lon[1]) except: # Azimuth_warning() # print("error1") return if r_coordsEdit.text() and r_coordsEdit.text().find(",") != -1: rlocation_string = r_coordsEdit.text() r_lat_lon = rlocation_string.split(",") try: r_latitiude = float(r_lat_lon[0]) r_longitude = float(r_lat_lon[1]) except: # Azimuth_warning() # print("error2") return try: LON1 = l_longitude * 0.0174532925199433 LAT1 = l_latitiude * 0.0174532925199433 LON2 = r_longitude * 0.0174532925199433 LAT2 = r_latitiude * 0.0174532925199433 A =math.sin((LAT2-LAT1)/2)*math.sin((LAT2-LAT1)/2)+math.cos(LAT1)*math.cos(LAT2)*math.sin((LON2-LON1)/2)*math.sin((LON2-LON1)/2) # print('A',A) AB= 2*math.atan2(math.sqrt(A),math.sqrt(1-A)) # print('AB',AB) AB_DIS=6371000*AB # print('AB_DIS',AB_DIS) # distance_text.setText("%0.1f m"%AB_DIS) self.distance_label.setText("%0.1f m"%AB_DIS) # distance_lable.setText("0.1f m"%AB_DIS) except: # print("error3") # Azimuth_warning() return def Azimuth_warning(): msg = QMessageBox() msg.setIcon(QMessageBox.Warning) msg.setWindowTitle("Azimuth calculation") msg.setText("The locations of local and remote UAVs cannot be empty.\nPlease input locations(latitude,longitude) correctly!") msg.exec_() # @pyqtSlot() # def pub_new_Azimuth(): # try: # N_Azimuth = float(Azimuth_data.text()) # except: # Azimuth_warning() # return # if math.fabs(N_Azimuth)>= 360: # Azimuth_warning() # return # pub_Azimuth.publish(N_Azimuth) @pyqtSlot() def change_baroalt(): # self._rrssivalue=str(rssi2) r_baro_text.setText("%.1f" % self._rbaroaltvalue) l_baro_text.setText("%.1f" % self._lbaroaltvalue) time.sleep(0.01) # time.sleep(0.01) def l_baro_callback(lbaroinfo): self._lbaroaltvalue = lbaroinfo.data[2] def r_baro_callback(rbaroinfo): self._rbaroaltvalue = rbaroinfo.data[2] @pyqtSlot() def change_rssi(): #self._rrssivalue=str(rssi2) r_rssi_text.setText(str(self._rrssivalue)) l_rssi_text.setText(str(self._lrssivalue)) time.sleep(0.01) #time.sleep(0.01) def l_rssi_callback(rssil): self._lrssivalue=rssil.data # self.l_rssi.emit(rssil.data) # del rssil #l_rssi_text.update() #l_rssi_text.setText(str(rssi.data)) def r_rssi_callback(rssir): self._rrssivalue=rssir.data # self.r_rssi.emit(rssir.data) # self.memory_tracker.print_diff() # print(sys.getsizeof(move_l_compass(headidng1))) #r_rssi_text.setText(str(rssi.data)) @pyqtSlot() def move_compass(): l_com_text.setText("%.1f" % self._lcomvalue) time.sleep(0.01) # print('work') l_com_widget.setAngle(self._lcomvalue) time.sleep(0.01) r_com_text.setText("%.1f" % self._rcomvalue) time.sleep(0.01) r_com_widget.setAngle(self._rcomvalue) time.sleep(0.01) # def map_rssi_fun_click(): # if self.rssi_test_gps_rssi==1: # for i in range(5): # pub_gps_Azimuth_enable.publish(0) # time.sleep(0.2) # self.rssi_test_gps_rssi=0 # else: # for i in range(5): # pub_gps_Azimuth_enable.publish(1) # time.sleep(0.2) # self.rssi_test_gps_rssi = 1 def l_com_callback(heading): self._lcomvalue=heading.data # print('work1') #l_com_text.setText("%.1f" % heading.data) # self.l_com.emit(heading.data) # heading.data =None #self.update() #l_com_widget.setAngle(heading.data) def r_com_callback(heading): self._rcomvalue=heading.data #r_com_text.setText("%.1f" % heading.data) # self.r_com.emit(heading.data) # del heading #r_com_widget.setAngle(heading.data) def local_imucal_callback(l_imu_msg): self.localimu=l_imu_msg.data # if self.localimu == 1 and self.remoteimu == 1: # imu_lock.acquire() # try: # IMU_state_state_label.setText("Done") # finally: # imu_lock.release() def remote_imucal_callback(r_imu_msg): self.remoteimu=r_imu_msg.data # if self.localimu == 1 and self.remoteimu == 1: # imu_lock.acquire() # try: # IMU_state_state_label.setText("Done") # finally: # imu_lock.release() @pyqtSlot() def imu_state_display(): if self._LOS_NLOS==1: if self.remoteimu==1 and self.localimu==1: IMU_state_state_label.setText("Done") self.imu_state_timer.stop() else: if self.localimu == 1: IMU_state_state_label.setText("Done") self.imu_state_timer.stop() @pyqtSlot() def local_bbb_conn(var): if var ==1: lbbb_conn_icon.setPixmap(green_icon) else: lbbb_conn_icon.setPixmap(red_icon) @pyqtSlot() def remote_bbb_conn(var): if var==1: rbbb_conn_icon.setPixmap(green_icon) else: rbbb_conn_icon.setPixmap(red_icon) @pyqtSlot() def local_connection_indicator(): # var: RSSI l_var = self._lrssivalue r_var = self._rrssivalue #r_var = -55 #l_var = -75 if l_var>=-60 and l_var<0: l_con_ind_icon.setPixmap(ConnectionBar5) elif l_var<-60 and l_var>=-70: l_con_ind_icon.setPixmap(ConnectionBar4) elif l_var <-70 and l_var>= -80: l_con_ind_icon.setPixmap(ConnectionBar3) elif l_var <-80 and l_var > -96: l_con_ind_icon.setPixmap(ConnectionBar2) else: l_con_ind_icon.setPixmap(ConnectionBar1) if r_var >= -60 and l_var<0: r_con_ind_icon.setPixmap(ConnectionBar5) elif r_var < -60 and r_var >= -70: r_con_ind_icon.setPixmap(ConnectionBar4) elif r_var < -70 and r_var >= -80: r_con_ind_icon.setPixmap(ConnectionBar3) elif r_var < -80 and r_var > -96: r_con_ind_icon.setPixmap(ConnectionBar2) else: r_con_ind_icon.setPixmap(ConnectionBar1) # def local_connection_indicator(): # var: RSSI # #var = self._rrssivalue # var = -55 # if var>=-60: # l_con_ind_icon.setPixmap(ConnectionBar5) # elif var<-60 and var>=-70: # l_con_ind_icon.setPixmap(ConnectionBar4) # elif var <-70 and var>= -80: # l_con_ind_icon.setPixmap(ConnectionBar3) # elif var <-80 and var > -96: # l_con_ind_icon.setPixmap(ConnectionBar2) # else: # l_con_ind_icon.setPixmap(ConnectionBar1) self._widget = QWidget() gcv=QVBoxLayout(self._widget) v = QVBoxLayout() h = QHBoxLayout() FUN_FONT= QFont() FUN_FONT.setBold(True) FUN_FONT.setPointSize(14) FUN_FONT.setWeight(87) pub_gps_Azimuth_enable = rospy.Publisher('lenablegpsazimuth', Int32, queue_size=10) temp_remote_gps = rospy.Publisher('temp_gps_location', Float64MultiArray,queue_size=1) temp_remote_gps_msg= Float64MultiArray() #ros_threading ############################# # star_ros_threading = threading.Thread(target= stat_ros_nodes) # # save_file_processing = multiprocessing.Process(target= save_topic_fun) # save_file_processing = threading.Thread(target= save_topic_fun) # star_ros_manual_com_threading = threading.Thread(target= stat_ros_manual_com_nodes) # imu_threading = threading.Thread(target=run_calibration_fun) green_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/greenenergy.png")) green_icon = green_icon.scaled(40, 40, Qt.KeepAspectRatio) red_icon = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/redenergy.png")) red_icon = red_icon.scaled(40, 40, Qt.KeepAspectRatio) ConnectionBar1 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex1.png")) ConnectionBar2 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex2.png")) ConnectionBar3 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex3.png")) ConnectionBar4 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex4.png")) ConnectionBar5 = QPixmap(("/home/dnc2/Documents/catkin_ws/src/directional_antenna_system/src/directional_antenna_system/ConnectionIndex5.png")) ConnectionBar1 = ConnectionBar1.scaled(50, 110, Qt.KeepAspectRatio) ConnectionBar2 = ConnectionBar2.scaled(50, 110, Qt.KeepAspectRatio) ConnectionBar3 = ConnectionBar3.scaled(50, 110, Qt.KeepAspectRatio) ConnectionBar4 = ConnectionBar4.scaled(50, 110, Qt.KeepAspectRatio) ConnectionBar5 = ConnectionBar5.scaled(50, 110, Qt.KeepAspectRatio) ################################## # local widget local_widget = QWidget() local_Vlayout = QVBoxLayout(local_widget) local_widget.setStyleSheet("background-color: white") l_coordsEdit = QLineEdit() l_coordsEdit.setFont(FUN_FONT) l_coordsEdit.setMinimumWidth(230) lgps_label = QLabel('GPS:') lgps_label.setFont(FUN_FONT) l_com_widget = CompassWidget() l_rssi_lable = QLabel('RSSI:') l_rssi_lable.setFont(FUN_FONT) l_rssi_text = QLineEdit() l_rssi_text.setFont(FUN_FONT) l_rssi_text.setMaximumWidth(70) l_baro_lable = QLabel('Alt:') l_baro_lable.setFont(FUN_FONT) l_baro_text = QLineEdit() l_baro_text.setFont(FUN_FONT) l_baro_text.setMaximumWidth(80) l_com_label = QLabel('Antenna Heading:') l_com_label.setFont(FUN_FONT) l_com_text = QLineEdit() l_com_text.setFont(FUN_FONT) # l_com_text.setText(self._lcomvalue) l_com_text.setMaximumWidth(80) l_h1 = QHBoxLayout() l_h1.addStretch() l_h1.addWidget(l_rssi_lable) l_h1.addWidget(l_rssi_text) l_h1.addWidget(l_com_label) l_h1.addWidget(l_com_text) l_h1.addStretch() l_h2 = QHBoxLayout() l_h2.addStretch() l_h2.addWidget(lgps_label) l_h2.addWidget(l_coordsEdit) l_h2.addWidget(l_baro_lable) l_h2.addWidget(l_baro_text) l_h2.addStretch() # l_h1.setSpacing(0) local_Vlayout.addWidget(l_com_widget) local_Vlayout.addLayout(l_h1) local_Vlayout.addLayout(l_h2) local_widget.setMinimumSize(450,300) local_widget.setMaximumWidth(500) lbbb_conn_icon=QLabel(l_com_widget) lbbb_conn_icon.setText("ICON") lbbb_conn_icon.setGeometry(380,200,40,40) lbbb_conn_icon.setPixmap(red_icon) l_con_ind_icon = QLabel(l_com_widget) l_con_ind_icon.setText("ICON") l_con_ind_icon.setGeometry(380,40,50,110) l_con_ind_icon.setPixmap(ConnectionBar1) ################################## # remote widget remote_widget = QWidget() remote_vlayout = QVBoxLayout(remote_widget) remote_widget.setStyleSheet("background-color: white") r_coordsEdit = QLineEdit() r_coordsEdit.setFont(FUN_FONT) r_coordsEdit.setMinimumWidth(230) rgps_label = QLabel('GPS:') rgps_label.setFont(FUN_FONT) r_com_widget = RCompassWidget() r_rssi_lable=QLabel('RSSI') r_rssi_lable.setFont(FUN_FONT) r_rssi_text=QLineEdit() r_rssi_text.setFont(FUN_FONT) r_rssi_text.setMaximumWidth(70) r_baro_lable = QLabel('Alt:') r_baro_lable.setFont(FUN_FONT) r_baro_text = QLineEdit() r_baro_text.setFont(FUN_FONT) r_baro_text.setMaximumWidth(80) r_com_label=QLabel('Antenna Heading:') r_com_label.setFont(FUN_FONT) r_com_text=QLineEdit() r_com_text.setFont(FUN_FONT) r_com_text.setMaximumWidth(80) r_h1 = QHBoxLayout() r_h1.addStretch() r_h1.addWidget(r_rssi_lable) r_h1.addWidget(r_rssi_text) r_h1.addWidget(r_com_label) r_h1.addWidget(r_com_text) r_h1.addStretch() r_h2 =QHBoxLayout() r_h2.addStretch() r_h2.addWidget(rgps_label) r_h2.addWidget(r_coordsEdit) r_h2.addWidget(r_baro_lable) r_h2.addWidget(r_baro_text) r_h2.addStretch() remote_vlayout.addWidget(r_com_widget) remote_vlayout.addLayout(r_h1) remote_vlayout.addLayout(r_h2) remote_widget.setMinimumSize(450,300) # remote_widget.setMinimumSize(local_widget.width()) rbbb_conn_icon = QLabel(r_com_widget) rbbb_conn_icon.setText("ICON") rbbb_conn_icon.setGeometry(380, 200, 40, 40) rbbb_conn_icon.setPixmap(red_icon) r_con_ind_icon = QLabel(r_com_widget) r_con_ind_icon.setText("ICON") r_con_ind_icon.setGeometry(380, 40, 50, 110) r_con_ind_icon.setPixmap(ConnectionBar1) ################################## #distance and CCQ distance_lable=QLabel("Distance") distance_text=QLabel("") CCQ_lable=QLabel("Transmit CCQ: ") CCQ_text=QLabel() DIS_CCQ_widget= QWidget() DIS_CCQ_layout=QHBoxLayout(DIS_CCQ_widget) DIS_CCQ_layout.addStretch(1) DIS_CCQ_layout.addWidget(distance_lable) DIS_CCQ_layout.addWidget(distance_text) DIS_CCQ_layout.addStretch(2) DIS_CCQ_layout.addWidget(CCQ_lable) DIS_CCQ_layout.addWidget(CCQ_text) DIS_CCQ_layout.addStretch(1) DIS_CCQ_widget.setStyleSheet("background-color: white") # functional buttons func_widget = QWidget() Func_widget_layout= QGridLayout(func_widget) # Func_widget_layout.setRowStretch(0,1) Func_widget_layout.setHorizontalSpacing(5) # Func_widget_layout.setSpacing(1) distan_option_label= QLabel("Distance Option: ") LOS = QRadioButton("LOS") LOS.setChecked(True) NLOS = QRadioButton("NLOS") NLOS.setChecked(False) channel_selection_label= QLabel("Channel Selection: ") channel_textedit= QLineEdit("CH") channel_optimize= QPushButton("Optimize") self.LOS_optimize_chal.connect(LOS_channel_testedit) self.NLOS_optimize_chal.connect(NLOS_channel_testedit) IMU_calibration_label= QLabel("IMU Calibration: ") IMU_run_stop_button = QPushButton("Run") # IMU_run_stop_button.seta IMU_state_state_label= QLineEdit("state") initial_heading_label=QLabel("Initial Heading: ") initial_heading_display_label= QLineEdit("Default") initial_heading_calculation_button = QPushButton("Scan") ROS_node_label= QLabel("ROS Nodes: ") Ros_node_run_button= QPushButton("Run") Ros_node_state_label= QLineEdit("state") camera_label =QLabel("Cameras in Use: ") camera_group_widget=QWidget() camera_group=QGridLayout(camera_group_widget) # camera_group.setAlignment(Qt.AlignLeft) optical_camera=QCheckBox("Optical") optical_camera.setChecked(True) infrared_camera=QCheckBox("Infrared") infrared_camera.setChecked(True) save_infrared=QCheckBox("Save") save_optical=QCheckBox("Save") camera_group.addWidget(optical_camera,0,0) camera_group.addWidget(save_optical,0,1) camera_group.addWidget(infrared_camera,1,0) camera_group.addWidget(save_infrared,1,1) camera_run_button= QPushButton("Confirm") #map_rssi_button=QPushButton("RSSI_MAPING") Func_widget_layout.addWidget(distan_option_label,0,0) Func_widget_layout.addWidget(LOS,0,1) Func_widget_layout.addWidget(NLOS,0,2) Func_widget_layout.addWidget(channel_selection_label,1,0) Func_widget_layout.addWidget(channel_textedit,1,2) Func_widget_layout.addWidget(channel_optimize,1,1) Func_widget_layout.addWidget(IMU_calibration_label,2,0) Func_widget_layout.addWidget(IMU_run_stop_button,2,1) Func_widget_layout.addWidget(IMU_state_state_label,2,2) Func_widget_layout.addWidget(initial_heading_label,3,0) Func_widget_layout.addWidget(initial_heading_display_label,3,2) Func_widget_layout.addWidget(initial_heading_calculation_button,3,1) Func_widget_layout.addWidget(ROS_node_label,4,0) Func_widget_layout.addWidget(Ros_node_run_button,4,1) Func_widget_layout.addWidget(Ros_node_state_label,4,2) Func_widget_layout.addWidget(camera_label, 5,0) Func_widget_layout.addWidget(camera_group_widget,5,1) Func_widget_layout.addWidget(camera_run_button,5,2) #Func_widget_layout.addWidget(map_rssi_button, 6,0) Func_widget_layout.setColumnMinimumWidth(2,70) # Func_widget_layout.set func_widget.setStyleSheet("color:white;font:bold 15px") # font1 = QFont() # font1.setPointSize(20) # # font1.setPixelSize(30) # # font1.setBold(True) # font1.setWeight(75) LOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") channel_optimize.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") # channel_textedit.setStyleSheet("background-color: rgb(245,128,38)") NLOS.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") IMU_run_stop_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left") # IMU_state_state_label.setStyleSheet("background-color: rgb(245,128,38)") initial_heading_calculation_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") Ros_node_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px;text_align:left") camera_run_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") #map_rssi_button.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") # camera_group.setStyleSheet("font:bold 11px") ############################### LOS.toggled.connect(change_los_nos_value) channel_optimize.clicked.connect(channel_on_click) IMU_run_stop_button.clicked.connect(imu_run_stop_click) initial_heading_calculation_button.clicked.connect(calculate_Azimuth) Ros_node_run_button.clicked.connect(stat_ros_click) camera_run_button.clicked.connect(start_camera_click) #map_rssi_button.clicked.connect(map_rssi_fun_click) # channel_button.clicked.connect(channel_on_click) # ROS_button.clicked.connect(stat_ros_click) # save_button.clicked.connect(save_file_click) # Azimuth_button.clicked.connect(calculate_Azimuth) # New_Azimuth_button.clicked.connect(pub_new_Azimuth) # Enable_gps_Azimuth.clicked.connect(enable_disable_gps_Azimuth) # manual_com.clicked.connect(stat_ros_manual_com_click) mid_layout=QHBoxLayout() #mid_layout.setSpacing(20) mid_layout.addWidget(local_widget) mid_layout.addWidget(remote_widget) top_layout=QVBoxLayout() # top_layout.addWidget(DIS_CCQ_widget) top_layout.addLayout(mid_layout) # top_layout.setStretchFactor(DIS_CCQ_layout,1) # top_layout.setStretchFactor(mid_layout,9) com_fun_layout= QHBoxLayout() com_fun_layout.addLayout(top_layout) com_fun_layout.addWidget(func_widget) com_fun_layout.setStretchFactor(mid_layout,5) com_fun_layout.setStretchFactor(func_widget,1.5) # mid_layout.addWidget(func_widget) # mid_layout.setStretchFactor(local_widget,4) # mid_layout.setStretchFactor(remote_vlayout,4) # mid_layout.setStretchFactor(func_widget,2) # mid_layout.addStretch(1) # gcv.addLayout(mid_layout) gcv.addLayout(com_fun_layout) # l.addRow('Coords:', coordsEdit) # l.addRow('lcoords:',r_coordsEdit) #h.addLayout(v) #l = QFormLayout() #h.addLayout(l) #coordsEdit = QLineEdit() #l.addRow('Coords:', coordsEdit) #coordsEdit.editingFinished.connect(goCoords) self.map = QOSM(self._widget) self.map.mapMoved.connect(onMapMoved) self.map.markerMoved.connect(onMarkerMoved) self.map.mapClicked.connect(onMapLClick) self.map.mapDoubleClicked.connect(onMapDClick) self.map.mapRightClicked.connect(onMapRClick) self.map.markerClicked.connect(onMarkerLClick) self.map.markerDoubleClicked.connect(onMarkerDClick) self.map.markerRightClicked.connect(onMarkerRClick) self.l_location.connect(move_l_mark) self.r_location.connect(move_r_mark) self.distance_widget=QWidget(self.map) self.distance_widget_layout=QHBoxLayout(self.distance_widget) self.distance_widget_layout.addWidget(QLabel("Distance: ")) self.distance_label = QLabel() self.distance_label.setText("the distance") self.distance_widget_layout.addWidget(self.distance_label) self.distance_widget.setGeometry(50,10,180,30) self.distance_widget.setStyleSheet("background-color: white") # self.distance_label.setGeometry(50,10,120,30) # self.distance_label.setStyleSheet("background-color: rgb(245,128,38);padding: 6px; border-radius:5px") # self.distance_label.setGeometry() # self.distance_dock= QDockWidget() # self.distance_dock.setWidget(self.distance_label) # self.distance_dock.setFloating(True) # self.addDockWidget(Qt.LeftDockWidgetArea,self.distance_dock) # self._widget.addDockWidget(Qt.LeftDockWidgetArea, self.distance_dock) gcv.addWidget(self.map) gcv.setStretchFactor(com_fun_layout, 5) gcv.setStretchFactor(self.map, 4) self.map.setSizePolicy( QSizePolicy.MinimumExpanding, QSizePolicy.MinimumExpanding) # l_com_timer = QTimer() # self.l_com_timer.setSingleShot(True) self.com_timer.timeout.connect(move_compass) self.com_timer.start(200) self.rssi_timer.timeout.connect(change_rssi) self.rssi_timer.start(1000) self.baro_timer.timeout.connect(change_baroalt) self.baro_timer.start(1000) self.ConnInd_timer.timeout.connect(local_connection_indicator) self.ConnInd_timer.start(1000) self.imu_state_timer=QTimer() self.imu_state_timer.setInterval(2000) # self.imu_state_timer.interval(2000) self.imu_state_timer.timeout.connect(imu_state_display) self.distance_timer = QTimer() self.distance_timer.setInterval(1000) self.distance_timer.timeout.connect(calculate_distance) self.distance_timer.start() # self.monitor_ccq = CCQ_threading() self.CONN_state=conn_Test() self.CONN_state.lBBB_conn.connect(local_bbb_conn) self.CONN_state.rBBB_conn.connect(remote_bbb_conn) self.CONN_state.start() # self.memory_tracker=tracker.SummaryTracker(o) # 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(rp.get_path('gps_com_node_v2'), 'resource', 'MyPlugin.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) self._widget.setStyleSheet("background-color:rgb(0,68,124)") # Give QObjects reasonable names self._widget.setObjectName('MyPluginUi') # 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.map.waitUntilReady() self.map.centerAt(32.731263, -97.114334) #self.map.centerAt(38.833005, -77.117415) self.map.setZoom(12) # Many icons at: https://sites.google.com/site/gmapsdevelopment/ coords = self.map.center() self.map.addMarker("local GPS", *coords, **dict( icon="file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/local_uav.png", draggable=True, title="locat GPS marker!" )) coords = coords[0] , coords[1] self.map.addMarker("remote GPS", *coords, **dict( # icon="https://thumb.ibb.co/bvE9FT/remote_uav.png", icon= "file:///home/dnc2/Documents/catkin_ws/src/gps_com_node _v2/src/gps_com_node_v2/remote_uav.png", draggable=True, title="remote GPS marker" )) sub1 = rospy.Subscriber("/local_gps", Float64MultiArray, local_callback) sub2 = rospy.Subscriber("/remote_gps", Float64MultiArray, remote_callback) sub3=rospy.Subscriber("/local_rssi",Int32,l_rssi_callback) sub4=rospy.Subscriber("/remote_rssi",Int32,r_rssi_callback) sub5=rospy.Subscriber("/local_com",Float64,l_com_callback) sub6=rospy.Subscriber("/remote_com",Float64,r_com_callback) sub7=rospy.Subscriber("/local_imucal_msg", Int32, local_imucal_callback) sub8=rospy.Subscriber("/remote_imucal_msg", Int32, remote_imucal_callback) sub9=rospy.Subscriber("/local_baro",Float64MultiArray,l_baro_callback) sub10=rospy.Subscriber("/remote_baro",Float64MultiArray,r_baro_callback) #time.sleep(10) def shutdown_plugin(self): # TODO unregister all publishers here # self.save_file_processing.terminate() # os.system("kill ") 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 JointTrajectoryController(Plugin): """ Graphical frontend for a C{JointTrajectoryController}. There are two modes for interacting with a controller: 1. B{Monitor mode} Joint displays are updated with the state reported by the controller. This is a read-only mode and does I{not} send control commands. Every time a new controller is selected, it starts in monitor mode for safety reasons. 2. B{Control mode} Joint displays update the control command that is sent to the controller. Commands are sent periodically evan if the displays are not being updated by the user. To control the aggressiveness of the motions, the maximum speed of the sent commands can be scaled down using the C{Speed scaling control} This plugin is able to detect and keep track of all active controller managers, as well as the JointTrajectoryControllers that are I{running} in each controller manager. For a controller to be compatible with this plugin, it must comply with the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - The controller exposes the C{command} and C{state} topics in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. A reference implementation of the C{JointTrajectoryController} is available in the C{joint_trajectory_controller} ROS package. """ _cmd_pub_freq = 10.0 # Hz _widget_update_freq = 30.0 # Hz _ctrlrs_update_freq = 1 # Hz _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration jointStateChanged = Signal([dict]) def __init__(self, context): super(JointTrajectoryController, self).__init__(context) self.setObjectName('JointTrajectoryController') self._node = context.node # Get the robot_description via a topic qos_profile = QoSProfile(depth=1, history=HistoryPolicy.KEEP_LAST, durability=DurabilityPolicy.TRANSIENT_LOCAL) self._robot_description_sub = self._node.create_subscription( String, 'robot_description', self._robot_description_cb, qos_profile) self._robot_description = None self._publisher = None self._widget = QWidget() _, package_path = get_resource('packages', 'rqt_joint_trajectory_controller') ui_file = os.path.join(package_path, 'share', 'rqt_joint_trajectory_controller', 'resource', 'joint_trajectory_controller.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('JointTrajectoryControllerUi') if context.serial_number() > 1: self._widget.setWindowTitle( self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) ns = self._node.get_namespace() self._widget.controller_group.setTitle('ns: ' + ns) # Setup speed scaler speed_scaling = DoubleEditor(1.0, 100.0) speed_scaling.spin_box.setSuffix('%') speed_scaling.spin_box.setValue(50.0) speed_scaling.spin_box.setDecimals(0) speed_scaling.setEnabled(False) self._widget.speed_scaling_layout.addWidget(speed_scaling) self._speed_scaling_widget = speed_scaling speed_scaling.valueChanged.connect(self._on_speed_scaling_change) self._on_speed_scaling_change(speed_scaling.value()) # 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) # Initialize members self._jtc_name = [] # Name of selected joint trajectory controller self._cm_ns = [] # Namespace of the selected controller manager self._joint_pos = {} # name->pos map for joints of selected controller self._joint_names = [] # Ordered list of selected controller joints self._robot_joint_limits = {} # Lazily evaluated on first use # Timer for sending commands to active controller self._update_cmd_timer = QTimer(self) self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq) self._update_cmd_timer.timeout.connect(self._update_cmd_cb) # Timer for updating the joint widgets from the controller state self._update_act_pos_timer = QTimer(self) self._update_act_pos_timer.setInterval(1000.0 / self._widget_update_freq) self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) # Timer for controller manager updates # TODO: self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) self._update_cm_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_jtc_list_timer = QTimer(self) self._update_jtc_list_timer.setInterval(1000.0 / self._ctrlrs_update_freq) self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() # Signal connections w = self._widget w.enable_button.toggled.connect(self._on_jtc_enabled) w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) self._cmd_pub = None # Controller command publisher self._state_sub = None # Controller state subscriber self._list_controllers = None self._traj_ns_topic_dict = None def shutdown_plugin(self): self._update_cmd_timer.stop() self._update_act_pos_timer.stop() self._update_cm_list_timer.stop() self._update_jtc_list_timer.stop() self._unregister_state_sub() self._unregister_cmd_pub() def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('cm_ns', self._cm_ns) instance_settings.set_value('jtc_name', self._jtc_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() cm_ns = instance_settings.value('cm_ns') cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) # Resore last session's controller, if running self._update_jtc_list() jtc_name = instance_settings.value('jtc_name') jtc_combo = self._widget.jtc_combo jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) except (ValueError): pass except (ValueError): pass # def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget # title bar # Usually used to open a modal configuration dialog def _update_cm_list(self): trajectory_topics = _search_for_trajectory_topics(self._node) ## TODO: remove test code #trajectory_topics.append('/my_test/controller') #trajectory_topics.append('/no_namespace') #trajectory_topics.append('no_root') self._traj_ns_topic_dict = {} for full_topic in trajectory_topics: ns, topic = _split_namespace_from_topic(full_topic) self._traj_ns_topic_dict.setdefault(ns, []).append(topic) update_combo(self._widget.cm_combo, list(self._traj_ns_topic_dict.keys())) def _update_jtc_list(self): # Clear controller list if no controller information is available if self._traj_ns_topic_dict is None: self._widget.jtc_combo.clear() return #print(get_joint_limits(self._robot_description)) ## List of running controllers with a valid joint limits specification ## for _all_ their joints #running_jtc = self._running_jtc_info() #if running_jtc and not self._robot_joint_limits: # self._robot_joint_limits = get_joint_limits() # Lazy evaluation #valid_jtc = [] #for jtc_info in running_jtc: # has_limits = all(name in self._robot_joint_limits # for name in _jtc_joint_names(jtc_info)) # if has_limits: # valid_jtc.append(jtc_info); #valid_jtc_names = [data.name for data in valid_jtc] # Get the currently selected namespace curr_ns = self._widget.cm_combo.currentText() topics = self._traj_ns_topic_dict[curr_ns] update_combo(self._widget.jtc_combo, topics) def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() def _on_joint_state_change(self, actual_pos): assert(len(actual_pos) == len(self._joint_pos)) for name in actual_pos.keys(): self._joint_pos[name]['position'] = actual_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns if cm_ns: self._widget.jtc_combo.clear() self._update_jtc_list() else: self._list_controllers = None def _on_jtc_change(self, jtc_name): self._unload_jtc() self._jtc_name = jtc_name if self._jtc_name: self._load_jtc() def _on_jtc_enabled(self, val): # Don't allow enabling if there are no controllers selected if not self._jtc_name: self._widget.enable_button.setChecked(False) return # Enable/disable joint displays for joint_widget in self._joint_widgets(): joint_widget.setEnabled(val) # Enable/disable speed scaling self._speed_scaling_widget.setEnabled(val) if val: # Widgets send desired position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: # Controller updates widgets with actual position self._update_cmd_timer.stop() self._update_act_pos_timer.start() def _load_jtc(self): self._robot_joint_limits = get_joint_limits(self._robot_description) self._joint_names = list(self._robot_joint_limits.keys()) self._joint_pos = { name: {} for name in self._joint_names } # Update joint display try: layout = self._widget.joint_group.layout() for name in self._joint_names: limits = self._robot_joint_limits[name] joint_widget = DoubleEditor(limits['min_position'], limits['max_position']) layout.addRow(name, joint_widget) # NOTE: Using partial instead of a lambda because lambdas # "will not evaluate/look up the argument values before it is # effectively called, breaking situations like using a loop # variable inside it" from functools import partial par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) except: # TODO: Can we do better than swallow the exception? from sys import exc_info print('Unexpected error:', exc_info()[0]) # Enter monitor mode (sending commands disabled) self._on_jtc_enabled(False) # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) #state_topic = jtc_ns + '/state' state_topic = 'state' #cmd_topic = jtc_ns + '/command' cmd_topic = '/joint_trajectory_controller/joint_trajectory' #self._state_sub = rospy.Subscriber(state_topic, # JointTrajectoryControllerState, # self._state_cb, # queue_size=1) qos_profile = QoSProfile(depth=1, history=HistoryPolicy.KEEP_LAST, durability=DurabilityPolicy.TRANSIENT_LOCAL) self._state_sub = self._node.create_subscription( JointTrajectoryControllerState, state_topic, self._state_cb, qos_profile) self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1) # Start updating the joint positions self.jointStateChanged.connect(self._on_joint_state_change) def _unload_jtc(self): # Stop updating the joint positions try: self.jointStateChanged.disconnect(self._on_joint_state_change) except: pass # Reset ROS interfaces self._unregister_state_sub() self._unregister_cmd_pub() # Clear joint widgets # NOTE: Implementation is a workaround for: # https://bugreports.qt-project.org/browse/QTBUG-15990 :( layout = self._widget.joint_group.layout() if layout is not None: while layout.count(): layout.takeAt(0).widget().deleteLater() # Delete existing layout by reparenting to temporary QWidget().setLayout(layout) self._widget.joint_group.setLayout(QFormLayout()) # Reset joint data self._joint_names = [] self._joint_pos = {} # Enforce monitor mode (sending commands disabled) self._widget.enable_button.setChecked(False) def _running_jtc_info(self): from controller_manager_msgs.utils\ import filter_by_type, filter_by_state controller_list = self._list_controllers() jtc_list = filter_by_type(controller_list, 'JointTrajectoryController', match_substring=True) running_jtc_list = filter_by_state(jtc_list, 'running') return running_jtc_list def _unregister_cmd_pub(self): if self._cmd_pub is not None: self._node.destroy_publisher(self._cmd_pub) self._cmd_pub = None def _unregister_state_sub(self): if self._state_sub is not None: self._node.destroy_subscription(self._state_sub) self._state_sub = None def _robot_description_cb(self, msg): self._robot_description = msg.data def _state_cb(self, msg): actual_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] joint_pos = msg.actual.positions[i] actual_pos[joint_name] = joint_pos self.jointStateChanged.emit(actual_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]['command'] = val def _update_cmd_cb(self): dur = [] traj = JointTrajectory() traj.joint_names = self._joint_names point = JointTrajectoryPoint() for name in traj.joint_names: pos = self._joint_pos[name]['position'] cmd = pos try: cmd = self._joint_pos[name]['command'] except (KeyError): pass max_vel = self._robot_joint_limits[name]['max_velocity'] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) point.positions.append(cmd) max_duration_scaled = max(dur) / self._speed_scale seconds = floor(max_duration_scaled) nanoseconds = (max_duration_scaled - seconds) * 1e9 duration = Duration(seconds=seconds, nanoseconds=nanoseconds) point.time_from_start = duration.to_msg() traj.points.append(point) self._cmd_pub.publish(traj) def _update_joint_widgets(self): joint_widgets = self._joint_widgets() for id in range(len(joint_widgets)): joint_name = self._joint_names[id] try: joint_pos = self._joint_pos[joint_name]['position'] joint_widgets[id].setValue(joint_pos) except (KeyError): pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? widgets = [] layout = self._widget.joint_group.layout() for row_id in range(layout.rowCount()): widgets.append(layout.itemAt(row_id, QFormLayout.FieldRole).widget()) return widgets
class MyPlugin(Plugin): def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') # 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_mypkg'), 'resource', '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('MyPluginUi') # 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) #MOVE ROCAM self._widget.Up.clicked[bool].connect(self.move_up) self._widget.Down.clicked[bool].connect(self.move_down) self._widget.Right.clicked[bool].connect(self.move_right) self._widget.Left.clicked[bool].connect(self.move_left) #MOVE MOTOR SHIELD self._widget.MUp.clicked[bool].connect(self.move_mup) self._widget.MDown.clicked[bool].connect(self.move_mdown) self._widget.MRight.clicked[bool].connect(self.move_mright) self._widget.MLeft.clicked[bool].connect(self.move_mleft) self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) #no need to instantiate another rosnode self.mpub = rospy.Publisher('/motorshield',Vector3, queue_size=1) #no need to instantiate another rosnode self.forward_delta = 1 self.left_delta = 1 self.cmd_vel = Twist() #instantiating the twist object to be modified for sending self.motor_vel = Vector3() 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 move_up(self,checked): print("up") self.cmd_vel.angular.z = self.forward_delta self.publish() def move_down(self,checked): print("down") self.cmd_vel.angular.z = -1*self.forward_delta self.publish() def move_right(self,checked): print("right") self.cmd_vel.linear.x = self.left_delta self.publish() def move_left(self,checked): print("left") self.cmd_vel.linear.x = -1*self.left_delta self.publish() def publish(self): self.pub.publish(self.cmd_vel) def move_mup(self,checked): print("mup") num12 = 400.0 self.motor_vel.x = num12 self.motor_vel.y = num12 self.publishmotor() def move_mdown(self,checked): print("mdown") num12 = 400.0 self.motor_vel.x = -1*num12 self.motor_vel.y = -1*num12 self.publishmotor() def move_mright(self,checked): print("mright") num12 = 400.0 self.motor_vel.x = num12 self.motor_vel.y = 0 self.publishmotor() def move_mleft(self,checked): print("mleft") num12 = 400.0 self.motor_vel.x = 0 self.motor_vel.y = num12 self.publishmotor() def publishmotor(self): self.mpub.publish(self.motor_vel)
class MyPlugin(Plugin): target_pos = [] target_force = [] s = [] s_cmd = [] port = 8002 # where do you expect to get a msg? bufferSize = 12 # whatever you need server_thread = [] client_address = ('192.168.0.102', 8000) def __init__(self, context): super(MyPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('MyPlugin') # 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('m3_rqt'), 'resource', '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('M3') # 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.target_pos = self._widget.findChild(QSlider, "target_pos") self.target_force = self._widget.findChild(QSlider, "target_force") self.target_pos.valueChanged.connect(self.pos_target_change) self.target_force.valueChanged.connect(self.force_target_change) import select, socket self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.s.bind(('255.255.255.255', self.port)) self.s.setblocking(0) self.server_thread = threading.Thread(target=self.receiveStatus) self.server_thread.daemon = True self.server_thread.start() self.s_cmd = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # thread fuction def receiveStatus(self): while not rospy.is_shutdown(): result = select.select([self.s], [], []) msg = result[0][0].recv(self.bufferSize) print(unpack('fff', msg)) 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 trigger_configuration(self): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget title bar # Usually used to open a modal configuration dialog def pos_target_change(self): setpoint = self.target_pos.value() print(setpoint) self.s_cmd.sendto(pack('fB', setpoint, 0), self.client_address) def force_target_change(self): setpoint = self.target_force.value() print(setpoint) self.s_cmd.sendto(pack('fB', setpoint, 1), self.client_address)
class RosTfTree(QObject): _deferred_fit_in_view = Signal() def __init__(self, context): super(RosTfTree, self).__init__(context) self.initialized = False self.setObjectName('RosTfTree') self._current_dotcode = None self._widget = QWidget() # factory builds generic dotcode items self.dotcode_factory = PydotFactory() # self.dotcode_factory = PygraphvizFactory() # generator builds rosgraph self.dotcode_generator = RosTfTreeDotcodeGenerator() self.tf2_buffer_ = tf2_ros.Buffer() self.tf2_listener_ = tf2_ros.TransformListener(self.tf2_buffer_) # dot_to_qt transforms into Qt elements using dot layout self.dot_to_qt = DotToQtGenerator() rp = rospkg.RosPack() ui_file = os.path.join(rp.get_path('rqt_tf_tree'), 'resource', 'RosTfTree.ui') loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) self._widget.setObjectName('RosTfTreeUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._scene = QGraphicsScene() self._scene.setBackgroundBrush(Qt.white) self._widget.graphics_view.setScene(self._scene) self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) self._widget.refresh_graph_push_button.pressed.connect(self._update_tf_graph) self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view) self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view) self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) self._widget.load_dot_push_button.pressed.connect(self._load_dot) self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_dot_push_button.pressed.connect(self._save_dot) self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image-x-generic')) self._widget.save_as_image_push_button.pressed.connect(self._save_image) self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) self._deferred_fit_in_view.emit() context.add_widget(self._widget) self._force_refresh = False def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) def restore_settings(self, plugin_settings, instance_settings): self._widget.auto_fit_graph_check_box.setChecked( instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) self._widget.highlight_connections_check_box.setChecked( instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) self.initialized = True self._refresh_tf_graph() def _update_tf_graph(self): self._force_refresh = True self._refresh_tf_graph() def _refresh_tf_graph(self): if not self.initialized: return self._update_graph_view(self._generate_dotcode()) def _generate_dotcode(self): force_refresh = self._force_refresh self._force_refresh = False rospy.wait_for_service('~tf2_frames') tf2_frame_srv = rospy.ServiceProxy('~tf2_frames', FrameGraph) return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory, tf2_frame_srv=tf2_frame_srv, force_refresh=force_refresh) def _update_graph_view(self, dotcode): if dotcode == self._current_dotcode: return self._current_dotcode = dotcode self._redraw_graph_view() def _generate_tool_tip(self, url): return url def _redraw_graph_view(self): self._scene.clear() if self._widget.highlight_connections_check_box.isChecked(): highlight_level = 3 else: highlight_level = 1 (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, highlight_level) for node_item in nodes.itervalues(): self._scene.addItem(node_item) for edge_items in edges.itervalues(): for edge_item in edge_items: edge_item.add_to_scene(self._scene) self._scene.setSceneRect(self._scene.itemsBoundingRect()) if self._widget.auto_fit_graph_check_box.isChecked(): self._fit_in_view() def _load_dot(self, file_name=None): if file_name is None: file_name, _ = QFileDialog.getOpenFileName( self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return try: fhandle = open(file_name, 'rb') dotcode = fhandle.read() fhandle.close() except IOError: return self._update_graph_view(dotcode) def _fit_in_view(self): self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) def _save_dot(self): file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'frames.dot', self.tr('DOT graph (*.dot)')) if file_name is None or file_name == '': return file = QFile(file_name) if not file.open(QIODevice.WriteOnly | QIODevice.Text): return file.write(self._current_dotcode) file.close() def _save_svg(self): file_name, _ = QFileDialog.getSaveFileName( self._widget, self.tr('Save as SVG'), 'frames.svg', self.tr('Scalable Vector Graphic (*.svg)')) if file_name is None or file_name == '': return generator = QSvgGenerator() generator.setFileName(file_name) generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) painter = QPainter(generator) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() def _save_image(self): file_name, _ = QFileDialog.getSaveFileName( self._widget, self.tr('Save as image'), 'frames.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) if file_name is None or file_name == '': return img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) painter = QPainter(img) painter.setRenderHint(QPainter.Antialiasing) self._scene.render(painter) painter.end() img.save(file_name)