class PropStatus(Plugin): def __init__(self, context): super(PropStatus, self).__init__(context) self.setObjectName('Status') self._widget = QWidget() loadUi(os.path.join(uipath, 'propagatorstatus.ui'), self._widget) context.add_widget(self._widget) rospy.Subscriber('/thrusters/info',ThrusterInfo,motordriver_callback) rospy.Subscriber('/skytraq_serial',SerialPacket,gps_callback) rospy.Subscriber('/imu/data_raw',Imu,imu_callback) rospy.Subscriber('/scan',LaserScan,lidar_callback) rospy.Subscriber('/mv_bluefox_camera_node/image_raw',Image,camera_callback) rospy.Timer(rospy.Duration(2),kill) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(100) def _on_update(self): self._widget.findChild(QListWidget, 'list').clear() for i in ['FR','FL','BR','BL','GPS','IMU','LIDAR','CAMERA']: pItem = QListWidgetItem(i); if i in active: pItem.setBackground(Qt.green); else: pItem.setBackground(Qt.red); self._widget.findChild(QListWidget, 'list').addItem(pItem)
def _add_thruster_widget(self, id): self._widget.findChild(QLabel, 'noThrustersLabel').setVisible(False) thruster_widget = QWidget() thruster_frame = self._widget.findChild(QFrame, 'thrusterFrame') pos = sum(1 for existing_id in self._thruster_widgets if id > existing_id) thruster_frame.layout().insertWidget(pos, thruster_widget) loadUi(os.path.join(uipath, 'thruster.ui'), thruster_widget) thruster_widget.findChild(QLabel, 'thrusterLabel').setText(id) thruster_widget.findChild(QPushButton, 'offButton').clicked.connect(lambda: self._on_stop_clicked(id)) self._thruster_widgets[id] = thruster_widget return thruster_widget
class AlarmPlugin(Plugin): _severity_mapping = { 0: qtg.QColor(255, 0, 0), 1: qtg.QColor(240, 100, 0), 2: qtg.QColor(220, 200, 0), 3: qtg.QColor(30, 255, 30), } _column_headers = ["Name", "Status", "Description", "Time Recieved"] def __init__(self, context): super(AlarmPlugin, self).__init__(context) self.setObjectName('AlarmPlugin') # Things seem to misbehave when this is missing # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) self._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('sub8_rqt'), 'resource', 'sub8_rqt_alarms.ui') # Extend the widget with all attributes and children from UI file self.ui = loadUi(ui_file, self._widget) self._widget.setObjectName('AlarmPlugin') self.alarm_tree = self._widget.findChild(qtg.QTreeWidget, 'alarm_tree') self.alarm_table = self._widget.findChild(qtg.QTableWidget, 'alarm_table') # Alarm parameter cache stores some additional information about the alarm # (for use when an alarm is clicked) self.alarm_parameter_cache = {} # Default row-count to 0 self.alarm_table.setRowCount(0) self.alarm_table.setColumnCount(len(self._column_headers)) self.alarm_table.setHorizontalHeaderLabels(self._column_headers) self.alarm_table.clicked.connect(self.open_alarm) # ---- ROS ---- self.alarm_sub = rospy.Subscriber('alarm', Alarm, self.new_alarm_callback) 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 new_alarm_callback(self, msg): '''React to a new alarm''' columns = [ 'alarm_name', 'severity', 'problem_description' ] row_elements = map(functools.partial(getattr, msg), columns) # Get pub time alarm_epoch = msg.header.stamp.to_time() formatted_time = datetime.datetime.fromtimestamp(alarm_epoch).strftime('%I:%M:%S.%f') row_elements.append(formatted_time) color = self._severity_mapping[msg.severity] self.alarm_table.insertRow(0) first_col = self.set_row(0, row_elements, color) self.alarm_parameter_cache[first_col] = msg.parameters def set_row(self, row, data, color): '''Set a whole row in the alarm table''' assert isinstance(data, list), "data must be a list" to_return = None for col, element in enumerate(data): table_item = newTableWidgetItem(element) table_item.setBackground(color) self.alarm_table.setItem(row, col, table_item) if col == 0: to_return = table_item return to_return def open_alarm(self, event): '''React when an alarm has been clicked 1. Use event to determine which cell has been clicked 2. Select that cell's whole row 3. Use the cached json_descriptions of each alarm to get that row's json data When an alarm has been clicked, we'll display its json parameters in the box on the right ''' items_selected = self.alarm_table.selectedItems() if len(items_selected) == 0: return row_selected = items_selected[0].row() self.alarm_table.selectRow(row_selected) key_item = self.alarm_table.itemAt(row_selected, 0) try: json_parameters = json.loads(self.alarm_parameter_cache[key_item]) except ValueError: rospy.logwarn("Could not decode alarm message") return self.alarm_tree.clear() try: build_tree_from_json(self.alarm_tree, json_parameters) except AssertionError: rospy.logwarn("Could not draw json tree for alarm (Is it a dictionary?)") def shutdown_plugin(self): '''Unregister our subsribers''' self.alarm_sub.unregister() def save_settings(self, plugin_settings, instance_settings): rospy.logwarn('Saving does not currently do anything') pass def restore_settings(self, plugin_settings, instance_settings): rospy.logwarn('Restoring does not currently do anything') pass
class AlarmPlugin(Plugin): _severity_mapping = { 0: qtg.QColor(255, 0, 0), 1: qtg.QColor(240, 100, 0), 2: qtg.QColor(220, 200, 0), 3: qtg.QColor(30, 255, 30), } _column_headers = ["Name", "Status", "Description", "Time Recieved"] def __init__(self, context): super(AlarmPlugin, self).__init__(context) self.setObjectName('AlarmPlugin') # Things seem to misbehave when this is missing # Process standalone plugin command-line arguments from argparse import ArgumentParser parser = ArgumentParser() # Add argument(s) to the parser. parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) self._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('sub8_rqt'), 'resource', 'sub8_rqt_alarms.ui') # Extend the widget with all attributes and children from UI file self.ui = loadUi(ui_file, self._widget) self._widget.setObjectName('AlarmPlugin') self.alarm_tree = self._widget.findChild(qtg.QTreeWidget, 'alarm_tree') self.alarm_table = self._widget.findChild(qtg.QTableWidget, 'alarm_table') # Alarm parameter cache stores some additional information about the alarm # (for use when an alarm is clicked) self.alarm_parameter_cache = {} # Default row-count to 0 self.alarm_table.setRowCount(0) self.alarm_table.setColumnCount(len(self._column_headers)) self.alarm_table.setHorizontalHeaderLabels(self._column_headers) self.alarm_table.clicked.connect(self.open_alarm) # ---- ROS ---- self.alarm_sub = rospy.Subscriber('alarm', Alarm, self.new_alarm_callback) 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 new_alarm_callback(self, msg): '''React to a new alarm''' columns = ['alarm_name', 'severity', 'problem_description'] row_elements = map(functools.partial(getattr, msg), columns) # Get pub time alarm_epoch = msg.header.stamp.to_time() formatted_time = datetime.datetime.fromtimestamp(alarm_epoch).strftime( '%I:%M:%S.%f') row_elements.append(formatted_time) color = self._severity_mapping[msg.severity] self.alarm_table.insertRow(0) first_col = self.set_row(0, row_elements, color) self.alarm_parameter_cache[first_col] = msg.parameters def set_row(self, row, data, color): '''Set a whole row in the alarm table''' assert isinstance(data, list), "data must be a list" to_return = None for col, element in enumerate(data): table_item = newTableWidgetItem(element) table_item.setBackground(color) self.alarm_table.setItem(row, col, table_item) if col == 0: to_return = table_item return to_return def open_alarm(self, event): '''React when an alarm has been clicked 1. Use event to determine which cell has been clicked 2. Select that cell's whole row 3. Use the cached json_descriptions of each alarm to get that row's json data When an alarm has been clicked, we'll display its json parameters in the box on the right ''' items_selected = self.alarm_table.selectedItems() if len(items_selected) == 0: return row_selected = items_selected[0].row() self.alarm_table.selectRow(row_selected) key_item = self.alarm_table.itemAt(row_selected, 0) try: json_parameters = json.loads(self.alarm_parameter_cache[key_item]) except ValueError: rospy.logwarn("Could not decode alarm message") return self.alarm_tree.clear() try: build_tree_from_json(self.alarm_tree, json_parameters) except AssertionError: rospy.logwarn( "Could not draw json tree for alarm (Is it a dictionary?)") def shutdown_plugin(self): '''Unregister our subsribers''' self.alarm_sub.unregister() def save_settings(self, plugin_settings, instance_settings): rospy.logwarn('Saving does not currently do anything') pass def restore_settings(self, plugin_settings, instance_settings): rospy.logwarn('Restoring does not currently do anything') pass
class Navigator_gui(Plugin): def __init__(self, context): super(Navigator_gui, self).__init__(context) # Give QObjects reasonable names self.setObjectName('Navigator_gui') # 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: pass # print 'arguments: ', args # print 'unknowns: ', unknowns self.wrench_out = WrenchStamped() # 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('navigator_rqt'), 'resource', 'navigator_gui.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('Navigator_gui') kill_button = self._widget.findChild(qtg.QPushButton, 'kill') kill_button.clicked.connect(self.toggle_kill) revive_button = self._widget.findChild(qtg.QPushButton, 'revive') revive_button.clicked.connect(self.revive) revive_button = self._widget.findChild(qtg.QPushButton, 'move_send') revive_button.clicked.connect(self.relative_move) station_hold_button = self._widget.findChild(qtg.QPushButton, 'station_hold') station_hold_button.clicked.connect(self.station_hold) station_hold_button = self._widget.findChild(qtg.QPushButton, 'docking_mode') station_hold_button.clicked.connect(self.toggle_docking) rc_mode_button = self._widget.findChild(qtg.QPushButton, 'rc_mode') rc_mode_button.clicked.connect(self.rc_mode) au_mode_button = self._widget.findChild(qtg.QPushButton, 'au_mode') au_mode_button.clicked.connect(self.au_mode) gui_mode_button = self._widget.findChild(qtg.QPushButton, 'gui_mode') gui_mode_button.clicked.connect(self.gui_mode) forward_slider = self._widget.findChild(qtg.QSlider, 'forward_slider') forward_slider.valueChanged.connect(self.forward_slider) backward_slider = self._widget.findChild(qtg.QSlider, 'backward_slider') backward_slider.valueChanged.connect(self.backward_slider) right_slider = self._widget.findChild(qtg.QSlider, 'right_slider') right_slider.valueChanged.connect(self.right_slider) left_slider = self._widget.findChild(qtg.QSlider, 'left_slider') left_slider.valueChanged.connect(self.left_slider) yaw_right_slider = self._widget.findChild(qtg.QSlider, 'yaw_right_slider') yaw_right_slider.valueChanged.connect(self.yaw_right_slider) yaw_left_slider = self._widget.findChild(qtg.QSlider, 'yaw_left_slider') yaw_left_slider.valueChanged.connect(self.yaw_left_slider) # 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 self.joy_pub = rospy.Publisher("/joy", Joy, queue_size=1) # 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.wrench_pub = rospy.Publisher("/wrench/gui", WrenchStamped, queue_size=1) self.move_pub = rospy.Publisher('/move_helper', Point, queue_size=1) self.wrench_changer = rospy.ServiceProxy('/change_wrench', WrenchSelect) rospy.Subscriber("/tf", TFMessage, self.wrench_pub_cb) def wrench_pub_cb(self, msg): self.wrench_pub.publish(self.wrench_out) def rc_mode(self): self.wrench_changer("rc") def au_mode(self): self.wrench_changer("autonomous") def gui_mode(self): self.wrench_changer("gui") def station_hold(self): msg = Joy() msg.buttons.append(1) for x in xrange(0, 8): msg.buttons.append(0) for x in xrange(0, 4): msg.axes.append(0) self.joy_pub.publish(msg) time.sleep(.1) clr_msg = Joy() clr_msg.buttons.append(0) for x in xrange(0, 8): clr_msg.buttons.append(0) for x in xrange(0, 4): clr_msg.axes.append(0) self.joy_pub.publish(clr_msg) def toggle_kill(self): msg = Joy() for x in xrange(0, 8): msg.buttons.append(0) msg.buttons.append(1) for x in xrange(0, 4): msg.axes.append(0) self.joy_pub.publish(msg) time.sleep(.1) clr_msg = Joy() for x in xrange(0, 8): clr_msg.buttons.append(0) clr_msg.buttons.append(0) for x in xrange(0, 4): clr_msg.axes.append(0) self.joy_pub.publish(clr_msg) def revive(self): msg = Joy() msg.buttons.append(0) msg.buttons.append(1) for x in xrange(0, 7): msg.buttons.append(0) for x in xrange(0, 4): msg.axes.append(0) self.joy_pub.publish(msg) time.sleep(.1) clr_msg = Joy() for x in xrange(0, 8): clr_msg.buttons.append(0) clr_msg.buttons.append(0) for x in xrange(0, 4): clr_msg.axes.append(0) self.joy_pub.publish(clr_msg) def toggle_docking(self): msg = Joy() for x in xrange(0, 6): msg.buttons.append(0) msg.buttons.append(1) msg.buttons.append(0) msg.buttons.append(0) for x in xrange(0, 4): msg.axes.append(0) self.joy_pub.publish(msg) time.sleep(.1) clr_msg = Joy() for x in xrange(0, 6): clr_msg.buttons.append(0) clr_msg.buttons.append(0) clr_msg.buttons.append(0) clr_msg.buttons.append(0) for x in xrange(0, 4): clr_msg.axes.append(0) self.joy_pub.publish(clr_msg) def relative_move(self): x = self._widget.findChild(qtg.QPlainTextEdit, 'x_send').toPlainText() y = self._widget.findChild(qtg.QPlainTextEdit, 'y_send').toPlainText() z = self._widget.findChild(qtg.QPlainTextEdit, 'z_send').toPlainText() x, y, z = float(x), float(y), float(z) to_send = Point(x, y, z) self.move_pub.publish(to_send) def forward_slider(self, value): if self.wrench_out.wrench.force.x >= 0: self.wrench_out.wrench.force.x = 5 * value def backward_slider(self, value): if self.wrench_out.wrench.force.x <= 0: self.wrench_out.wrench.force.x = -5 * value def right_slider(self, value): if self.wrench_out.wrench.force.y <= 0: self.wrench_out.wrench.force.y = -5 * value def left_slider(self, value): if self.wrench_out.wrench.force.y >= 0: self.wrench_out.wrench.force.y = 5 * value def yaw_right_slider(self, value): if self.wrench_out.wrench.torque.z <= 0: self.wrench_out.wrench.torque.z = -5 * value def yaw_left_slider(self, value): if self.wrench_out.wrench.torque.z >= 0: self.wrench_out.wrench.torque.z = 5 * value def forward_slider(self, value): if self.wrench_out.wrench.force.x >= 0: self.wrench_out.wrench.force.x = 5 * value
class ThrusterPlugin(Plugin): def __init__(self, context): super(ThrusterPlugin, self).__init__(context) self.setObjectName('ThrusterPlugin') self._listener = ThrusterListener() self._thruster_widgets = {} self._manual_control = False self._widget = QWidget() loadUi(os.path.join(uipath, 'thrusterplugin.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QPushButton, 'stopAllButton').clicked.connect(self._on_stopall_clicked) self._widget.findChild(QCheckBox, 'manualControlCheckBox').clicked.connect(self._on_manualcontrol_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._update) self._update_timer.start(100) def _update(self): active_ids = set() for thruster_info in self._listener.get_thrusters(): active_ids.add(thruster_info.id) if thruster_info.id not in self._thruster_widgets: self._add_thruster_widget(thruster_info.id) for id in self._thruster_widgets.keys(): if id not in active_ids: self._remove_thruster_widget(id) else: self._update_thruster(id) def _add_thruster_widget(self, id): self._widget.findChild(QLabel, 'noThrustersLabel').setVisible(False) thruster_widget = QWidget() thruster_frame = self._widget.findChild(QFrame, 'thrusterFrame') pos = sum(1 for existing_id in self._thruster_widgets if id > existing_id) thruster_frame.layout().insertWidget(pos, thruster_widget) loadUi(os.path.join(uipath, 'thruster.ui'), thruster_widget) thruster_widget.findChild(QLabel, 'thrusterLabel').setText(id) thruster_widget.findChild(QPushButton, 'offButton').clicked.connect(lambda: self._on_stop_clicked(id)) self._thruster_widgets[id] = thruster_widget return thruster_widget def _remove_thruster_widget(self, thruster): if thruster not in self._thruster_widgets: return self._thruster_widgets[thruster].deleteLater() del self._thruster_widgets[thruster] if len(self._thruster_widgets) == 0: self._widget.findChild(QLabel, 'noThrustersLabel').setVisible(True) def _update_thrusters(self): for thruster in self._listener.get_thrusters(): self._update_thruster(thruster) def _update_thruster(self, id): thruster = self._listener.get_thruster(id) thruster_widget = self._thruster_widgets[id] slider_widget = thruster_widget.findChild(QSlider, 'thrusterSlider') reverse_widget = thruster_widget.findChild(QCheckBox, 'reverseCheckbox') thruster_widget.findChild(QPushButton, 'offButton').setEnabled(self._manual_control) thruster_widget.findChild(QCheckBox, 'reverseCheckbox').setEnabled(self._manual_control) thruster_widget.findChild(QSlider, 'thrusterSlider').setEnabled(self._manual_control) if not self._manual_control: command = self._listener.get_command(id) if command is None: return if command.force > 0: effort = command.force / thruster.max_force else: effort = command.force / thruster.min_force slider_widget.setValue(abs(effort) * 100) reverse_widget.setChecked(effort < 0) else: effort = slider_widget.value()/100 if not reverse_widget.isChecked(): force = effort * thruster.max_force else: force = effort * thruster.min_force self._listener.send_command(id, force) if abs(effort) > .9: color = 'red' elif abs(effort) > .75: color = 'yellow' else: color = 'black' thruster_widget.findChild(QLabel, 'thrusterLabel').setText( '<span style="color: ' + color + '">' + id + '</span>') def _on_stopall_clicked(self): for thruster_widget in self._thruster_widgets.values(): thruster_widget.findChild(QSlider, 'thrusterSlider').setValue(0) def _on_stop_clicked(self, id): self._thruster_widgets[id].findChild(QSlider, 'thrusterSlider').setValue(0) def _on_manualcontrol_clicked(self): self._manual_control = self._widget.findChild(QCheckBox, 'manualControlCheckBox').isChecked() def shutdown_plugin(self): self._listener.unregister()
class ActuatorPlugin(Plugin): switches_changed = pyqtSignal(Switches) def __init__(self, context): super(ActuatorPlugin, self).__init__(context) self.setObjectName("ActuatorPlugin") self._widget = QWidget() loadUi(os.path.join(uipath, "actuatorplugin.ui"), self._widget) context.add_widget(self._widget) self._setvalve = rospy.ServiceProxy("actuator_driver/set_valve", SetValve) self._pulsevalve = rospy.ServiceProxy("actuator_driver/pulse_valve", PulseValve) self._switches_sub = rospy.Subscriber( "actuator_driver/switches", Switches, lambda msg: self.switches_changed.emit(msg) ) self.switches_changed.connect(self._on_switches_changed) self._widget.findChild(QPushButton, "shootLeftButton").clicked.connect( lambda: self._pulsevalve(VALVE_SHOOTER_LEFT, rospy.Duration(0.3)) ) self._widget.findChild(QPushButton, "shootRightButton").clicked.connect( lambda: self._pulsevalve(VALVE_SHOOTER_RIGHT, rospy.Duration(0.3)) ) self._widget.findChild(QPushButton, "extendButton").clicked.connect( lambda: self._setvalve(VALVE_GAS_POWERED_STICK, True) ) self._widget.findChild(QPushButton, "retractButton").clicked.connect( lambda: self._setvalve(VALVE_GAS_POWERED_STICK, False) ) self._widget.findChild(QPushButton, "openButton").clicked.connect(self._open_grabber) self._widget.findChild(QPushButton, "closeButton").clicked.connect(self._close_grabber) self._widget.findChild(QPushButton, "dropButton").clicked.connect( lambda: self._pulsevalve(VALVE_DROPPER, rospy.Duration(0.5)) ) self._widget.findChild(QPushButton, "shutAllValvesButton").clicked.connect(self._shut_valves) def shutdown_plugin(self): self._switches_sub.unregister() def _open_grabber(self): self._setvalve(VALVE_GRABBER_CLOSE, False) self._setvalve(VALVE_GRABBER_OPEN, True) def _close_grabber(self): self._setvalve(VALVE_GRABBER_OPEN, False) self._setvalve(VALVE_GRABBER_CLOSE, True) def _shut_valves(self): for i in xrange(6): self._setvalve(i, False) def _on_switches_changed(self, msg): if all(msg.pressed): msg = '<span style="color: red">Pressed</span>' elif any(msg.pressed): msg = '<span style="color: yellow">Single pressed</span>' else: msg = '<span style="color: green">Released</span>' self._widget.findChild(QLabel, "switchLabel").setText(msg)
class GPSPlugin(Plugin): def __init__(self, context): super(GPSPlugin, self).__init__(context) self.setObjectName('GPS') self._widget = QWidget() loadUi(os.path.join(uipath, 'gps.ui'), self._widget) context.add_widget(self._widget) self.waypoint_ecef = rospy.Publisher('/gps_ecef_waypoint',PointStamped) self.tasks = rospy.Publisher('/task_waypoints',PointStamped) rospy.Subscriber('/gps_conv/pos',PointStamped,pos_callback) self._widget.findChild(QPushButton, 'record_entered_waypoint').clicked.connect(self._on_record_entered_clicked) self._widget.findChild(QPushButton, 'record_current_waypoint').clicked.connect(self._on_record_current_clicked) self._widget.findChild(QPushButton, 'publish_waypoint_list').clicked.connect(self._on_pub_list_clicked) self._widget.findChild(QPushButton, 'delete_').clicked.connect(self._on_delete_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(1000) global tasks for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])) def _on_update(self): global tasks for i in tasks: self.tasks.publish(PointStamped( header=Header( stamp = rospy.Time.now(), frame_id=i[0], ), point=Point(i[1][0], i[1][1], i[1][2]), )) def _on_pub_list_clicked(self): self.rec_waypoint = self._widget.findChild(QListWidget, 'waypoint_list').currentItem().text() self.list = self.rec_waypoint.split(',') self.x = self.list[1] self.y = self.list[2] self.z = self.list[3] self.waypoint_ecef.publish(PointStamped( header=Header( stamp = rospy.Time.now(), frame_id='/ecef', ), point=Point(float(self.x), float(self.y), float(self.z)), )) def _on_record_current_clicked(self): global position,tasks self.name = self._widget.findChild(QLineEdit, 'waypoint_name').displayText() self._widget.findChild(QLineEdit, 'waypoint_name').clear() self._widget.findChild(QListWidget, 'waypoint_list').addItem(str(self.name)+','+str(position[0])+','+str(position[1])+','+str(position[2])) if str(self.name) in ['rings','buoys','button','spock','dock']: for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) tasks.append([str(self.name),[float(position[0]),float(position[1]),float(position[2])]]) with open(path,'w') as task_loc: for i in tasks: task_loc.write(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])+'\n') self._widget.findChild(QListWidget, 'waypoint_list').clear() for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])) def _on_record_entered_clicked(self): self.lat = self._widget.findChild(QLineEdit, 'lat_in').displayText() self.long = self._widget.findChild(QLineEdit, 'long_in').displayText() self.alt = self._widget.findChild(QLineEdit, 'alt_in').displayText() self.name = self._widget.findChild(QLineEdit, 'waypoint_name').displayText() self._widget.findChild(QLineEdit, 'lat_in').clear() self._widget.findChild(QLineEdit, 'long_in').clear() self._widget.findChild(QLineEdit, 'alt_in').clear() self._widget.findChild(QLineEdit, 'waypoint_name').clear() ecef = ecef_from_latlongheight(float(self.lat), float(self.long), float(self.alt)) global tasks if str(self.name) in ['rings','buoys','button','spock','dock']: for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) tasks.append([str(self.name),[float(ecef[0]),float(ecef[1]),float(ecef[2])]]) with open(path,'w') as task_loc: for i in tasks: task_loc.write(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])+'\n') self._widget.findChild(QListWidget, 'waypoint_list').clear() for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])) def _on_delete_clicked(self): self.rec_waypoint = self._widget.findChild(QListWidget, 'waypoint_list').currentItem().text() self.list = self.rec_waypoint.split(',') self.name = self.list[0] for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) with open(path,'w') as task_loc: for i in tasks: task_loc.write(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])+'\n') for SelectedItem in self._widget.findChild(QListWidget, 'waypoint_list').selectedItems(): self._widget.findChild(QListWidget, 'waypoint_list').takeItem(self._widget.findChild(QListWidget, 'waypoint_list').row(SelectedItem))
class KillPlugin(Plugin): def __init__(self, context): super(KillPlugin, self).__init__(context) self.setObjectName('KillPlugin') self._listener = KillListener() self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin') self._kill_active = False self._widget = QWidget() loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels( ["Name", "Status", "Description"]) self._widget.findChild(QPushButton, 'killButton').clicked.connect( self._on_kill_clicked) self._widget.findChild(QPushButton, 'unkillButton').clicked.connect( self._on_unkill_clicked) self._widget.findChild(QPushButton, 'runButton').clicked.connect( self._on_run_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(100) def _on_update(self): self._update_kills() self._update_kill() def _on_kill_clicked(self): self._kill_active = True self._broadcaster.send(True) self._update_kill() def _on_unkill_clicked(self): self._kill_active = False self._broadcaster.send(False) self._update_kill() def _on_run_clicked(self): self._on_unkill_clicked() def _update_kills(self): all_kills = self._listener.get_all_kills() if all_kills is not None: new_kills = { kill.id: kill for kill in self._listener.get_all_kills() } else: new_kills = {} table = self._widget.findChild(QTableWidget, 'killTable') row = 0 while row < table.rowCount(): id = table.item(row, 1) if id in new_kills: self._update_kill_entry(row, new_kills[id]) del new_kills[id] row += 1 else: table.removeRow(row) for kill in new_kills.values(): row = table.rowCount() table.setRowCount(row + 1) self._update_kill_entry(row, kill) def _update_kill_entry(self, row, kill): color = QColor(255, 255, 255) if not kill.active else QColor( 255, 200, 200) self._update_item(row, 0, color, kill.id) self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled") self._update_item(row, 2, color, kill.description) def _update_item(self, row, col, color, string): item = QTableWidgetItem(string) item.setBackground(color) self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item) def _update_kill(self): kills = self._listener.get_all_kills() if kills is not None: other_kill_count = len([ kill for kill in kills if kill.id != rospy.get_name() and kill.active ]) self._widget.findChild( QPushButton, 'runButton').setVisible(other_kill_count == 0) self._widget.findChild( QPushButton, 'unkillButton').setVisible(other_kill_count > 0) status = 'Sub status: ' if not self._listener.get_killed(): status += '<span style="color:green">Running</span>' else: status += '<span style="color:red">Killed</span>' self._widget.findChild(QLabel, 'killStatusLabel').setText(status) else: self._widget.findChild(QPushButton, 'runButton').setVisible(False) self._widget.findChild(QPushButton, 'unkillButton').setVisible(False) self._widget.findChild(QLabel, 'killStatusLabel').setText( '<span style="color:red">No kill information</span>')
class Engineering_Plant(Plugin): def __init__(self, context): super(Engineering_Plant, self).__init__(context) self.setObjectName('Engineering_Plant') self._widget = QWidget() loadUi(os.path.join(uipath, 'engineering_plant.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QPushButton, 'Kill').clicked.connect(self.Kill) self._widget.findChild(QPushButton, 'UnKill').clicked.connect(self.Un_kill) rospy.Subscriber('/power_router/status',prstatus,data_update) self.killservice = rospy.ServiceProxy('/power_router/setkill', SetKill) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(1000) def _on_update(self): global computer_current, motor_current, temperature, battery compratio = int(float(computer_current/300.0)*100) self._widget.findChild(QProgressBar, 'ComputerCurrent').setValue(compratio) motorratio = int(float(motor_current/9000.0)*100) self._widget.findChild(QProgressBar, 'MotorCurrent').setValue(motorratio) #tempratio = int(float(temperature/150)*100) self._widget.findChild(QLCDNumber, 'tempLCD').display(temperature) battratio = int(float(battery/100)*100) self._widget.findChild(QProgressBar, 'Battery').setValue(battratio) def Kill(self): self.killservice(True) def Un_kill(self): self.killservice(False)
class PropaGatorGUI(Plugin): def __init__(self, contex): super(PropaGatorGUI, self).__init__(contex) # Initilize variables self._float_status = False self.last_odom_msg = Odometry() self._current_controller = 'Controller: Unknown' self._controller = '' self._controllers = [] # Assign a name self.setObjectName('PropaGatorGUI') # Create a widget self._widget = QWidget() loadUi(os.path.join(cwd, 'propagatorgui.ui'), self._widget) self._widget.setObjectName('PropaGatorGUI') contex.add_widget(self._widget) # Grab all the children from the widget self._kill_label = self._widget.findChild(QLabel, 'kill_label') self._float_label = self._widget.findChild(QLabel, 'float_label') self._controller_label = self._widget.findChild( QLabel, 'controller_label') self._odom_x_label = self._widget.findChild(QLabel, 'odom_x_label') self._odom_y_label = self._widget.findChild(QLabel, 'odom_y_label') self._odom_yaw_label = self._widget.findChild(QLabel, 'odom_yaw_label') self._odom_d_x_label = self._widget.findChild(QLabel, 'odom_d_x_label') self._odom_d_y_label = self._widget.findChild(QLabel, 'odom_d_y_label') self._odom_d_yaw_label = self._widget.findChild( QLabel, 'odom_d_yaw_label') self._placeholder_label = self._widget.findChild( QLabel, 'placeholder_label') self._kill_push_btn = self._widget.findChild(QPushButton, 'kill_push_btn') self._float_push_btn = self._widget.findChild(QPushButton, 'float_push_btn') self._controller_combo_box = self._widget.findChild( QComboBox, 'controller_combo_box') # Load images self._green_indicator = QPixmap( os.path.join(cwd, 'green_indicator.png')) self._red_indicator = QPixmap(os.path.join(cwd, 'red_indicator.png')) self._placeholder_image = QPixmap(os.path.join(cwd, 'placeholder.png')) # Set up ROS interfaces self._kill_listener = KillListener() self._kill_broadcaster = KillBroadcaster( id='PropaGator GUI', description='PropaGator GUI kill') self._odom_sub = rospy.Subscriber('/odom', Odometry, self._odom_cb) self._float_sub = rospy.Subscriber('/controller/float_status', Bool, self._float_cb) self._float_proxy = rospy.ServiceProxy('/controller/float_mode', FloatMode) self._current_controller_sub = rospy.Subscriber( '/controller/current_controller', String, self._current_controller_cb) self._get_all_controllers_proxy = rospy.ServiceProxy( '/controller/get_all_controllers', get_all_controllers) self._request_controller_proxy = rospy.ServiceProxy( '/controller/request_controller', request_controller) # Connect push buttons self._kill_push_btn.toggled.connect(self._on_kill_push_btn_toggle) self._float_push_btn.toggled.connect(self._on_float_push_btn_toggle) # Connect combo boxes self._controller_combo_box.activated[str].connect( self._controller_combo_box_cb) # Set up update timer at 10Hz # A Qt timer is used instead of a ros timer since Qt components are updated self.update_timer = QTimer() self.update_timer.timeout.connect(self._onUpdate) self.update_timer.start(100) # Temp self._placeholder_label.setPixmap(self._placeholder_image) # Everything needs to be turned off here def shutdown_plugin(self): self.update_timer.stop() self._odom_sub.unregister() del self._odom_sub self._float_sub.unregister() del self._float_sub # Kill broadcaster is not cleared, the user should unkill before closing the GUI del self._kill_broadcaster del self._kill_listener # Subscriber callbacks # Since this is in a different thread it is possible and likely that # the drawing thread will try and draw while the text is being changed # this causes all kinds of mahem such as segmentation faults, double free, ... # To prevent this from hapening this thread updates only none QT variables # described here http://wiki.ros.org/rqt/Tutorials/Writing%20a%20Python%20Plugin def _odom_cb(self, msg): self.last_odom_msg = msg def _float_cb(self, status): self._float_status = status.data def _current_controller_cb(self, controller): self._controller = controller.data self._current_controller = 'Controller: ' + controller.data def _odom_update(self): pos = self.last_odom_msg.pose.pose.position yaw = quat_to_rotvec( xyzw_array(self.last_odom_msg.pose.pose.orientation))[2] vel = self.last_odom_msg.twist.twist.linear dYaw = self.last_odom_msg.twist.twist.angular.z self._odom_x_label.setText('X: %3.3f' % pos.x) self._odom_y_label.setText('Y: %3.3f' % pos.y) self._odom_yaw_label.setText('Yaw: %3.3f' % yaw) self._odom_d_x_label.setText('dX: %3.3f' % vel.x) self._odom_d_y_label.setText('dY: %3.3f' % vel.y) self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw) # Push btn callbacks def _on_kill_push_btn_toggle(self, checked): if checked: self._kill_broadcaster.send(True) else: self._kill_broadcaster.send(False) def _on_float_push_btn_toggle(self, checked): # TODO: Check if float service active try: if checked: self._float_proxy(True) else: self._float_proxy(False) except rospy.service.ServiceException: pass # Combo box callbacks def _controller_combo_box_cb(self, text): self._request_controller_proxy(text) # Update functions def _updateStatus(self): # Check if killed if self._kill_listener.get_killed(): self._kill_label.setPixmap(self._red_indicator) else: self._kill_label.setPixmap(self._green_indicator) # Check float status if self._float_status: self._float_label.setPixmap(self._red_indicator) else: self._float_label.setPixmap(self._green_indicator) # Check if in autonomous or RC self._controller_label.setText(self._current_controller) def _updateControl(self): # Wait until we get the first controller if self._controller == '': return controllers = self._get_all_controllers_proxy().controllers if controllers != self._controllers: self._controllers = controllers self._controller_combo_box.clear() self._controller_combo_box.addItems(controllers) index = self._controller_combo_box.findText(self._controller) if index != -1 and index != self._controller_combo_box.currentIndex(): self._controller_combo_box.setCurrentIndex(index) def _updateLidar(self): pass def _onUpdate(self): self._updateStatus() self._updateControl() self._updateLidar() self._odom_update()
class KillPlugin(Plugin): def __init__(self, context): super(KillPlugin, self).__init__(context) self.setObjectName('KillPlugin') self._listener = KillListener() self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin') self._kill_active = False self._widget = QWidget() loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget) context.add_widget(self._widget) self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels(["Name", "Status", "Description"]) self._widget.findChild(QPushButton, 'killButton').clicked.connect(self._on_kill_clicked) self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(self._on_unkill_clicked) self._widget.findChild(QPushButton, 'runButton').clicked.connect(self._on_run_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(100) def _on_update(self): self._update_kills() self._update_kill() def _on_kill_clicked(self): self._kill_active = True self._broadcaster.send(True) self._update_kill() def _on_unkill_clicked(self): self._kill_active = False self._broadcaster.send(False) self._update_kill() def _on_run_clicked(self): self._on_unkill_clicked() def _update_kills(self): all_kills = self._listener.get_all_kills() if all_kills is not None: new_kills = {kill.id: kill for kill in self._listener.get_all_kills()} else: new_kills = {} table = self._widget.findChild(QTableWidget, 'killTable') row = 0 while row < table.rowCount(): id = table.item(row, 1) if id in new_kills: self._update_kill_entry(row, new_kills[id]) del new_kills[id] row += 1 else: table.removeRow(row) for kill in new_kills.values(): row = table.rowCount() table.setRowCount(row+1) self._update_kill_entry(row, kill) def _update_kill_entry(self, row, kill): color = QColor(255, 255, 255) if not kill.active else QColor(255, 200, 200) self._update_item(row, 0, color, kill.id) self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled") self._update_item(row, 2, color, kill.description) def _update_item(self, row, col, color, string): item = QTableWidgetItem(string) item.setBackground(color) self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item) def _update_kill(self): kills = self._listener.get_all_kills() if kills is not None: other_kill_count = len([kill for kill in kills if kill.id != rospy.get_name() and kill.active]) self._widget.findChild(QPushButton, 'runButton').setVisible(other_kill_count == 0) self._widget.findChild(QPushButton, 'unkillButton').setVisible(other_kill_count > 0) status = 'Sub status: ' if not self._listener.get_killed(): status += '<span style="color:green">Running</span>' else: status += '<span style="color:red">Killed</span>' self._widget.findChild(QLabel, 'killStatusLabel').setText(status) else: self._widget.findChild(QPushButton, 'runButton').setVisible(False) self._widget.findChild(QPushButton, 'unkillButton').setVisible(False) self._widget.findChild(QLabel, 'killStatusLabel').setText('<span style="color:red">No kill information</span>')
class MissionPlugin(Plugin): on_plans_changed = pyqtSignal(PlansStamped) def __init__(self, context): Plugin.__init__(self, context) self.setObjectName("MissionPlugin") self._plans = None self._widget = QWidget() loadUi(os.path.join(uipath, "missionplugin.ui"), self._widget) context.add_widget(self._widget) self._widget.findChild(QPushButton, "addButton").clicked.connect(self._on_add) self._widget.findChild(QPushButton, "removeButton").clicked.connect(self._on_remove) self._widget.findChild(QPushButton, "skipToButton").clicked.connect(self._on_skip_to) self._widget.findChild(QPushButton, "startButton").clicked.connect(self._on_start) self._widget.findChild(QPushButton, "stopButton").clicked.connect(self._on_stop) self.on_plans_changed.connect(self._on_plans) self._plans_sub = rospy.Subscriber("mission/plans", PlansStamped, lambda msg: self.on_plans_changed.emit(msg)) self._modify_srv = rospy.ServiceProxy("mission/modify_plan", ModifyPlan) self._run_action = None def shutdown_plugin(self): self._plans_sub.unregister() def _on_add(self, event): selected_items = self._widget.findChild(QTreeWidget, "missionTreeWidget").selectedItems() if len(selected_items) == 0: return item = selected_items[0] if item.parent() is not None: plan = item.parent().text(0) pos = item.parent().indexOfChild(item) + 1 else: plan = item.text(0) pos = 0 contigency = self._widget.findChild(QComboBox, "contigencyCombo").currentText() if contigency == "none": contigency = "" entry = PlanEntry( mission=self._widget.findChild(QComboBox, "missionCombo").currentText(), timeout=rospy.Duration(int(self._widget.findChild(QSpinBox, "timeoutSpin").value())), contigency_plan=contigency, path=self._widget.findChild(QComboBox, "pathCombo").currentText(), dist=float(self._widget.findChild(QDoubleSpinBox, "distSpin").value()), ) self._modify_srv(plan, ModifyPlanRequest.INSERT, pos, entry) def _on_remove(self, event): selected_items = self._widget.findChild(QTreeWidget, "missionTreeWidget").selectedItems() if len(selected_items) == 0: return item = selected_items[0] if item.parent() is None: return plan = item.parent().text(0) pos = item.parent().indexOfChild(item) self._modify_srv(plan, ModifyPlanRequest.REMOVE, pos, PlanEntry()) def _on_skip_to(self, event): selected_items = self._widget.findChild(QTreeWidget, "missionTreeWidget").selectedItems() if len(selected_items) == 0: return item = selected_items[0] if item.parent() is None: return plan = item.parent().text(0) pos = item.parent().indexOfChild(item) self._modify_srv( plan, ModifyPlanRequest.REPLACE, pos, PlanEntry(item.text(0), rospy.Duration(int(item.text(1))), item.text(2), "none", 0), ) for i in xrange(pos): self._modify_srv(plan, ModifyPlanRequest.REMOVE, 0, PlanEntry()) def _on_start(self, event): if self._run_action is None: self._run_action = actionlib.SimpleActionClient("mission/run", RunMissionsAction) print "waiting for server" self._run_action.wait_for_server() print "sending goal" self._run_action.send_goal(RunMissionsGoal()) def _on_stop(self, event): if self._run_action is not None: self._run_action.cancel_goal() def _on_plans(self, msg): if self._plans == msg.plans: return self._plans = msg.plans mission_tree = self._widget.findChild(QTreeWidget, "missionTreeWidget") mission_tree.clear() contigency_combo = self._widget.findChild(QComboBox, "contigencyCombo") contigency_combo.clear() for plan in sorted(self._plans, key=lambda plan: plan.name): item = QTreeWidgetItem([plan.name]) for entry in plan.entries: contigency = entry.contigency_plan if len(contigency) == 0: contigency = "none" dist = str(entry.dist) if entry.dist != 0 else "" subitem = QTreeWidgetItem([entry.mission, str(entry.timeout.secs), contigency, entry.path, dist]) item.setFlags(item.flags() | Qt.ItemIsEditable) item.addChild(subitem) mission_tree.addTopLevelItem(item) item.setExpanded(True) if plan.name != "main": contigency_combo.addItem(plan.name) contigency_combo.addItem("none") mission_combo = self._widget.findChild(QComboBox, "missionCombo") mission_combo.clear() mission_combo.addItems(sorted(msg.available_missions))
class ActuatorPlugin(Plugin): switches_changed = pyqtSignal(Switches) def __init__(self, context): super(ActuatorPlugin, self).__init__(context) self.setObjectName('ActuatorPlugin') self._widget = QWidget() loadUi(os.path.join(uipath, 'actuatorplugin.ui'), self._widget) context.add_widget(self._widget) self._setvalve = rospy.ServiceProxy('actuator_driver/set_valve', SetValve) self._pulsevalve = rospy.ServiceProxy('actuator_driver/pulse_valve', PulseValve) self._switches_sub = rospy.Subscriber('actuator_driver/switches', Switches, lambda msg: self.switches_changed.emit(msg)) self.switches_changed.connect(self._on_switches_changed) self._widget.findChild(QPushButton, 'shootLeftButton').clicked.connect( lambda: self._pulsevalve(VALVE_SHOOTER_LEFT, rospy.Duration(.3))) self._widget.findChild(QPushButton, 'shootRightButton').clicked.connect( lambda: self._pulsevalve(VALVE_SHOOTER_RIGHT, rospy.Duration(.3))) self._widget.findChild(QPushButton, 'extendButton').clicked.connect( lambda: self._setvalve(VALVE_GAS_POWERED_STICK, True)) self._widget.findChild(QPushButton, 'retractButton').clicked.connect( lambda: self._setvalve(VALVE_GAS_POWERED_STICK, False)) self._widget.findChild(QPushButton, 'openButton').clicked.connect(self._open_grabber) self._widget.findChild(QPushButton, 'closeButton').clicked.connect(self._close_grabber) self._widget.findChild(QPushButton, 'dropButton').clicked.connect( lambda: self._pulsevalve(VALVE_DROPPER, rospy.Duration(.5))) self._widget.findChild(QPushButton, 'shutAllValvesButton').clicked.connect(self._shut_valves) def shutdown_plugin(self): self._switches_sub.unregister() def _open_grabber(self): self._setvalve(VALVE_GRABBER_CLOSE, False) self._setvalve(VALVE_GRABBER_OPEN, True) def _close_grabber(self): self._setvalve(VALVE_GRABBER_OPEN, False) self._setvalve(VALVE_GRABBER_CLOSE, True) def _shut_valves(self): for i in xrange(6): self._setvalve(i, False) def _on_switches_changed(self, msg): if all(msg.pressed): msg = '<span style="color: red">Pressed</span>' elif any(msg.pressed): msg = '<span style="color: yellow">Single pressed</span>' else: msg = '<span style="color: green">Released</span>' self._widget.findChild(QLabel, 'switchLabel').setText(msg)
class GPSPlugin(Plugin): def __init__(self, context): super(GPSPlugin, self).__init__(context) self.setObjectName('GPS') self._widget = QWidget() loadUi(os.path.join(uipath, 'gps.ui'), self._widget) context.add_widget(self._widget) self.waypoint_ecef = rospy.Publisher('/gps_ecef_waypoint', PointStamped) self.tasks = rospy.Publisher('/task_waypoints', PointStamped) rospy.Subscriber('/gps_conv/pos', PointStamped, pos_callback) self._widget.findChild(QPushButton, 'record_entered_waypoint').clicked.connect( self._on_record_entered_clicked) self._widget.findChild(QPushButton, 'record_current_waypoint').clicked.connect( self._on_record_current_clicked) self._widget.findChild(QPushButton, 'publish_waypoint_list').clicked.connect( self._on_pub_list_clicked) self._widget.findChild(QPushButton, 'delete_').clicked.connect( self._on_delete_clicked) self._update_timer = QTimer(self._widget) self._update_timer.timeout.connect(self._on_update) self._update_timer.start(1000) global tasks for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2])) def _on_update(self): global tasks for i in tasks: self.tasks.publish( PointStamped( header=Header( stamp=rospy.Time.now(), frame_id=i[0], ), point=Point(i[1][0], i[1][1], i[1][2]), )) def _on_pub_list_clicked(self): self.rec_waypoint = self._widget.findChild( QListWidget, 'waypoint_list').currentItem().text() self.list = self.rec_waypoint.split(',') self.x = self.list[1] self.y = self.list[2] self.z = self.list[3] self.waypoint_ecef.publish( PointStamped( header=Header( stamp=rospy.Time.now(), frame_id='/ecef', ), point=Point(float(self.x), float(self.y), float(self.z)), )) def _on_record_current_clicked(self): global position, tasks self.name = self._widget.findChild(QLineEdit, 'waypoint_name').displayText() self._widget.findChild(QLineEdit, 'waypoint_name').clear() self._widget.findChild(QListWidget, 'waypoint_list').addItem( str(self.name) + ',' + str(position[0]) + ',' + str(position[1]) + ',' + str(position[2])) if str(self.name) in ['rings', 'buoys', 'button', 'spock', 'dock']: for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) tasks.append([ str(self.name), [float(position[0]), float(position[1]), float(position[2])] ]) with open(path, 'w') as task_loc: for i in tasks: task_loc.write( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2]) + '\n') self._widget.findChild(QListWidget, 'waypoint_list').clear() for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2])) def _on_record_entered_clicked(self): self.lat = self._widget.findChild(QLineEdit, 'lat_in').displayText() self.long = self._widget.findChild(QLineEdit, 'long_in').displayText() self.alt = self._widget.findChild(QLineEdit, 'alt_in').displayText() self.name = self._widget.findChild(QLineEdit, 'waypoint_name').displayText() self._widget.findChild(QLineEdit, 'lat_in').clear() self._widget.findChild(QLineEdit, 'long_in').clear() self._widget.findChild(QLineEdit, 'alt_in').clear() self._widget.findChild(QLineEdit, 'waypoint_name').clear() ecef = ecef_from_latlongheight(float(self.lat), float(self.long), float(self.alt)) global tasks if str(self.name) in ['rings', 'buoys', 'button', 'spock', 'dock']: for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) tasks.append([ str(self.name), [float(ecef[0]), float(ecef[1]), float(ecef[2])] ]) with open(path, 'w') as task_loc: for i in tasks: task_loc.write( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2]) + '\n') self._widget.findChild(QListWidget, 'waypoint_list').clear() for i in tasks: self._widget.findChild(QListWidget, 'waypoint_list').addItem( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2])) def _on_delete_clicked(self): self.rec_waypoint = self._widget.findChild( QListWidget, 'waypoint_list').currentItem().text() self.list = self.rec_waypoint.split(',') self.name = self.list[0] for i in tasks: if (i[0] == str(self.name)): tasks.remove(i) with open(path, 'w') as task_loc: for i in tasks: task_loc.write( str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' + str(i[1][2]) + '\n') for SelectedItem in self._widget.findChild( QListWidget, 'waypoint_list').selectedItems(): self._widget.findChild(QListWidget, 'waypoint_list').takeItem( self._widget.findChild(QListWidget, 'waypoint_list').row(SelectedItem))