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 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 HelpingHandGUI(Plugin): # Various member variables # _topic_subscriber = None # _topic_good = False # _digitalinput_subscriber = None # _namespace = None _available_ns = [] _conf_yaml = None _trigger_sources = [] _data_buffer = {} _refresh_data_table_contents_sig = pyqtSignal() # _pattern = re.compile(r'\/[^/]+\/') # _digitalinput = None # _gravcomp_service = None # _dmp_action_server_client = None # _topic_name = None # _num_weights = 0 # _recorded_dmp = None # _msg_store = None # _database_service = None # _dmp_save_name = None # _pyaudio = None # _stream = None # _beep = None # _joint_space = None # _cartesian_space = None # _available_frames = [] # DMP trajectory buffer _joint_traj = [] # DMP encoding service _dmp_ecoder = None # Logging _init_status_report = { 'status': 'idle', 'last_entry': 'None', 'last_type': 'None', } # Logics _old_gravbutton = 0 _old_rec_button = 0 _recording = False _gravComp = False # Some signal _dmp_recording_started_signal = pyqtSignal() _dmp_recording_stopped_signal = pyqtSignal() # TF2 _tf2_buffer = tf2_ros.Buffer() _tf2_listener = tf2_ros.TransformListener(_tf2_buffer) _tf_listener = tf.TransformListener() # Available topics _available_topics_array = [] _available_topics_dict = dict() _old_index = 0 def __init__(self, context): super(HelpingHandGUI, self).__init__(context) # Give QObjects reasonable names self.setObjectName('HelpingHandGUI') # 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: # rospy.logdebug 'arguments: ', args # rospy.logdebug '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('helping_hand_gui'), 'qt', 'helpinghand.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names # self._widget.setObjectName('DMPRecordToolGUIUi') # 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) # Populate configuration table's headers self._conf_table = self._widget.findChild(QTableWidget, 'configTable') self._conf_table.setHorizontalHeaderLabels(( 'Trigger Name', 'Robot Namespace', 'Trigger Topic', 'Trigger Type', 'Trigger Value', 'Trigger Callback')) # Populate data table's headers self._data_table = self._widget.findChild(QTableWidget, 'data_table') self._data_table.setHorizontalHeaderLabels(( 'Timestamp', 'Trigger Conf', 'Trigger Reason', 'Data Type')) header = self._data_table.horizontalHeader() header.setSectionResizeMode(QHeaderView.ResizeToContents) # Hide the 5th column that contains data identifiers self._data_table.setColumnCount(self._data_table.columnCount()+1) self._data_table.setColumnHidden(self._data_table.columnCount()-1, True) # Connect the refresh signal with the update function self._refresh_data_table_contents_sig.connect(self._update_data_table) # Connect the item selected signal to the display details self._data_table.itemSelectionChanged.connect(self._display_item_details) # Parse config self._parse_yaml_config() # Show conf in table self._write_conf_to_table() # Display what we are listening to self._display_addresses() # Print initial status report self.status_report = StatusReport(self._init_status_report, self) self._print_status() # DMP Encoding service self._dmp_ecoder = rospy.ServiceProxy('/encode_traj_to_dmp', EncodeTrajectoryToDMP) # Save button widget save_button = self._widget.findChild(QPushButton, 'save_button') save_button.clicked.connect(partial(self._save_button_pressed, save_button)) # Create database proxy self._msg_store = MessageStoreProxy() # Create the service proxies for into saving databse self._tf_capture_srv = rospy.ServiceProxy('tf_capture', CaptureTF) self._joint_capture_srv = rospy.ServiceProxy('joint_capture', CaptureJoint) self._dmp_capture_srv = rospy.ServiceProxy('dmp_capture', CaptureDMP) # Make sure the services are running !! try: self._tf_capture_srv.wait_for_service(0.5) self._joint_capture_srv.wait_for_service(0.5) self._dmp_capture_srv.wait_for_service(0.5) except Exception as e: rospy.logerr("Could not establish a connection to services. See exception:\n{}".format(e)) exit() def _print_status(self): status_text_widget = self._widget.findChild(QTextEdit, 'statusWindow') status_text = \ "Status: {0}\n"\ "Last saved entry: {1}\n"\ "Last entry type: {2}".format( self.status_report['status'], self.status_report['last_entry'], self.status_report['last_type'] ) status_text_widget.setText(status_text) # YAML config parser def _parse_yaml_config(self): conf_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'conf', 'triggers_conf.yml') self._conf_yaml = yaml.load(open(conf_file), Loader=yaml.FullLoader) tmp_ns = [] for conf_key, conf_val in self._conf_yaml.items(): tmp_ns.append(conf_val['robot_ns']) if conf_val['trig_type'] == 'rising_edge': conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._rising_edge_cb, callback_args=(conf_key)) elif conf_val['trig_type'] == 'falling_edge': conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._falling_edge_cb, callback_args=(conf_key)) elif conf_val['trig_type'] == 'hold': conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._hold_cb, callback_args=(conf_key)) else: self.status_report['status'] = "Error parsing trigger types." if conf_val['trig_callback'] == 'joint_save': joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic'] conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key)) if conf_val['trig_callback'] == 'joint_pose_save': joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic'] conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key)) if conf_val['trig_callback'] == 'joint_dmp_save': joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic'] conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key)) # Add TF # Remove duplicate namespaces self._available_ns = list(dict.fromkeys(tmp_ns)) def _write_conf_to_table(self): self._conf_table.setRowCount(len(self._conf_yaml)) for row, conf_key in enumerate(self._conf_yaml.items()): conf = conf_key[1] self._conf_table.setItem(row, 0, QTableWidgetItem(conf['trig_name'])) self._conf_table.setItem(row, 1, QTableWidgetItem(conf['robot_ns'])) self._conf_table.setItem(row, 2, QTableWidgetItem(conf['trig_topic'])) self._conf_table.setItem(row, 3, QTableWidgetItem(conf['trig_type'])) self._conf_table.setItem(row, 4, QTableWidgetItem(str(conf['trig_value']))) self._conf_table.setItem(row, 5, QTableWidgetItem(conf['trig_callback'])) def _update_data_table(self): self._data_table.setRowCount(0) for data_key, data_dict in self._data_buffer.items(): current_row = self._data_table.rowCount() self._data_table.insertRow(current_row) self._data_table.setItem(current_row, 0, QTableWidgetItem(data_dict['time'])) self._data_table.setItem(current_row, 1, QTableWidgetItem(data_dict['trig_name'])) self._data_table.setItem(current_row, 2, QTableWidgetItem(data_dict['trig_type'])) self._data_table.setItem(current_row, 3, QTableWidgetItem(data_dict['data']._type)) self._data_table.setItem(current_row, 4, QTableWidgetItem(data_key)) self._data_table.sortItems(0, Qt.DescendingOrder) def _display_item_details(self): text_box = self._widget.findChild(QTextEdit, 'entry_details') try: curent_item = self._data_table.currentItem() current_row = curent_item.row() entry_identifier = self._data_table.item(current_row, 4).text() self.status_report['status'] = 'Item selected' data_details = self._data_buffer[entry_identifier]['data'], text_box.setText(format(data_details)) except Exception as e: pass def _display_addresses(self): self.discover_available_topics_array() addr_list_widget = self._widget.findChild(QListWidget, 'addr_list') for topic in self._available_topics_array: addr_list_widget.addItem(QListWidgetItem(topic)) # topic = '/ur10_2/joint_states' # self._available_ns # Get a list of topics that match a certain type def discover_available_topics_array(self): all_topics = rospy.get_published_topics() for topic, msg_type in all_topics: if msg_type in DESIRED_MSGS: self._available_topics_array.append(topic) self._available_topics_dict[topic] = msg_type self._available_topics_array.sort() # The following line removes possible duplicate entries from the array self._available_topics_array = list(set(self._available_topics_array)) # Discover available robots def discover_frames(self): # Define a search pattern, it will look for all namespaces that contain the 'received_control_mode' topic search_pattern = re.compile(r'^(?!.*link).*') # search_pattern = re.compile(r'^((?!.*link)|(.*base)).*$') # Get the list of all currently availabel frames in TF1 (this is a temporary solution until a better one is found) time.sleep(1) # Give tf time to start working # all_frames = self._tf_listener.getFrameStrings() yaml_frames = yaml.load(self._tf2_buffer.all_frames_as_yaml()) # Look through all avaiable topics for entry in yaml_frames: namespace = search_pattern.findall(entry) if len(namespace): self._available_frames.append(namespace[0]) # Sort the namespaces alphabetically self._available_frames.sort() # rospy.logdebug out the result of the search # rospy.loginfo("Found robots with namespace: \n%s", self._available_frames) # Add robots namespaces to combobox self._combobox = self._widget.findChildren(QComboBox, QRegExp('frameSelector'))[0] self._combobox.insertItems(0, self._available_frames) self._namespace = self._combobox.currentText() # Quit if no robots are found if len(self._available_frames) == 0: rospy.logerr("[%s]: No robots available !!", rospy.get_name()) sys.exit() def _save_button_pressed(self, save_button): save_name_widget = self._widget.findChild(QLineEdit, 'save_name') save_name = save_name_widget.text() # Check if a name was entered if not(len(save_name)): self.status_report['status'] = 'No name provided !!' return -1 # Get selected item curent_item = self._data_table.currentItem() if curent_item is None: self.status_report['status'] = 'No item selected !!' return -1 # Get selected item row current_row = curent_item.row() if current_row == -1: self.status_report['status'] = 'No item selected !!' return -1 # Get the item identifier and retrieve the data entry_identifier = self._data_table.item(current_row, 4).text() data = self._data_buffer[entry_identifier]['data'] # Update the status box self.status_report['status'] = 'Save button pressed' rospy.logdebug("Save button pressed !!") # Handle transform stamped if data._type == 'geometry_msgs/TransformStamped': trig_name = self._data_buffer[entry_identifier]['trig_name'] for keys, vals in self._data_buffer.items(): if (vals['trig_name'] == trig_name) and (vals['data']._type == 'sensor_msgs/JointState'): pass # Handle Joint space configuration elif data._type == 'sensor_msgs/JointState': self._joint_capture_srv.call(data, save_name) # Handle Joint space configuration elif data._type == 'robot_module_msgs/JointSpaceDMP': self._dmp_capture_srv.call(data, CartesianSpaceDMP(), save_name) # Handle Joint space DMP # # Saving to database according to the text in the saveName field # if self._joint_space: # existingDMP = self._msg_store.query_named(saveName, JointSpaceDMP._type)[0] # if existingDMP == None: # rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!") # self._msg_store.insert_named(saveName, self._recorded_dmp) # else: # rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!") # self._msg_store.update_named(saveName, self._recorded_dmp) # else: # existingDMP = self._msg_store.query_named(saveName, CartesianSpaceDMP._type)[0] # if existingDMP == None: # rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!") # self._msg_store.insert_named(saveName, self._recorded_dmp) # else: # rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!") # self._msg_store.update_named(saveName, self._recorded_dmp) # Write to report self.status_report['status'] = 'Data saved' self.status_report['last_entry'] = save_name self.status_report['last_type'] = data._type # Update text fields save_name_widget.setText('') details_text_box = self._widget.findChild(QTextEdit, 'entry_details') details_text_box.setText('') # Clean up table self._data_table.clearSelection() self._data_table.removeRow(current_row) # # Remove data from buffer # self._data_buffer.pop(entry_identifier) 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 # Topic subscribers def _joint_state_cb(self, data, conf_name): self._conf_yaml[conf_name]['joint_data'] = data # def save_joint_conf_sig_h(self): # rospy.loginfo('Saving joint conf !!') def _rising_edge_cb(self, data, conf_name): if not(self._conf_yaml[conf_name].has_key('sig_val')): self._conf_yaml[conf_name]['sig_val'] = data.data old_val = self._conf_yaml[conf_name]['sig_val'] new_val = data.data if old_val - new_val == -1: rospy.logdebug('Rising edge!!') self._handle_trigger(self._conf_yaml[conf_name]) self._conf_yaml[conf_name]['sig_val'] = new_val def _falling_edge_cb(self, data, conf_name): if not(self._conf_yaml[conf_name].has_key('sig_val')): self._conf_yaml[conf_name]['sig_val'] = data.data old_val = self._conf_yaml[conf_name]['sig_val'] new_val = data.data if old_val - new_val == 1: rospy.logdebug('Falling edge!!') self._handle_trigger(self._conf_yaml[conf_name]) self._conf_yaml[conf_name]['sig_val'] = new_val def _hold_cb(self, data, conf_name): if not('sig_val' in self._conf_yaml[conf_name]): self._conf_yaml[conf_name]['sig_val'] = data.data old_val = self._conf_yaml[conf_name]['sig_val'] new_val = data.data if falling_edge(old_val, new_val): rospy.logdebug("[_hold_cb] Falling edge") if len(self._joint_traj): self._handle_trigger(self._conf_yaml[conf_name]) if rising_edge(old_val, new_val): pass rospy.logdebug("[_hold_cb] Rising edge") if old_val + new_val == 2: rospy.logdebug("[_hold_cb] Holding") trig_type = self._conf_yaml[conf_name]['trig_callback'] if trig_type == "joint_dmp_save": rospy.logdebug("Appending !!") self._joint_traj.append(self._conf_yaml[conf_name]['joint_data']) self._conf_yaml[conf_name]['sig_val'] = new_val def _handle_trigger(self, trigg_conf): trigg_conf['robot_ns'] trig_type = trigg_conf['trig_callback'] save_data = [] if trig_type == "joint_save": rospy.logdebug('joint_save') save_data = [trigg_conf['joint_data']] elif trig_type == "joint_pose_save": joint_data = trigg_conf['joint_data'] rospy.logdebug('joint_pose_save') save_data = [joint_data, TransformStamped()] # , tf_data] elif trig_type == "joint_dmp_save": rospy.logdebug('joint_dmp_save') traj_dur = traj_duration_sec(self._joint_traj) if traj_dur < 1: rospy.logwarn("Trajectory too short to store") else: dmp_request = EncodeTrajectoryToDMPRequest() dmp_request.demonstratedTrajectory = self._joint_traj dmp_request.dmpParameters.N = trigg_conf['joint_dmp_N'] result_dmp = self._dmp_ecoder.call(dmp_request).encodedDMP save_data = [result_dmp] self._joint_traj = [] else: rospy.logerr("Unknown saving type !!") rospy.logerr("trig_type = {}".format(trig_type)) # self.status_report['status'] = "Error. See terminal" for data in save_data: now = datetime.now() entry_identifier = now.strftime("%H%M%S%f") current_time = now.strftime("%H:%M:%S") self._data_buffer[entry_identifier] = { 'data': data, 'time': current_time, 'trig_name': trigg_conf['trig_name'], 'trig_type': trigg_conf['trig_type'] } self._refresh_data_table_contents_sig.emit()
class SimulationRendererPlugin(Plugin): """Basic rqt plugin that allows to start/stop a reload in the renderer.""" def __init__(self, context): super(SimulationRendererPlugin, self).__init__(context) self._widget = QWidget() ui_file = os.path.join( rospkg.RosPack().get_path("simulation_rqt"), "resource", "SimulationRenderer.ui" ) loadUi(ui_file, self._widget) context.add_widget(self._widget) # Buttons self.button_stop = self._widget.findChild(QPushButton, "buttonStop") self.button_reload = self._widget.findChild(QPushButton, "buttonReload") # GUI Callbacks self.button_reload.clicked.connect(self.reload_renderer) self.button_stop.clicked.connect(self.stop_renderer) groundtruth_topics = self._load_groundtruth_topics() renderer_topics = groundtruth_topics["renderer"] self.reload_publisher = rospy.Publisher( renderer_topics["reload"], EmptyMsg, queue_size=1 ) self.stop_publisher = rospy.Publisher( renderer_topics["interrupt"], EmptyMsg, queue_size=1 ) self.info_subscriber = rospy.Subscriber( groundtruth_topics["status"], GroundtruthStatus, queue_size=1, callback=self.receive_groundtruth_status, ) self.button_reload.setEnabled(True) self.button_stop.setEnabled(False) def _load_groundtruth_topics(self): topic_file = os.path.join( rospkg.RosPack().get_path("simulation_groundtruth"), "param", "groundtruth", "topics.yaml", ) with open(topic_file, "r") as file: return yaml.safe_load(file) def receive_groundtruth_status(self, msg): """Receive new status update from renderer.""" self.button_reload.setEnabled(msg.status == GroundtruthStatus.READY) self.button_stop.setEnabled( msg.status == GroundtruthStatus.REMOVE_OLD_TILES or msg.status == GroundtruthStatus.RENDER_NEW_TILES ) def reload_renderer(self): """Publish message on renderer reload topic.""" self.reload_publisher.publish() def stop_renderer(self): """Publish message on renderer stop topic.""" self.stop_publisher.publish()
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 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('pressure_controller_setup'), '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') self.settings = self.get_settings() self.init_sliders() #self.init_config() self._client = actionlib.SimpleActionClient( 'pressure_control', pressure_controller_ros.msg.CommandAction) self._client_connected = self._client.wait_for_server( timeout=rospy.rostime.Duration(1)) if not self._client_connected: print("No command server avaiable... changes will not be sent") self.set_graph_state(True) self.send_channel_state(0, self.settings['channel_states'][0]) # 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 init_config(self): # self._plot_type_index = 0 # self.plot_types = [ # { # 'title': 'PyQtGraph', # 'widget_class': PyQtGraphDataPlot, # 'description': 'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph', # 'enabled': True,#PyQtGraphDataPlot is not None, # }, # { # 'title': 'MatPlot', # 'widget_class': MatDataPlot, # 'description': 'Based on MatPlotLib\n- needs most CPU\n- needs matplotlib >= 1.1.0\n- if using PySide: PySide > 1.1.0', # 'enabled': True,#MatDataPlot is not None, # }, # { # 'title': 'QwtPlot', # 'widget_class': QwtDataPlot, # 'description': 'Based on QwtPlot\n- does not use timestamps\n- uses least CPU\n- needs Python Qwt bindings', # 'enabled': True,#QwtDataPlot is not None, # }, # ] def init_sliders(self): sliderbox = self._widget.findChild(QLayout, 'Sliders') graph_button = QPushButton() graph_button.setCheckable(True) graph_button.setText("Graph Off") graph_button.toggle() graph_button.clicked.connect(self.set_graph_state) self.graph_button = graph_button firstCol = QVBoxLayout() firstCol.addWidget(graph_button) sliderbox.addLayout(firstCol) self.sliders = [] all_rows_layout = QVBoxLayout() chan_idx = 0 for num_channels_row in self.settings['num_channels']: row_layout = QHBoxLayout() for i in range(num_channels_row): idx = chan_idx * 1 slider_group = { 'slider_p': None, 'number_p': None, 'slider_v': None, 'number_v': None, 'on_off': None } layout_cluster = QVBoxLayout() slider_cluster = QHBoxLayout() label = QLabel() label.setText("Chan. %d" % (idx + 1)) label.setAlignment(Qt.AlignCenter) layout_cluster.addWidget(label) for j in range(2): layout = QVBoxLayout() layout.setAlignment(Qt.AlignHCenter) slider = QSlider(Qt.Vertical) slider.setMinimum(0) slider.setMaximum(255) slider.setValue( self.settings['valve_offsets'][chan_idx][j]) slider.setTickPosition(QSlider.TicksRight) slider.setTickInterval(5) spinbox = QSpinBox() spinbox.setRange(0, 255) spinbox.setValue( self.settings['valve_offsets'][chan_idx][j]) slider.valueChanged.connect(spinbox.setValue) spinbox.valueChanged.connect(slider.setValue) cb_function_curr = lambda value, idx=idx: self.send_slider_value( idx, value) slider.valueChanged.connect(cb_function_curr) label = QLabel() label.setAlignment(Qt.AlignCenter) if j == 0: slider_group['slider_p'] = slider slider_group['number_p'] = spinbox label.setText("P") else: slider_group['slider_v'] = slider slider_group['number_v'] = spinbox label.setText("V") labelmax = QLabel() labelmax.setAlignment(Qt.AlignCenter) labelmax.setText("255") labelmin = QLabel() labelmin.setAlignment(Qt.AlignCenter) labelmin.setText("0") layout.addWidget(label) layout.addWidget(labelmax) layout.addWidget(slider, Qt.AlignHCenter) layout.addWidget(labelmin) layout.addWidget(spinbox, Qt.AlignHCenter) layout.setAlignment(slider, Qt.AlignHCenter) layout.setAlignment(spinbox, Qt.AlignHCenter) slider_cluster.addLayout(layout) on_button = QPushButton() on_button.setCheckable(True) on_button.setText("Off") if self.settings['channel_states'][chan_idx]: on_button.toggle() on_button.setText("On") on_button.clicked.connect( lambda state, idx=idx: self.send_channel_state(idx, state)) slider_group['on_off'] = on_button layout_cluster.addLayout(slider_cluster) layout_cluster.addWidget(on_button) row_layout.addLayout(layout_cluster) row_layout.addSpacing(20) self.sliders.append(slider_group) chan_idx += 1 all_rows_layout.addLayout(row_layout) sliderbox.addLayout(all_rows_layout) #self._widget.setLayout(sliderbox) def get_settings(self): settings = {} settings['channel_states'] = rospy.get_param( '/config_node/channels/states', [1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) settings['num_channels'] = rospy.get_param( '/pressure_control/num_channels', []) settings['num_channels_tot'] = sum(settings['num_channels']) settings['valve_offsets'] = rospy.get_param( '/config_node/valve_offsets', [[220, 220]] * settings['num_channels_tot']) return settings def send_slider_value(self, idx, value): v1 = self.sliders[idx]['slider_p'].value() v2 = self.sliders[idx]['slider_v'].value() self.settings['valve_offsets'][idx][0] = v1 self.settings['valve_offsets'][idx][1] = v2 self.send_command(command='voffset', args=[idx, v1, v2]) rospy.set_param('/config_node/valve_offsets', self.settings['valve_offsets']) def send_channel_state(self, idx, state): if state: self.settings['channel_states'][idx] = 1 self.sliders[idx]['on_off'].setText("On") else: self.settings['channel_states'][idx] = 0 self.sliders[idx]['on_off'].setText("Off") self.send_command(command='chan', args=self.settings['channel_states']) rospy.set_param('/config_node/channels/states', self.settings['channel_states']) def set_graph_state(self, value): if value: on_off_str = 'on' self.graph_button.setText("Graph ON") else: on_off_str = 'off' self.graph_button.setText("Graph OFF") self.send_command(command=on_off_str, args=[]) def send_command(self, command, args, wait_for_ack=False): if self._client_connected: # Send commands to the command server and wait for things to be taken care of goal = pressure_controller_ros.msg.CommandGoal(command=command, args=args, wait_for_ack=False) self._client.send_goal(goal) self._client.wait_for_result() if not self._client.get_result(): raise ('Something went wrong and a setting was not validated') pass else: pass else: print(command, args) def shutdown_sliders(self): print("") print("Final Valve Offsets:") print(self.settings['valve_offsets']) print("") print("These settings were saved on the pressure controller") print("") print( "To ensure these parameters are always recalled, you can either:") print( " 1) Copy these settings into 'valve_offsets' in all your control config files" ) print( " 2) Remove the 'valve_offsets' parameter from your control config files" ) self.set_graph_state(False) for slider_group in self.sliders: for widget in slider_group: if 'on_off' in widget: slider_group[widget].clicked.disconnect() else: slider_group[widget].valueChanged.disconnect() def save_mcu_settings(self): self.send_command(command='save', args=[]) def shutdown_plugin(self): self._client.cancel_all_goals() self.shutdown_sliders() self.save_mcu_settings() 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 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('pressure_controller_setup'), '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('Set Pressures') self.settings = self.get_settings() self.init_sliders() self._client = actionlib.SimpleActionClient( 'pressure_control', pressure_controller_ros.msg.CommandAction) self._client_connected = self._client.wait_for_server( timeout=rospy.rostime.Duration(1)) if not self._client_connected: print("No command server avaiable... changes will not be sent") self.set_graph_state(True) self.send_channel_state(0, self.settings['channel_states'][0]) # 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 init_sliders(self): sliderbox = self._widget.findChild(QLayout, 'Sliders') firstCol = QVBoxLayout() graph_button = QPushButton() graph_button.setCheckable(True) graph_button.setText("Graph Off") graph_button.toggle() graph_button.clicked.connect(self.set_graph_state) reset_button = QPushButton() reset_button.setCheckable(False) reset_button.setText("Reset") reset_button.clicked.connect(self.set_reset) self.graph_button = graph_button self.reset_button = reset_button firstCol.addWidget(graph_button) firstCol.addWidget(reset_button) firstCol.setAlignment(graph_button, Qt.AlignVCenter) firstCol.setAlignment(reset_button, Qt.AlignVCenter) zero_button = QPushButton() zero_button.setCheckable(False) zero_button.setText("Set All Zero") zero_button.clicked.connect(self.set_pressure_zero) self.zero_button = zero_button firstCol.addWidget(zero_button) firstCol.setAlignment(zero_button, Qt.AlignVCenter) transition_box = QVBoxLayout() label = QLabel() label.setAlignment(Qt.AlignCenter) label.setText("Transition Time") spinbox = QDoubleSpinBox() spinbox.setMinimum(0) spinbox.setMaximum(10) spinbox.setValue(self.settings['transitions']) spinbox.setDecimals(1) spinbox.setSingleStep(0.1) spinbox.setSuffix(" sec") spinbox.valueChanged.connect(self.set_transition_value) transition_box.addWidget(label) transition_box.addWidget(spinbox) transition_box.setAlignment(label, Qt.AlignBottom) transition_box.setAlignment(spinbox, Qt.AlignTop) firstCol.addLayout(transition_box) self.sliders = [] sliderbox.addLayout(firstCol) all_rows_layout = QVBoxLayout() chan_idx = 0 for num_channels_row in self.settings['num_channels']: row_layout = QHBoxLayout() for i in range(num_channels_row): idx = chan_idx * 1 slider_group = {'slider': None, 'number': None, 'on_off': None} layout_cluster = QVBoxLayout() layout = QVBoxLayout() layout.setAlignment(Qt.AlignHCenter) slider = QSlider(Qt.Vertical) slider.setMinimum(self.settings['min_pressure'][idx] * 10.0) slider.setMaximum(self.settings['max_pressure'][idx] * 10.0) slider.setValue(0) slider.setTickPosition(QSlider.TicksRight) slider.setTickInterval(20) spinbox = QDoubleSpinBox() spinbox.setMinimum(self.settings['min_pressure'][idx]) spinbox.setMaximum(self.settings['max_pressure'][idx]) spinbox.setValue(0) spinbox.setDecimals(1) spinbox.setSingleStep(0.1) cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value( idx, value, slider) cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value( idx, value, slider) slider.valueChanged.connect(cb_function_curr2) spinbox.valueChanged.connect(cb_function_curr) labelmax = QLabel() labelmax.setAlignment(Qt.AlignCenter) labelmax.setText("%0.1f" % (self.settings['max_pressure'][idx])) labelmin = QLabel() labelmin.setAlignment(Qt.AlignCenter) labelmin.setText("%0.1f" % (self.settings['min_pressure'][idx])) layout.addWidget(labelmax) layout.addWidget(slider) layout.addWidget(labelmin) layout.addWidget(spinbox) layout.setAlignment(slider, Qt.AlignHCenter) layout.setAlignment(spinbox, Qt.AlignHCenter) label = QLabel() label.setText("Chan. %d" % (chan_idx + 1)) label.setAlignment(Qt.AlignCenter) layout_cluster.addWidget(label) layout_cluster.addLayout(layout) slider_group['slider'] = slider slider_group['number'] = spinbox on_button = QPushButton() on_button.setCheckable(True) on_button.setText("Off") if self.settings['channel_states'][idx]: on_button.toggle() on_button.setText("On") on_button.clicked.connect( lambda state, idx=idx: self.send_channel_state(idx, state)) slider_group['on_off'] = on_button layout_cluster.addWidget(on_button) row_layout.addLayout(layout_cluster) row_layout.addSpacing(20) self.sliders.append(slider_group) chan_idx += 1 all_rows_layout.addLayout(row_layout) sliderbox.addLayout(all_rows_layout) #self._widget.setLayout(sliderbox) def get_settings(self): settings = {} settings['channel_states'] = rospy.get_param( '/config_node/channels/states', [1, 1, 1, 1, 1, 1, 1, 1, 1, 1]) settings['num_channels'] = rospy.get_param( '/pressure_control/num_channels', []) settings['transitions'] = rospy.get_param('/config_node/transitions', 1.0) maxp = rospy.get_param('/config_node/max_pressure', 40.0) minp = rospy.get_param('/config_node/min_pressure', -40.0) settings['num_channels_tot'] = sum(settings['num_channels']) if isinstance(maxp, list): settings['max_pressure'] = maxp elif maxp is not None: settings['max_pressure'] = [maxp] * settings['num_channels_tot'] if isinstance(minp, list): settings['min_pressure'] = minp elif minp is not None: settings['min_pressure'] = [minp] * settings['num_channels_tot'] settings['pressures'] = [0] * settings['num_channels_tot'] return settings def set_transition_value(self, value): self.settings['transitions'] = value rospy.set_param('/config_node/transitions', self.settings['transitions']) def set_pressure_zero(self, value): self.settings['pressures'] = [0] * self.settings['num_channels_tot'] self.send_command(command='set', args=[self.settings['transitions']] + self.settings['pressures']) for slider_group in self.sliders: #slider_group['slider'].setValue(0) slider_group['number'].setValue(0) def send_slider_value(self, idx, value, slider): if slider: press = self.sliders[idx]['slider'].value() / 10.0 self.sliders[idx]['number'].setValue(press) else: press = self.sliders[idx]['number'].value() self.sliders[idx]['slider'].setValue(press * 10.0) self.settings['pressures'][idx] = press self.send_command(command='set', args=[self.settings['transitions']] + self.settings['pressures']) def send_channel_state(self, idx, state): if state: self.settings['channel_states'][idx] = 1 self.sliders[idx]['on_off'].setText("On") else: self.settings['channel_states'][idx] = 0 self.sliders[idx]['on_off'].setText("Off") self.send_command(command='chan', args=self.settings['channel_states']) rospy.set_param('/config_node/channels/states', self.settings['channel_states']) def set_graph_state(self, value): if value: on_off_str = 'on' self.graph_button.setText("Graph ON") else: on_off_str = 'off' self.graph_button.setText("Graph OFF") self.send_command(command=on_off_str, args=[]) def set_reset(self, value): self.send_command(command='mode', args=[3]) def send_command(self, command, args, wait_for_ack=False): if self._client_connected: # Send commands to the command server and wait for things to be taken care of goal = pressure_controller_ros.msg.CommandGoal(command=command, args=args, wait_for_ack=False) self._client.send_goal(goal) self._client.wait_for_result() if not self._client.get_result(): raise ('Something went wrong and a setting was not validated') pass else: pass else: print(command, args) def shutdown_sliders(self): print("") print("Final Pressures:") print(self.settings['pressures']) print("") self.set_graph_state(False) for slider_group in self.sliders: for widget in slider_group: if 'on_off' in widget: slider_group[widget].clicked.disconnect() else: slider_group[widget].valueChanged.disconnect() def save_mcu_settings(self): self.send_command(command='save', args=[]) def shutdown_plugin(self): self._client.cancel_all_goals() self.shutdown_sliders() self.save_mcu_settings() 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 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('pressure_controller_setup'), '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('Set Pressures') self.settings=self.get_settings() self.init_sliders() self._client = actionlib.SimpleActionClient('pressure_control', pressure_controller_ros.msg.CommandAction) self._client_connected=self._client.wait_for_server(timeout=rospy.rostime.Duration(1)) if not self._client_connected: print("No command server avaiable... changes will not be sent") self.set_graph_state(True) self.send_channel_state(0,self.settings['channel_states'][0]) # 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 init_sliders(self): sliderbox = self._widget.findChild(QLayout,'Sliders') firstCol = QVBoxLayout() graph_button=QPushButton() graph_button.setCheckable(True) graph_button.setText("Graph Off") graph_button.toggle() graph_button.clicked.connect(self.set_graph_state) self.graph_button = graph_button reset_button=QPushButton() reset_button.setCheckable(False) reset_button.setText("Reset") reset_button.clicked.connect(self.set_reset) self.graph_button = graph_button self.reset_button = reset_button firstCol.addWidget(graph_button) firstCol.addWidget(reset_button) firstCol.addWidget(graph_button) firstCol.setAlignment(graph_button,Qt.AlignVCenter) zero_button=QPushButton() zero_button.setCheckable(False) zero_button.setText("Set All Zero") zero_button.clicked.connect(self.set_pressure_zero) self.zero_button = zero_button firstCol.addWidget(zero_button) firstCol.setAlignment(zero_button,Qt.AlignVCenter) transition_box = QVBoxLayout() label = QLabel() label.setAlignment(Qt.AlignCenter) label.setText("Transition Time") spinbox = QDoubleSpinBox() spinbox.setMinimum(0) spinbox.setMaximum(10) spinbox.setValue(self.settings['transitions']) spinbox.setDecimals(1) spinbox.setSingleStep(0.1) spinbox.setSuffix(" sec") spinbox.valueChanged.connect(self.set_transition_value) transition_box.addWidget(label) transition_box.addWidget(spinbox) transition_box.setAlignment(label,Qt.AlignBottom) transition_box.setAlignment(spinbox,Qt.AlignTop) firstCol.addLayout(transition_box) self.sliders = [] sliderbox.addLayout(firstCol) all_rows_layout = QVBoxLayout() g_idx = 0 for row in self.settings['gui_config']: num_groups_row = len(row) row_layout = QHBoxLayout() for gr_idx, s_group in enumerate(row): g_channels = s_group['channels'] g_layout = s_group['layout'] if 'horiz' in g_layout: group_layout = QHBoxLayout() else: group_layout = QVBoxLayout() control_group = {'sliders': [], 'on_off': None} label = QLabel() label.setText("Group. %d"%(g_idx+1)) label.setAlignment(Qt.AlignCenter) group_layout.addWidget(label) for c_idx, s_idx in enumerate(g_channels): idx = s_idx*1 slider_group={'slider':None, 'number':None} layout_cluster = QVBoxLayout() labelfirst = QLabel() labelfirst.setAlignment(Qt.AlignCenter) labellast = QLabel() labellast.setAlignment(Qt.AlignCenter) layout = QVBoxLayout() if 'diff' in g_layout and c_idx == 0: sublayout=QHBoxLayout() layout.setAlignment(Qt.AlignVCenter) slider = QSlider(Qt.Horizontal) slider.setMinimum(-100) slider.setMaximum(100) slider.setValue(0) slider.setTickPosition(QSlider.TicksRight) slider.setTickInterval(20) spinbox = QDoubleSpinBox() spinbox.setMinimum(-10) spinbox.setMaximum(10) spinbox.setValue(0) spinbox.setDecimals(1) spinbox.setSingleStep(0.1) labellast.setText("%0.1f"%(10)) # These are flipped becasue of order labelfirst.setText("%0.1f"%(-10)) max_label = labellast min_label = labelfirst else: layout.setAlignment(Qt.AlignHCenter) slider = QSlider(Qt.Vertical) slider.setMinimum(self.settings['min_pressure'][idx]*10.0) slider.setMaximum(self.settings['max_pressure'][idx]*10.0) slider.setValue(0) slider.setTickPosition(QSlider.TicksRight) slider.setTickInterval(20) spinbox = QDoubleSpinBox() spinbox.setMinimum(self.settings['min_pressure'][idx]) spinbox.setMaximum(self.settings['max_pressure'][idx]) spinbox.setValue(0) spinbox.setDecimals(1) spinbox.setSingleStep(0.1) labelfirst.setText("%0.1f"%(self.settings['max_pressure'][idx])) labellast.setText("%0.1f"%(self.settings['min_pressure'][idx])) max_label = labelfirst min_label = labellast cb_function_number = lambda value, g_idx=g_idx, s_idx=c_idx, slider=False: self.send_slider_value(g_idx,s_idx,value,slider) cb_function_slider = lambda value, g_idx=g_idx, s_idx=c_idx, slider=True: self.send_slider_value(g_idx,s_idx,value,slider) slider.valueChanged.connect(cb_function_slider) spinbox.valueChanged.connect(cb_function_number) if 'diff' in g_layout and c_idx == 0: sublayout.addWidget(labelfirst) sublayout.addWidget(slider) sublayout.addWidget(labellast) layout.addWidget(spinbox) layout.addLayout(sublayout) else: layout.addWidget(labelfirst) layout.addWidget(slider) layout.addWidget(labellast) layout.addWidget(spinbox) layout.setAlignment(slider, Qt.AlignHCenter) layout.setAlignment(spinbox, Qt.AlignHCenter) layout_cluster.addLayout(layout) slider_group['slider'] = slider slider_group['number'] = spinbox slider_group['max_label'] = max_label slider_group['min_label'] = min_label control_group['sliders'].append(slider_group) group_layout.addLayout(layout_cluster) on_button=QPushButton() on_button.setCheckable(True) on_button.setText("Off") if self.settings['channel_states'][idx]: on_button.toggle() on_button.setText("On") on_button.clicked.connect(lambda state, g_idx=g_idx: self.send_channel_state(g_idx,state)) group_layout.addWidget(on_button) row_layout.addLayout(group_layout) row_layout.addSpacing(20) control_group['on_off'] = on_button self.sliders.append(control_group) g_idx+=1 all_rows_layout.addLayout(row_layout) sliderbox.addLayout(all_rows_layout) # for num_channels_row in self.settings['num_channels']: # row_layout = QHBoxLayout() # for i in range(num_channels_row): # idx = chan_idx*1 # slider_group={'slider':None, 'number':None, 'on_off':None} # layout_cluster = QVBoxLayout() # layout = QVBoxLayout() # layout.setAlignment(Qt.AlignHCenter) # slider = QSlider(Qt.Vertical) # slider.setMinimum(self.settings['min_pressure'][idx]*10.0) # slider.setMaximum(self.settings['max_pressure'][idx]*10.0) # slider.setValue(0) # slider.setTickPosition(QSlider.TicksRight) # slider.setTickInterval(20) # spinbox = QDoubleSpinBox() # spinbox.setMinimum(self.settings['min_pressure'][idx]) # spinbox.setMaximum(self.settings['max_pressure'][idx]) # spinbox.setValue(0) # spinbox.setDecimals(1) # spinbox.setSingleStep(0.1) # cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(idx,value,slider) # cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(idx,value,slider) # slider.valueChanged.connect(cb_function_curr2) # spinbox.valueChanged.connect(cb_function_curr) # labelmax = QLabel() # labelmax.setAlignment(Qt.AlignCenter) # labelmax.setText("%0.1f"%(self.settings['max_pressure'][idx])) # labelmin = QLabel() # labelmin.setAlignment(Qt.AlignCenter) # labelmin.setText("%0.1f"%(self.settings['min_pressure'][idx])) # layout.addWidget(labelmax) # layout.addWidget(slider) # layout.addWidget(labelmin) # layout.addWidget(spinbox) # layout.setAlignment(slider, Qt.AlignHCenter) # layout.setAlignment(spinbox, Qt.AlignHCenter) # label = QLabel() # label.setText("Chan. %d"%(chan_idx+1)) # label.setAlignment(Qt.AlignCenter) # layout_cluster.addWidget(label) # layout_cluster.addLayout(layout) # slider_group['slider'] = slider # slider_group['number'] = spinbox # on_button=QPushButton() # on_button.setCheckable(True) # on_button.setText("Off") # if self.settings['channel_states'][idx]: # on_button.toggle() # on_button.setText("On") # on_button.clicked.connect(lambda state, idx=idx: self.send_channel_state(idx,state)) # slider_group['on_off'] = on_button # layout_cluster.addWidget(on_button) # row_layout.addLayout(layout_cluster) # row_layout.addSpacing(20) # self.sliders.append(slider_group) # chan_idx+=1 # all_rows_layout.addLayout(row_layout) # sliderbox.addLayout(all_rows_layout) #self._widget.setLayout(sliderbox) def get_settings(self): settings={} settings['channel_states'] = rospy.get_param('/config_node/channels/states',[1,1,1,1,1,1,1,1,1,1]) settings['num_channels'] = rospy.get_param('/pressure_control/num_channels',[]) settings['transitions'] = rospy.get_param('/config_node/transitions',1.0) settings['gui_config'] = rospy.get_param('/config_node/gui_config',None) maxp = rospy.get_param('/config_node/max_pressure',40.0) minp = rospy.get_param('/config_node/min_pressure',-40.0) settings['num_channels_tot'] = sum(settings['num_channels']) if isinstance(maxp, list): settings['max_pressure'] = maxp elif maxp is not None: settings['max_pressure'] = [maxp]*settings['num_channels_tot'] if isinstance(minp, list): settings['min_pressure'] = minp elif minp is not None: settings['min_pressure'] = [minp]*settings['num_channels_tot'] settings['pressures'] = [0]*settings['num_channels_tot'] # If GUI Config is not located, default to rows of pressure sliders for each hardware device if settings['gui_config'] is None: chan_idx = 0 gui_config=[] for num_channels in settings['num_channels']: row=[] for idx in range(num_channels): row.append({'channels':[chan_idx], 'layout':'single'}) chan_idx+=1 gui_config.append(row) settings['gui_config'] = gui_config print(gui_config) settings['gui_config_flat'] = [] for row in settings['gui_config']: for group in row: settings['gui_config_flat'].append(group) return settings def set_transition_value(self, value): self.settings['transitions'] = value rospy.set_param('/config_node/transitions',self.settings['transitions']) def set_pressure_zero(self, value): self.settings['pressures'] = [0]*self.settings['num_channels_tot'] self.send_command(command='set', args=[self.settings['transitions']]+self.settings['pressures']) for slider_group in self.sliders: for slider in slider_group['sliders']: #slider_group['slider'].setValue(0) slider['number'].setValue(0) def send_slider_value(self, g_idx, s_idx, value, slider): if slider: press = self.sliders[g_idx]['sliders'][s_idx]['slider'].value()/10.0 self.sliders[g_idx]['sliders'][s_idx]['number'].setValue(press) else: press = self.sliders[g_idx]['sliders'][s_idx]['number'].value() self.sliders[g_idx]['sliders'][s_idx]['slider'].setValue(press*10.0) print(press) config_set = self.settings['gui_config_flat'][g_idx] if 'diff' in config_set['layout']: diff_p = self.sliders[g_idx]['sliders'][0]['number'].value() main_p = self.sliders[g_idx]['sliders'][1]['number'].value() if 'inv' in config_set['layout']: p1 = main_p+diff_p p2 = main_p-diff_p else: p1 = main_p-diff_p p2 = main_p+diff_p self.settings['pressures'][config_set['channels'][0]] = p1 self.settings['pressures'][config_set['channels'][1]] = p2 maxp1=self.settings['max_pressure'][config_set['channels'][0]] maxp2=self.settings['max_pressure'][config_set['channels'][1]] avg_maxp = sum([maxp1, maxp2])/2.0 max_bound = avg_maxp - main_p self.update_slider_bounds(g_idx,0,[-max_bound,max_bound]) print(g_idx, s_idx, main_p, diff_p) else: self.settings['pressures'][config_set['channels'][s_idx]]=press self.send_command(command='set', args=[self.settings['transitions']]+self.settings['pressures']) def send_channel_state(self, g_idx, state): if state: self.sliders[g_idx]['on_off'].setText("On") state_num=1 else: self.sliders[g_idx]['on_off'].setText("Off") state_num=0 config_set = self.settings['gui_config_flat'][g_idx] channels = config_set['channels'] for channel in channels: self.settings['channel_states'][channel] = state_num self.send_command(command='chan', args=self.settings['channel_states']) rospy.set_param('/config_node/channels/states',self.settings['channel_states']) def set_graph_state(self, value): if value: on_off_str='on' self.graph_button.setText("Graph ON") else: on_off_str='off' self.graph_button.setText("Graph OFF") self.send_command(command=on_off_str, args=[]) def set_reset(self, value): self.send_command(command='mode', args=[3]) def send_command(self, command, args, wait_for_ack=False): if self._client_connected: # Send commands to the command server and wait for things to be taken care of goal = pressure_controller_ros.msg.CommandGoal(command=command, args=args, wait_for_ack = False) self._client.send_goal(goal) self._client.wait_for_result() if not self._client.get_result(): raise ('Something went wrong and a setting was not validated') pass else: pass else: print(command, args) def update_slider_bounds(self, g_idx, s_idx, values): self.sliders[g_idx]['sliders'][s_idx]['slider'].setMinimum(values[0]*10.0) self.sliders[g_idx]['sliders'][s_idx]['number'].setMinimum(values[0]) self.sliders[g_idx]['sliders'][s_idx]['slider'].setMaximum(values[1]*10.0) self.sliders[g_idx]['sliders'][s_idx]['number'].setMaximum(values[1]) self.sliders[g_idx]['sliders'][s_idx]['min_label'].setText("%0.1f"%(values[0])) self.sliders[g_idx]['sliders'][s_idx]['max_label'].setText("%0.1f"%(values[1])) def shutdown_sliders(self): print("") print("Final Pressures:") print(self.settings['pressures']) print("") self.set_graph_state(False) for slider_group in self.sliders: for slider in slider_group['sliders']: slider['slider'].valueChanged.disconnect() slider['number'].valueChanged.disconnect() on_off = slider_group['on_off'] on_off.clicked.disconnect() def save_mcu_settings(self): self.send_command(command='save', args=[]) def shutdown_plugin(self): self._client.cancel_all_goals() self.shutdown_sliders() self.save_mcu_settings() 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): # pass
class GaitSelectionPlugin(Plugin): def __init__(self, context): super(GaitSelectionPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('GaitSelectionPlugin') # Connect to services try: rospy.wait_for_service('/march/gait_selection/get_version_map', 3) rospy.wait_for_service('/march/gait_selection/get_directory_structure', 3) rospy.wait_for_service('/march/gait_selection/set_version_map', 3) rospy.wait_for_service('/march/gait_selection/update_default_versions', 3) except rospy.ROSException: rospy.logerr('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.') rospy.signal_shutdown('Shutting down march_rqt_gait_selection, could not connect to march_gait_selection.') sys.exit(0) self.get_version_map = rospy.ServiceProxy('/march/gait_selection/get_version_map', Trigger) self.get_directory_structure = rospy.ServiceProxy('/march/gait_selection/get_directory_structure', Trigger) self.set_version_map = rospy.ServiceProxy('/march/gait_selection/set_version_map', StringTrigger) self.update_default_versions = rospy.ServiceProxy('/march/gait_selection/update_default_versions', Trigger) # 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('march_rqt_gait_selection'), 'resource', 'gait_selection.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('Gait Selection') # 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.Gaits.findChild(QWidget, 'scrollArea').findChild(QWidget, 'content').setLayout(QVBoxLayout()) self.refresh() self._widget.findChild(QPushButton, 'Refresh').clicked.connect(lambda: self.refresh(True)) self._widget.findChild(QPushButton, 'Apply').clicked.connect( lambda: [ self.set_gait_selection_map(True), self.refresh(), ]) self._widget.findChild(QPushButton, 'SaveDefault').clicked.connect( lambda: [ self.set_gait_selection_map(), self.update_defaults(True), self.refresh(), ]) self.log('Welcome to the Gait Selection.', Color.Debug) self.log('Select the versions of subgaits you want to select and press Apply.', Color.Info) self.log('Save as default persists between launches', Color.Info) self.log('Any warnings or errors will be displayed in this log.', Color.Warning) self.log('--------------------------------------', Color.Info) def log(self, msg, level): self._widget \ .findChild(QPlainTextEdit, 'Log') \ .appendHtml('<p style="color:' + str(level.value) + '">' + msg + '</p>') scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar() scrollbar.setValue(scrollbar.maximum()) def refresh(self, notify=False): if notify: self.log('Refreshing gait directory', Color.Debug) rospy.logdebug('Refreshing ui with %s', str(self.get_directory_structure().message)) try: gait_version_map = ast.literal_eval(self.get_version_map().message) except ValueError: self.log('Gait selection map is not valid' + str(self.get_version_map().message), Color.Error) rospy.logerr('Gait selection map is not valid' + str(self.get_version_map().message)) return try: gait_directory_structure = ast.literal_eval(self.get_directory_structure().message) except ValueError: self.log('Gait directory structure is not valid ' + str(self.get_directory_structure().message), Color.Error) rospy.logerr('Gait directory structure is not valid ' + str(self.get_directory_structure().message)) return gaits = self._widget.Gaits \ .findChild(QWidget, 'scrollArea') \ .findChild(QWidget, 'content') \ .findChildren(QGroupBox, 'Gait') for gait in gaits: gait.deleteLater() self.load_ui(gait_directory_structure, gait_version_map) def load_ui(self, gait_directory_structure, gait_selection_map): for gait_name, gait in gait_directory_structure.iteritems(): try: selection_map = gait_selection_map[gait_name] except KeyError: selection_map = None gait_group_box = self.create_gait(gait_name, gait, selection_map) self._widget.Gaits \ .findChild(QWidget, 'scrollArea') \ .findChild(QWidget, 'content') \ .layout() \ .addWidget(gait_group_box) def create_gait(self, name, gait, selections): gait_group_box = QGroupBox() gait_group_box.setObjectName('Gait') gait_group_box.setLayout(QHBoxLayout()) gait_group_box.setTitle(name) image = QLabel() image.setStyleSheet( 'background: url(' + gait['image'] + ') no-repeat center center 100px 100px;') image.setFixedSize(64, 80) gait_group_box.layout().addWidget(image) for subgait_name, subgait in gait['subgaits'].iteritems(): subgait_group_box = self.create_subgait(subgait_name, subgait, selections) gait_group_box.layout().addWidget(subgait_group_box) return gait_group_box def create_subgait(self, name, subgait, version_selection): subgait_group_box = QGroupBox() subgait_group_box.setLayout(QGridLayout()) subgait_group_box.setObjectName('Subgait') subgait_group_box.setTitle(name) try: version_name = version_selection[name] except TypeError: version_name = None except KeyError: version_name = None dropdown = self.create_dropdown(subgait, version_name) subgait_group_box.layout().addWidget(dropdown, 0, 0) return subgait_group_box def create_dropdown(self, options, selection): try: index = options.index(selection) except ValueError: rospy.loginfo('Selection %s not found in options %s.', str(selection), str(options)) index = -1 dropdown = QComboBox() for option in options: dropdown.addItem(option) dropdown.setCurrentIndex(index) return dropdown def set_gait_selection_map(self, notify=False): gait_selection_map = {} gaits = self._widget.Gaits \ .findChild(QWidget, 'scrollArea') \ .findChild(QWidget, 'content') \ .findChildren(QGroupBox, 'Gait') for gait in gaits: gait_name = str(gait.title()) gait_selection_map[gait_name] = {} subgaits = gait.findChildren(QGroupBox, 'Subgait') for subgait in subgaits: subgait_name = str(subgait.title()) version = str(subgait.findChild(QComboBox).currentText()) gait_selection_map[gait_name][subgait_name] = version res = self.set_version_map(str(gait_selection_map)) if res.success: if notify: self.log('Selection applied.', Color.Debug) rospy.loginfo(res.message) else: self.log(res.message, Color.Error) self.log('Resetting to last valid selection', Color.Warning) rospy.logwarn(res.message) def update_defaults(self, notify=False): res = self.update_default_versions() if res.success: if notify: self.log('Default selection updated.', Color.Debug) rospy.loginfo(res.message) else: self.log(res.message, Color.Error) self.log('Resetting to last valid selection', Color.Warning) rospy.logwarn(res.message)
class BenchmarkGui(Plugin): set_timer_signal = pyqtSignal(int, bool) set_benchmark_info_signal = pyqtSignal(str) def __init__(self, context): super(BenchmarkGui, self).__init__(context) self.setObjectName('EUROBENCH Benchmark Control') self._widget = QWidget() ui_file = path.join(path.dirname(path.realpath(__file__)), 'benchmark_gui.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('EUROBENCH Benchmark Control') self._widget.setWindowTitle('EUROBENCH Benchmark Control') self.benchmark_group = rospy.get_param('benchmark_group') # UI elements self.robot_spinbox = self._widget.findChild(QSpinBox, 'robot_spinbox') self.run_spinbox = self._widget.findChild(QSpinBox, 'run_spinbox') self.clock_value = self._widget.findChild(QLabel, 'clock_value') self.benchmark_status_value = self._widget.findChild( QLabel, 'benchmark_status_value') self.core_status_value = self._widget.findChild( QLabel, 'core_status_value') self.testbed_status_value = self._widget.findChild( QLabel, 'testbed_status_value') self.rosbag_controller_status_value = self._widget.findChild( QLabel, 'rosbag_controller_status_value') self.start_button = self._widget.findChild(QPushButton, 'start_button') self.stop_button = self._widget.findChild(QPushButton, 'stop_button') # UI callbacks self.start_button.clicked.connect(self.on_startbutton_click) self.stop_button.clicked.connect(self.on_stopbutton_click) # Disable start buttons, set their listeners self.start_button.setEnabled(False) self.stop_button.setEnabled(False) # Status of required nodes self.benchmark_core_available = False self.rosbag_controller_available = False self.testbed_node_available = False self.core_status = None # Subscribe to benchmark state rospy.Subscriber('bmcore/state', BenchmarkCoreState, self.state_callback, queue_size=1) # Run heartbeat checks for required nodes (every second) rospy.Timer(rospy.Duration(1), self.check_required_nodes) # Update status labels rospy.Timer(rospy.Duration(0.1), self.update_status_labels) context.add_widget(self._widget) def on_startbutton_click(self): robot_name = self.robot_spinbox.value() run_number = self.run_spinbox.value() try: start_benchmark = rospy.ServiceProxy('bmcore/start_benchmark', StartBenchmark) start_request = StartBenchmarkRequest() start_request.robot_name = robot_name start_request.run_number = run_number start_benchmark(start_request) except rospy.ServiceException as e: rospy.logerr( "bmcore/start_benchmark couldn't be called: {ex_val}".format( ex_val=str(e))) @staticmethod def on_stopbutton_click(): try: stop_benchmark = rospy.ServiceProxy('bmcore/stop_benchmark', StopBenchmark) stop_benchmark() except rospy.ServiceException as e: rospy.logerr( "bmcore/stop_benchmark couldn't be called: {ex_val}".format( ex_val=str(e))) def state_callback(self, core_status_msg): self.core_status = core_status_msg def update_status_labels(self, _): all_required_nodes_available = self.benchmark_core_available and self.rosbag_controller_available and self.testbed_node_available benchmark_running = self.core_status is not None and self.core_status.status == BenchmarkCoreState.RUNNING_BENCHMARK # ready_to_start if all_required_nodes_available: if not benchmark_running: self.benchmark_status_value.setText('Ready to start') self.run_spinbox.setEnabled(True) self.robot_spinbox.setEnabled(True) self.start_button.setEnabled(True) self.stop_button.setEnabled(False) if benchmark_running: self.benchmark_status_value.setText('Running benchmark') self.run_spinbox.setEnabled(False) self.robot_spinbox.setEnabled(False) self.start_button.setEnabled(False) self.stop_button.setEnabled(True) else: self.benchmark_status_value.setText( 'Cannot start (required nodes not running)') self.run_spinbox.setEnabled(False) self.robot_spinbox.setEnabled(False) self.start_button.setEnabled(False) self.stop_button.setEnabled(False) # Set clock timer if benchmark_running: seconds_passed = self.core_status.current_benchmark_seconds_passed if seconds_passed >= 0: minutes_passed = seconds_passed // 60 seconds_passed = seconds_passed % 60 self.clock_value.setText('%02d:%02d' % (minutes_passed, seconds_passed)) else: self.clock_value.setText('%02d' % seconds_passed) else: self.clock_value.setText('--:--') def shutdown_plugin(self): rospy.signal_shutdown('Shutting down') def save_settings(self, plugin_settings, instance_settings): # Save intrinsic configuration pass def restore_settings(self, plugin_settings, instance_settings): # Restore intrinsic configuration pass def check_required_nodes(self, _): # Benchmark core try: rospy.wait_for_service('bmcore/start_benchmark', timeout=0.5) self.benchmark_core_available = True except rospy.ROSException: self.benchmark_core_available = False # Rosbag controller try: rospy.wait_for_service( '/eurobench_rosbag_controller/start_recording', timeout=0.5) self.rosbag_controller_available = True except rospy.ROSException: self.rosbag_controller_available = False # Testbed if self.benchmark_group == 'MADROB': set_mode_service_name = '/madrob/door/set_mode' try: rospy.wait_for_service(set_mode_service_name, timeout=0.5) self.testbed_node_available = True except rospy.ROSException: self.testbed_node_available = False if self.benchmark_group == 'BEAST': some_service_name = '/beast_cart/reset_encoders' try: rospy.wait_for_service(some_service_name, timeout=0.5) self.testbed_node_available = True except rospy.ROSException: self.testbed_node_available = False if self.benchmark_core_available: self.core_status_value.setText('OK') else: self.core_status_value.setText('Off') if self.rosbag_controller_available: self.rosbag_controller_status_value.setText('OK') else: self.rosbag_controller_status_value.setText('Off') if self.testbed_node_available: self.testbed_status_value.setText('OK') else: self.testbed_status_value.setText('Off')
class RosCanGUI(Plugin): def __init__(self, context): super(RosCanGUI, self).__init__(context) self.setObjectName('RosCanSimGUI') rp = rospkg.RosPack() # Create QWidget self._widget = QWidget() # Get path to UI file which is a sibling of this file ui_file = os.path.join(rp.get_path('ros_can_sim'), 'resource', 'RosCanSimGUI.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) self._widget.setObjectName('RosCanSimUi') # 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())) # setup states and missions # enumrations taken from CanState.msg self.states = { CanState.AS_OFF: "OFF", CanState.AS_READY: "READY", CanState.AS_DRIVING: "DRIVING", CanState.AS_EMERGENCY_BRAKE: "EMERGENCY", CanState.AS_FINISHED: "FINISHED" } self.missions = { CanState.AMI_NOT_SELECTED: "NOT_SELECTED", CanState.AMI_ACCELERATION: "ACCELERATION", CanState.AMI_SKIDPAD: "SKIDPAD", CanState.AMI_AUTOCROSS: "AUTOCROSS", CanState.AMI_TRACK_DRIVE: "TRACK_DRIVE", CanState.AMI_AUTONOMOUS_DEMO: "AUTONOMOUS_DEMO", CanState.AMI_ADS_INSPECTION: "ADS_INSPECTION", CanState.AMI_ADS_EBS: "ADS_EBS", CanState.AMI_DDT_INSPECTION_A: "DDT_INSPECTION_A", CanState.AMI_DDT_INSPECTION_B: "DDT_INSPECTION_B", CanState.AMI_MANUAL: "MANUAL" } for mission in self.missions.values(): self._widget.findChild(QComboBox, "MissionSelectMenu").addItem(mission) # hook up buttons to callbacks self._widget.findChild( QPushButton, "SetMissionButton").clicked.connect(self.setMission) self._widget.findChild(QPushButton, "ResetButton").clicked.connect(self.resetState) self._widget.findChild(QPushButton, "RequestEBS").clicked.connect(self.requestEBS) self._widget.findChild(QPushButton, "DriveButton").clicked.connect(self.justDrive) # Subscribers self.state_sub = rospy.Subscriber("/ros_can/state", CanState, self.stateCallback) # Publishers self.set_mission_pub = rospy.Publisher("/ros_can/set_mission", CanState, queue_size=1) # Services self.ebs_srv = rospy.ServiceProxy("/ros_can/ebs", Trigger) self.reset_srv = rospy.ServiceProxy("/ros_can/reset", Trigger) # Add widget to the user interface context.add_widget(self._widget) def setMission(self): """Sends a mission request to the simulated ros_can The mission request is of message type eufs_msgs/CanState where only the ami_mission field is used""" mission = self._widget.findChild(QComboBox, "MissionSelectMenu").currentText() rospy.logdebug("Sending mission request for " + str(mission)) # create message to be sent mission_msg = CanState() # find enumerated mission and set for enum, mission_name in self.missions.items(): if mission_name == mission: mission_msg.ami_state = enum self.set_mission_pub.publish(mission_msg) rospy.logdebug("Mission request sent successfully") def resetState(self): """Requests ros_can to reset it's state machine""" rospy.logdebug("Requesting ros_can_sim reset") try: self.reset_srv.wait_for_service(timeout=1) # 1 second result = self.reset_srv() rospy.logdebug("ros_can_sim reset successful") rospy.logdebug(result) except: rospy.logwarn( "Requested ros_can_sim reset but /ros_can/reset service is not available" ) def requestEBS(self): """Requests ros_can to go into EMERGENCY_BRAKE state""" rospy.logdebug("Requesting ros_can_sim reset") try: self.ebs_srv.wait_for_service(timeout=1) # 1 second result = self.ebs_srv() rospy.logdebug("ros_can_sim reset successful") rospy.logdebug(result) except: rospy.logwarn( "Requested ros_can_sim EBS but /ros_can/ebs service is not available" ) def stateCallback(self, msg): """Reads the ros_can state from the message and displays it within the GUI Args: msg (eufs_msgs/CanState): state of ros_can """ self._widget.findChild(QLabel, "StateDisplay").setText( self.states[msg.as_state]) self._widget.findChild(QLabel, "MissionDisplay").setText( self.missions[msg.ami_state]) def justDrive(self): """overrides the state machine of the car and just makes it drive""" # create message to be sent state_msg = CanState() state_msg.as_state = CanState.AS_DRIVING state_msg.ami_state = CanState.AMI_MANUAL self.set_mission_pub.publish(state_msg) def shutdown_plugin(self): """stop all publisher, subscriber and services necessary for clean shutdown""" self.set_mission_pub.unregister() self.state_sub.unregister() self.ebs_srv.close() self.reset_srv.close() def save_settings(self, plugin_settings, instance_settings): # don't know how to use # don't delete function as it breaks rqt pass def restore_settings(self, plugin_settings, instance_settings): # don't know how to use # don't delete function as it breaks rqt pass
class SoftwareCheckPlugin(Plugin): def __init__(self, context): super(SoftwareCheckPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('SoftwareCheckPlugin') # 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('march_launch'), 'resource', 'software_check.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('SoftwareCheck') # 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.log('Welcome to the Software Check.', Color.Debug) self.log('Select the software checks you want to perform.', Color.Info) self.check_runner = CheckRunner(self.log) self._widget.Checks.findChild(QPushButton, 'GitBranch') \ .clicked.connect(lambda: self.run_test('GitBranch')) self._widget.Checks.findChild(QPushButton, 'GaitFileDirectory') \ .clicked.connect(lambda: self.run_test('GaitFileDirectory')) self._widget.Checks.findChild(QPushButton, 'URDF') \ .clicked.connect(lambda: self.run_test('URDF')) self._widget.Checks.findChild(QPushButton, 'SlaveCount') \ .clicked.connect(lambda: self.run_test('SlaveCount')) def log(self, msg, level): self._widget.findChild(QPlainTextEdit, 'Log').appendHtml('<p style="color:' + str(level.value) + '">' + str(msg) + '</p>') scrollbar = self._widget.findChild(QPlainTextEdit, 'Log').verticalScrollBar() scrollbar.setValue(scrollbar.maximum()) def run_test(self, name): result = self.check_runner.run_check_by_name(name) self.update_test_button(name, result) def update_test_button(self, name, result): button = self._widget.Checks.findChild(QPushButton, name) if result: button.setStyleSheet('background-color: ' + str(Color.Check_Passed.value)) else: button.setStyleSheet('background-color: ' + str(Color.Check_Failed.value))
class madrob_settings_gui(Plugin): def __init__(self, context): super(madrob_settings_gui, self).__init__(context) self.setObjectName('MADROB Settings') self._widget = QWidget() ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'madrob_settings_gui.ui') loadUi(ui_file, self._widget) self.showing_calibration = False self._widget.setObjectName('MADROB Settings') self._widget.setWindowTitle('MADROB Settings') self.benchmark_type_combo = self._widget.findChild( QComboBox, 'benchmark_type_combo') self.door_opening_side_combo = self._widget.findChild( QComboBox, 'door_opening_side_combo') self.robot_approach_side_combo = self._widget.findChild( QComboBox, 'robot_approach_side_combo') self.calibration_button = self._widget.findChild( QPushButton, 'show_hide_calib') self.calibration_button.clicked.connect(self.toggle_calibration_menu) self.calibration_menu = self._widget.findChild(QGroupBox, 'calib_groupbox') self.calibration_menu.hide() self.calib_loadcell_cw_button = self._widget.findChild( QPushButton, 'calib_loadcell_cw_button') self.calib_loadcell_cw_button.clicked.connect( self.calibrate_loadcell_cw) self.calib_loadcell_ccw_button = self._widget.findChild( QPushButton, 'calib_loadcell_ccw_button') self.calib_loadcell_ccw_button.clicked.connect( self.calibrate_loadcell_ccw) self.loadcell_weight_textedit = self._widget.findChild( QPlainTextEdit, 'calib_weight_textedit') self.calib_motor_start_button = self._widget.findChild( QPushButton, 'calib_motor_start') self.calib_motor_start_button.clicked.connect( self.start_motor_calibration) self.calib_encoder_cw_button = self._widget.findChild( QPushButton, 'calib_encoder_cw') self.calib_encoder_cw_button.clicked.connect(self.calibrate_encoder_cw) self.calib_encoder_closed_button = self._widget.findChild( QPushButton, 'calib_encoder_closed') self.calib_encoder_closed_button.clicked.connect( self.calibrate_encoder_closed) self.calib_encoder_ccw_button = self._widget.findChild( QPushButton, 'calib_encoder_ccw') self.calib_encoder_ccw_button.clicked.connect( self.calibrate_encoder_ccw) self.door_opening_side_combo.addItems(['CW', 'CCW']) self.robot_approach_side_combo.addItems(['CW', 'CCW']) # Every second, check for MADROB's settings from the core self.benchmark_types_set = False rospy.Timer(rospy.Duration(1), self.check_madrob_settings) context.add_widget(self._widget) self.run_rospy_node() def check_madrob_settings(self, _): if not self.benchmark_types_set: try: get_madrob_settings = rospy.ServiceProxy( 'madrob/settings', MadrobSettings) madrob_settings = get_madrob_settings() self.benchmark_type_combo.addItems( madrob_settings.benchmark_types) self.benchmark_types_set = True except rospy.ServiceException: pass # Core not available yet 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 toggle_calibration_menu(self): if self.showing_calibration: self.calibration_menu.hide() self.calibration_button.setText('Show calibration menu') else: self.calibration_menu.show() self.calibration_button.setText('Hide calibration menu') self.showing_calibration = not self.showing_calibration def calibrate_loadcell_cw(self): try: weight = int( round(float(self.loadcell_weight_textedit.toPlainText()))) except: rospy.logerr( 'Error parsing integer for "weight": no calibration done.') return calibrate_handle_service_name = '/madrob/handle/calibrate' try: rospy.wait_for_service(calibrate_handle_service_name, timeout=1) except rospy.ROSException: rospy.logerr('{service_name} unavailable'.format( service_name=calibrate_handle_service_name)) return calibrate_handle = rospy.ServiceProxy(calibrate_handle_service_name, CalibrateHandle) calibrate_handle_req = CalibrateHandleRequest() calibrate_handle_req.samples = 160 calibrate_handle_req.step = 0 calibrate_handle_req.force = -weight calibrate_handle_res = calibrate_handle(calibrate_handle_req) if calibrate_handle_res.success: rospy.loginfo('Loadcell CW calibrated') else: rospy.logerr('Could not calibrate loadcell CW: %s' % calibrate_handle_res.message) def calibrate_loadcell_ccw(self): try: weight = int( round(float(self.loadcell_weight_textedit.toPlainText()))) except: rospy.logerr( 'Error parsing integer for "weight": no calibration done.') return calibrate_handle_service_name = '/madrob/handle/calibrate' try: rospy.wait_for_service(calibrate_handle_service_name, timeout=1) except rospy.ROSException: rospy.logerr('{service_name} unavailable'.format( service_name=calibrate_handle_service_name)) return calibrate_handle = rospy.ServiceProxy(calibrate_handle_service_name, CalibrateHandle) calibrate_handle_req = CalibrateHandleRequest() calibrate_handle_req.samples = 160 calibrate_handle_req.step = 1 calibrate_handle_req.force = weight calibrate_handle_res = calibrate_handle(calibrate_handle_req) if calibrate_handle_res.success: rospy.loginfo('Loadcell CCW calibrated') else: rospy.logerr('Could not calibrate loadcell CCW: %s' % calibrate_handle_res.message) def start_motor_calibration(self): # TODO start motor calibration procedure pass def calibrate_encoder_closed(self): self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_ZERO) def calibrate_encoder_cw(self): self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_CW) def calibrate_encoder_ccw(self): self.calibrate_encoder(CalibrateDoorPositionRequest.POSITION_CCW) def calibrate_encoder(self, position): calibrate_position_service_name = '/madrob/door/calibrate_position' try: rospy.wait_for_service(calibrate_position_service_name, timeout=1) except rospy.ROSException: rospy.logerr('{service_name} unavailable'.format( service_name=calibrate_position_service_name)) return calibrate_position = rospy.ServiceProxy( calibrate_position_service_name, CalibrateDoorPosition) calibrate_position_req = CalibrateDoorPositionRequest() calibrate_position_req.position = position calibrate_position_res = calibrate_position(calibrate_position_req) if calibrate_position_res.success: rospy.loginfo('Door encoder calibrated. Position: %d' % position) else: rospy.logerr('Could not calibrate door encoder. Position: %d' % position) def run_rospy_node(self): rospy.Service('madrob/gui/benchmark_params', MadrobBenchmarkParams, self.benchmark_params_callback) def benchmark_params_callback(self, _): benchmark_params_response = MadrobBenchmarkParamsResponse() benchmark_params_response.benchmark_type = self.benchmark_type_combo.currentText( ) benchmark_params_response.door_opening_side = self.door_opening_side_combo.currentText( ) benchmark_params_response.robot_approach_side = self.robot_approach_side_combo.currentText( ) return benchmark_params_response
class RqtFileDialog(Plugin): def __init__(self, context): super(RqtFileDialog, self).__init__(context) # Give QObjects reasonable names self.setObjectName('RqtFileDialog') 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_file_dialog'), 'resource', 'file_dialog.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('RqtFileDialogUi') # 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.select_button = self._widget.findChild(QPushButton, 'select_button') self.select_button.clicked.connect(self.handle_select) self.current_line_edit = self._widget.findChild(QLineEdit, 'current_line_edit') self.current_line_edit.editingFinished.connect(self.publish) self.pub = None def handle_select(self): # TODO(lucasw) have a parameter define which kind of dialog to use file_dir = self.current_line_edit.text() new_file_dir = None if self.mode == "file_save": new_file_dir, tmp = QFileDialog.getSaveFileName(caption="Select a save file", directory=os.path.dirname(file_dir)) elif self.mode == "file_open": new_file_dir, tmp = QFileDialog.getOpenFileName(caption="Select a file", directory=os.path.dirname(file_dir)) else: # elif mode == "dir": new_file_dir = QFileDialog.getExistingDirectory(caption="Select a directory", directory=os.path.dirname(file_dir)) if new_file_dir is not None and new_file_dir != "": self.current_line_edit.setText(new_file_dir) self.publish() def publish(self): if self.pub is not None: self.pub.publish(self.current_line_edit.text()) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value("file_dir", self.current_line_edit.text()) instance_settings.set_value("mode", self.mode) instance_settings.set_value("select_text", self.select_button.text()) if self.pub: instance_settings.set_value("topic", self.topic) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains("file_dir"): file_dir = instance_settings.value("file_dir") self.current_line_edit.setText(file_dir) self.publish() self.mode = "file_save" if instance_settings.contains("mode"): self.mode = instance_settings.value("mode") self.topic = "file_dir" if instance_settings.contains("topic"): self.topic = instance_settings.value("topic") self.pub = rospy.Publisher(self.topic, String, queue_size=1, latch=True) if instance_settings.contains("select_text"): self.select_button.setText(instance_settings.value("select_text"))