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 # ROS Publisher self._publisher = None robotInterface.initializeRobotInterface() # Service Proxy and Subscriber self._service_proxy = None self._subscriber = None # 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('client'), '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') ### Map motors to their respective UI elements self.motor_widgets = { 0:self._widget.motor0_spinbox, 1:self._widget.motor1_spinbox, 2:self._widget.motor2_spinbox, 3:self._widget.motor3_spinbox, 4:self._widget.motor4_spinbox, 5:self._widget.motor5_spinbox, 6:self._widget.motor6_spinbox, 7:self._widget.motor7_spinbox } self.sensor_widgets = { 0:self._widget.sensor0_lineedit, 1:self._widget.sensor1_lineedit, 2:self._widget.sensor2_lineedit, 3:self._widget.sensor3_lineedit, 4:self._widget.sensor4_lineedit, 5:self._widget.sensor5_lineedit, 6:self._widget.sensor6_lineedit, 7:self._widget.sensor7_lineedit, 8:self._widget.sensor8_lineedit, 9:self._widget.sensor9_lineedit, 10:self._widget.sensor10_lineedit, 13:self._widget.sensor13_lineedit, 19:self._widget.sensor19_lineedit, 23:self._widget.sensor23_lineedit, 24:self._widget.sensor24_lineedit, 25:self._widget.sensor25_lineedit, 26:self._widget.sensor26_lineedit, 27:self._widget.sensor27_lineedit, 28:self._widget.sensor28_lineedit, 29:self._widget.sensor29_lineedit, 30:self._widget.sensor30_lineedit, 31:self._widget.sensor31_lineedit, 32:self._widget.sensor32_lineedit, } # Assigned zeros for values that indicate motor speed # Motors 4 and 5 are set to the most recent position from SensorValues self.zero_values = { 0:0, 1:0, 2:0, 3:0, 6:0, 7:0 } #print(self.motor_widgets) # Hook Qt UI Elements up """ self._widget.vertical_add_button.pressed.connect(self.increase_linear_speed_pressed) self._widget.vertical_subtract_button.pressed.connect(self.decrease_linear_speed_pressed) """ self.ENABLE_DEBUGGING = False self._widget.motor0_spinbox.valueChanged.connect(self.motor0_spinbox_changed) self._widget.motor1_spinbox.valueChanged.connect(self.motor1_spinbox_changed) self._widget.motor2_spinbox.valueChanged.connect(self.motor2_spinbox_changed) self._widget.motor3_spinbox.valueChanged.connect(self.motor3_spinbox_changed) self._widget.motor4_spinbox.valueChanged.connect(self.motor4_spinbox_changed) self._widget.motor5_spinbox.valueChanged.connect(self.motor5_spinbox_changed) self._widget.motor6_spinbox.valueChanged.connect(self.motor6_spinbox_changed) self._widget.motor7_spinbox.valueChanged.connect(self.motor7_spinbox_changed) self._widget.general_speed_spinbox.valueChanged.connect(self.general_spinbox_changed) self._widget.general_speed_slider.valueChanged.connect(self.general_slider_changed) self._widget.emergency_stop_button.pressed.connect(self.estop_pressed) self._widget.w_button.pressed.connect(self.w_pressed) self._widget.a_button.pressed.connect(self.a_pressed) self._widget.s_button.pressed.connect(self.s_pressed) self._widget.d_button.pressed.connect(self.d_pressed) self._widget.zero_locomotion_button.pressed.connect(self.zero_locomotion_speeds) self._widget.start_shift_button.pressed.connect(self.setup_translate_shift_timer) self._widget.translate_cancel_button.pressed.connect(self.cancel_shift) self._widget.attitude_shift_button.pressed.connect(self.setup_attitude_timer) self._widget.attitude_shift_cancel_button.pressed.connect(self.cancel_attitude_shift) self._widget.update_sensors_button.pressed.connect(self._setup_timer_update_sensors) self._widget.stop_sensor_update_button.pressed.connect(self._stop_update_timer) self._widget.debug_checkbox.toggled.connect(self.debugging_checkbox_checked) # ROS Connection Fields """ TODO: Omitted """ if self.check_if_published("") ### 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.setFocusPolicy(0x8) self._widget.setFocus() ### Keyboard teleop setup self._widget.keyPressEvent = self.keyPressEvent self._widget.keyReleaseEvent = self.keyReleaseEvent # timer to consecutively send service messages self._update_translate_timer = QTimer(self) self._update_attitude_timer = QTimer(self) self._update_sensors_timer = QTimer(self) def debugging_checkbox_checked(self): debugging_status = self._widget.debug_checkbox.isChecked() self.ENABLE_DEBUGGING = debugging_status # Keyboard Teleop with signalling """ Messy, but it works, theoretically. """ def keyPressEvent(self, event): motor_speed = self.get_general_motor_val() #print("general motor value is: %s" % motor_speed) if event.key() == Qt.Key_W: #print("W down") self.w_pressed(motor_speed) elif event.key() == Qt.Key_S: #print("S down") self.s_pressed(motor_speed) elif event.key() == Qt.Key_A: #print("A down") self.a_pressed(motor_speed) elif event.key() == Qt.Key_D: #print("D down") self.d_pressed(motor_speed) # Emergency stop, triggers for all motors. Can include one explicitly defined for locomotion though elif event.key() == Qt.Key_E: print("Emergency Stopping") self.estop_pressed() # Deposition keys elif event.key() == Qt.Key_U: print("Press U key") # Arrow keys to manipulate the general spinbox speed elif event.key() == Qt.Key_Up: print("Key up") self._widget.general_speed_spinbox.setValue(motor_speed + 1) elif event.key() == Qt.Key_Down: print("Key down") self._widget.general_speed_spinbox.setValue(motor_speed - 1) # Generalize sending spinbox value, handle print/error here def send_spinbox_value(self, motorNum, value): resp = robotInterface.sendMotorCommand(motorNum, value) print("For motor %s sent value %s successfully: %s" % (motorNum, value, resp)) # Currently only geared toward locomotion def keyReleaseEvent(self, event): if event.key() in (Qt.Key_W, Qt.Key_S, Qt.Key_A, Qt.Key_D): if not event.isAutoRepeat(): print("Key released") robotInterface.sendDriveCommand(0, 0) def set_locomotion_speeds(self, port_speed, starboard_speed): respPort = robotInterface.sendMotorCommand(0, port_speed) respStarboard = robotInterface.sendMotorCommand(1, starboard_speed) print("Set locomotion speeds: %s" % (respPort and respStarboard)) def zero_locomotion_speeds(self): self.motor_widgets.get(0).setValue(0) self.motor_widgets.get(1).setValue(0) self.set_locomotion_speeds(0,0) def get_general_motor_val(self): val = int(self._widget.general_speed_spinbox.value()) return val def w_pressed(self, motor_speed=None): if motor_speed is None: motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(0, motor_speed) if self.ENABLE_DEBUGGING: print("w key pressed") def a_pressed(self, motor_speed=None): if motor_speed is None: motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(3, motor_speed) if self.ENABLE_DEBUGGING: print("a key pressed") def s_pressed(self, motor_speed=None): if motor_speed is None: motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(1, motor_speed) if self.ENABLE_DEBUGGING: print("s key pressed") def d_pressed(self, motor_speed=None): if motor_speed is None: motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(2, motor_speed) if self.ENABLE_DEBUGGING: print("d key pressed") def estop_pressed(self): print("ESTOP: Attempting to stop all motors...") # Set all known motors to value 0 for motor_id, zero_value in self.zero_values.items(): #ui_widget.setValue(0) robotInterface.sendMotorCommand(motor_id, 0) # Motors 4 and 5 are set by position translationPos = robotInterface.sensorValueMap.get(4) bcAttitudePos = robotInterface.sensorValueMap.get(5) print("ESTOP: Setting translation position: %s and attitude: %s" % (translationPos, bcAttitudePos) ) robotInterface.sendMotorCommand(4, translationPos) robotInterface.sendMotorCommand(5, bcAttitudePos) # Stop any updated changes to the translation system self._update_translate_timer = QTimer(self) # ROS Connection things """ @Slot(str) def _on_topic_changed(self, topic): topic = str(topic) self._unregister_publisher() print("trying topic: ", topic) if topic == '': return try: self._publisher = rospy.Publisher(topic, Twist, queue_size = 10) except TypeError: self._publisher = rospy.Publisher(topic, Twist) """ def check_if_published(topic_name): topic_list = rospy.get_published_topics() if topic_name in topic_list: return True else: return False """ Unregister ROS publisher """ def _unregister_publisher(self): if self._publisher is not None: self._publisher.unregister() self._publisher = None if self._service_proxy is not None: # TODO:Doesn't actually shutdown/unregister?? #self._service_proxy.shutdown('Shutting down service proxy...') self._service_proxy = None #if robotInterface is not None: # robotInterface.motorCommandPub.unregister() #### Speed and Angle change Functions """ Individual Motor Change Functions """ def motor0_spinbox_changed(self): val = int(self.motor_widgets.get(0).value()) self.send_spinbox_value(0, val) def motor1_spinbox_changed(self): val = int(self.motor_widgets.get(1).value()) self.send_spinbox_value(1, val) def motor2_spinbox_changed(self): val = int(self.motor_widgets.get(2).value()) self.send_spinbox_value(2, val) def motor3_spinbox_changed(self): val = int(self.motor_widgets.get(3).value()) self.send_spinbox_value(3, val) def motor4_spinbox_changed(self): val = int(self.motor_widgets.get(4).value()) self.send_spinbox_value(4, val) def motor5_spinbox_changed(self): val = int(self.motor_widgets.get(5).value()) self.send_spinbox_value(5, val) self._widget.attitude_slider.setValue(val) def motor5_slider_changed(self): val = int(self._widget.attitude_slider.value()) ### Looky Spinboxes def motor6_spinbox_changed(self): val = int(self.motor_widgets.get(6).value()) self.send_spinbox_value(6, val) def motor7_spinbox_changed(self): val = int(self.motor_widgets.get(7).value()) self.send_spinbox_value(7, val) ### Translation timer-updated shift to some specified value def setup_translate_shift_timer(self): updatesPerSec = int(self._widget.steps_per_sec_spinbox.value()) msUpdate = int(1000/updatesPerSec) self._update_translate_timer = QTimer() self._update_translate_timer.timeout.connect(self.shift_timer_func) self._update_translate_timer.start(msUpdate) def shift_timer_func(self): currentValue = self._widget.motor4_spinbox.value() targetValue = int(self._widget.target_translate_spinbox.value()) if(targetValue == currentValue): self._update_translate_timer = QTimer(self) self._widget.translate_cancel_button.setEnabled(False) return else: delta = (targetValue - currentValue)/abs(targetValue - currentValue) #print(delta) self._widget.motor4_spinbox.setValue(currentValue + delta) self._widget.translate_cancel_button.setEnabled(True) def setup_attitude_timer(self): updatesPerSec = int(self._widget.steps_attitude_spinbox.value()) msUpdate = int(1000/updatesPerSec) self._update_attitude_timer = QTimer() self._update_attitude_timer.timeout.connect(self.shift_timer_attitude_func) self._update_attitude_timer.start(msUpdate) def shift_timer_attitude_func(self): currentValue = self._widget.motor5_spinbox.value() targetValue = int(self._widget.target_attitude_spinbox.value()) if(targetValue == currentValue): self._update_attitude_timer = QTimer(self) self._widget.attitude_shift_cancel_button.setEnabled(False) else: delta = (targetValue - currentValue)/abs(targetValue - currentValue) self._widget.motor5_spinbox.setValue(currentValue + delta) self._widget.attitude_shift_cancel_button.setEnabled(True) # Cancel the 'shift' by just setting the reference to a new QTimer def cancel_shift(self): self._update_translate_timer.stop() self._widget.translate_cancel_button.setEnabled(False) def cancel_attitude_shift(self): self._update_attitude_timer.stop() self._widget.attitude_shift_cancel_button.setEnabled(False) """ Grouped Motor Control Functions """ def general_spinbox_changed(self): self._widget.general_speed_slider.setValue(self.get_general_motor_val()) if self._widget.general_assign_checkbox.isChecked(): motor_speed = self.get_general_motor_val() for motor_id, ui_widget in self.motor_widgets.items(): ui_widget.setValue(motor_speed) else: return def general_slider_changed(self): sliderValue = int(self._widget.general_speed_slider.value()) self._widget.general_speed_spinbox.setValue(sliderValue) #print("Slider value is: %s" % sliderValue) """ Update sensor values """ """ Sending messages """ # Iteratively send values def _on_parameter_changed(self): for motor_id, ui_widget in self.motor_widgets.items(): # If widget is spinbox, val = int(ui_widget.value()) resp = self._send_motor_command(motor_id, val) print("on motor: %s" % motor_id + ", value: %s" % val + "sending result: %s" % resp) """ Sends a single commanded value (0-100) to a specified motor id """ def _send_motor_command(self, motor_id, val): try: robotInterface.sendMotorCommand(motor_id, val) print("Sent motor command for motor: %s" % motor_id + " with value: %s" % val) except Exception as exc: print("There was a problem sending the motor command: " + str(exc)) """ Sensor value updating """ def _stop_update_timer(self): self._update_sensors_timer = QTimer() def _setup_timer_update_sensors(self): updatesPerSec = 100 msUpdate = int(1000/updatesPerSec) self._update_sensors_timer = QTimer() self._update_sensors_timer.timeout.connect(self.try_update_sensors) self._update_sensors_timer.start(msUpdate) def try_update_sensors(self): try: for sensor_id, sensor_widget in self.sensor_widgets.items(): # Try to get value sensorVal = robotInterface.sensorValueMap.get(sensor_id) sensor_widget.setText(str(sensorVal)) except Exception as exc: print("There was an unusual problem updating sensors: " + str(exc)) def _set_status_text(self, text): self._widget.status_label.setText(text) def shutdown_plugin(self): # TODO unregister all publishers here #self._update_parameter_timer.stop() self._unregister_publisher() 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('client'), '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') 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.setFocusPolicy(0x8) self._widget.setFocus() #Widget vars self.ENABLE_DEBUGGING = False self.general_speed_slider_conversion_factor = 0.1 self.excavation_angle_slider_conversion_factor = 0.01 self._subscriber_widget_map = {} self._widget.stop_updating_button.setEnabled(False) '''Set reasonable ranges for the motors deposition bucket speed is from -1 to 1''' self._widget.dep_bucket_speed_spinbox.setRange(-1, 1) self._widget.dep_bucket_speed_spinbox.setSingleStep(0.1) '''excavation speed is from -1 to 1''' self._widget.excavation_speed_spinbox.setRange(-1, 1) self._widget.excavation_speed_spinbox.setSingleStep(0.1) '''excavation depth is from 0 to 0.4''' self._widget.excavation_depth_spinbox.setRange(0, 0.4) self._widget.excavation_depth_spinbox.setSingleStep(0.05) '''excavation angle is from 0 to 1.57''' self._widget.excavation_angle_spinbox.setRange(0, 1.57) self._widget.excavation_angle_spinbox.setSingleStep(0.02) #Configure the slider counterpart self._widget.excavation_angle_slider.setRange(0, 157) self._widget.excavation_angle_slider.setSingleStep(2) '''drive speed is from -0.6 to 0.6 can also used to control all other motor spinbox values''' self._widget.general_speed_spinbox.setRange(0, 0.6) self._widget.general_speed_spinbox.setSingleStep(0.1) #Configure the slider counterpart self._widget.general_speed_slider.setRange(0, 6) self._widget.general_speed_slider.setSingleStep(1) #Configure the goal value slider self._widget.goal_value_slider.setRange(0, 2) self._widget.goal_value_slider.setSingleStep(1) #All spinbox valueChanged callbacks self._widget.dep_bucket_speed_spinbox.valueChanged.connect( self.dep_bucket_speed_changed) self._widget.excavation_speed_spinbox.valueChanged.connect( self.excavation_speed_changed) self._widget.excavation_depth_spinbox.valueChanged.connect( self.excavation_depth_changed) self._widget.excavation_angle_spinbox.valueChanged.connect( self.excavation_angle_changed) self._widget.general_speed_spinbox.valueChanged.connect( self.general_spinbox_changed) #All slider valueChanged callbacks self._widget.excavation_angle_slider.valueChanged.connect( self.excavation_angle_slider_changed) self._widget.goal_value_slider.valueChanged.connect( self.goal_value_slider_changed) self._widget.general_speed_slider.valueChanged.connect( self.general_slider_changed) #All button pressed callbacks self._widget.emergency_stop_button.pressed.connect(self.estop_pressed) self._widget.w_button.pressed.connect(self.w_pressed) self._widget.a_button.pressed.connect(self.a_pressed) self._widget.s_button.pressed.connect(self.s_pressed) self._widget.d_button.pressed.connect(self.d_pressed) self._widget.dig_button.pressed.connect(self.dig_pressed) self._widget.dump_button.pressed.connect(self.dump_pressed) self._widget.debug_checkbox.toggled.connect( self.debugging_checkbox_checked) self._widget.start_updating_button.pressed.connect( self.start_updating_sensors) self._widget.stop_updating_button.pressed.connect( self.stop_updating_sensors) ### Keyboard teleop setup self._widget.keyPressEvent = self.keyPressEvent self._widget.keyReleaseEvent = self.keyReleaseEvent def debugging_checkbox_checked(self): debugging_status = self._widget.debug_checkbox.isChecked() self.ENABLE_DEBUGGING = debugging_status # Keyboard Teleop with signalling """ Messy, but it works, theoretically. """ def keyPressEvent(self, event): if event.key() == Qt.Key_W: self.w_pressed() elif event.key() == Qt.Key_S: self.s_pressed() elif event.key() == Qt.Key_A: self.a_pressed() elif event.key() == Qt.Key_D: self.d_pressed() # Emergency stop, triggers for all motors. Can include one explicitly defined for locomotion though elif event.key() == Qt.Key_E: rospy.loginfo("Emergency Stopping") self.estop_pressed() # Deposition keys elif event.key() == Qt.Key_U: rospy.loginfo("Press U key") # Arrow keys to manipulate the general spinbox speed elif event.key() == Qt.Key_Up: motor_speed = self.get_general_motor_val() rospy.loginfo("Key up") self._widget.general_speed_spinbox.setValue(motor_speed + 1) elif event.key() == Qt.Key_Down: motor_speed = self.get_general_motor_val() rospy.loginfo("Key down") self._widget.general_speed_spinbox.setValue(motor_speed - 1) # Currently only geared toward locomotion def keyReleaseEvent(self, event): if event.key() in (Qt.Key_W, Qt.Key_S, Qt.Key_A, Qt.Key_D): if not event.isAutoRepeat(): rospy.loginfo("Key released") robotInterface.sendDriveCommand(0, 0) def get_general_motor_val(self): val = float(self._widget.general_speed_spinbox.value()) return val def w_pressed(self): motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(0, motor_speed) if self.ENABLE_DEBUGGING: rospy.loginfo("w key pressed") def a_pressed(self): motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(3, motor_speed) if self.ENABLE_DEBUGGING: rospy.loginfo("a key pressed") def s_pressed(self): motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(1, motor_speed) if self.ENABLE_DEBUGGING: rospy.loginfo("s key pressed") def d_pressed(self): motor_speed = self.get_general_motor_val() robotInterface.sendDriveCommand(2, motor_speed) if self.ENABLE_DEBUGGING: rospy.loginfo("d key pressed") def estop_pressed(self): rospy.loginfo("ESTOP: Attempting to stop all motors...") # Set all known motors to value 0 robotInterface.sendWheelSpeed(0) robotInterface.sendExcavationSpeed(0) robotInterface.sendDepositionBucketSpeed(0) # Excavation depth and conveyor angle are set to the most recent position from SensorValues exc_depth = self._widget.excavation_depth_sv.text() exc_angle = self._widget.excavation_depth_sv.text() if exc_depth == '': exc_depth = 0.0 if exc_angle == '': exc_angle = 0.0 robotInterface.sendExcavationDepth(float(exc_depth)) robotInterface.sendExcavationAngle(float(exc_angle)) def dig_pressed(self): val = int(self._widget.goal_value_slider.value()) robotInterface.sendDigAction(val) def dump_pressed(self): val = int(self._widget.goal_value_slider.value()) robotInterface.sendDumpAction(val) """ Unregister ROS publisher """ def _unregister_publisher(self): self.stop_updating_sensors() """ Individual Motor Change Functions """ def dep_bucket_speed_changed(self): val = float(self._widget.dep_bucket_speed_spinbox.value()) robotInterface.sendDepositionBucketSpeed(val) def excavation_speed_changed(self): val = float(self._widget.excavation_speed_spinbox.value()) robotInterface.sendExcavationSpeed(val) def excavation_depth_changed(self): val = float(self._widget.excavation_depth_spinbox.value()) robotInterface.sendExcavationDepth(val) def excavation_angle_changed(self): val = float(self._widget.excavation_angle_spinbox.value()) robotInterface.sendExcavationAngle(val) self._widget.excavation_angle_slider.setValue(int(val * 100)) def excavation_angle_slider_changed(self): val = float(self._widget.excavation_angle_slider.value() * self.excavation_angle_slider_conversion_factor) self._widget.excavation_angle_spinbox.setValue(val) def goal_value_slider_changed(self): val = self._widget.goal_value_slider.value() self._widget.goal_value_label.setText(str(val)) """ Grouped Motor Control Functions """ def general_spinbox_changed(self): self._widget.general_speed_slider.setValue( int(self.get_general_motor_val() * 10)) if self._widget.general_assign_checkbox.isChecked(): motor_speed = self.get_general_motor_val() self._widget.dep_bucket_speed_spinbox.setValue(motor_speed) self._widget.excavation_speed_spinbox.setValue(motor_speed) self._widget.excavation_depth_spinbox.setValue(motor_speed) self._widget.excavation_angle_spinbox.setValue(motor_speed) def general_slider_changed(self): sliderValue = int(self._widget.general_speed_slider.value()) self._widget.general_speed_spinbox.setValue( sliderValue * self.general_speed_slider_conversion_factor) """ Sensor value updating """ def start_updating_sensors(self): self._widget.stop_updating_button.setEnabled(True) self._widget.start_updating_button.setEnabled(False) #Initialize the subscribers self._imu_data_sub = rospy.Subscriber( '/imu/data_raw', Imu, lambda data: self._widget.imu_data_raw_sv.setText( str(data.angular_velocity.z))) self._realsense_data_sub = rospy.Subscriber( '/imu_realsense/data_raw', Imu, lambda data: self._widget.realsense_data_raw_sv.setText( str(data.angular_velocity.z))) self._top_limit_switch_sub = rospy.Subscriber( '/dumper/top_limit_switch', Bool, lambda data: self._widget.top_limit_switch_sv.setText( str(data.data))) self._dumper_position_sub = rospy.Subscriber( '/dumper/position', Float32, lambda data: self._widget.dumper_position_sv.setText( str(data.data))) self._dumper_weight_sub = rospy.Subscriber( '/dumper/weight', Float32, lambda data: self._widget.dumper_weight_sv.setText(str(data.data))) self._exc_depth_sub = rospy.Subscriber( '/excavation/depth', Float32, lambda data: self._widget.excavation_depth_sv.setText( str(data.data))) self._exc_angle_sub = rospy.Subscriber( '/excavation/angle', Float32, lambda data: self._widget.excavation_angle_sv.setText( str(data.data))) self._encocoders_sub = rospy.Subscriber( '/glenn_base/encoders', Encoders, lambda data: self._widget. encoders_sv.setText('Left: %.3f, right: %.3f' % (data.left, data.right))) #Link subscribers to their widgets self._subscriber_widget_map = { self._imu_data_sub: self._widget.imu_data_raw_sv, self._realsense_data_sub: self._widget.realsense_data_raw_sv, self._top_limit_switch_sub: self._widget.top_limit_switch_sv, self._dumper_position_sub: self._widget.dumper_position_sv, self._dumper_weight_sub: self._widget.dumper_weight_sv, self._exc_depth_sub: self._widget.excavation_depth_sv, self._exc_angle_sub: self._widget.excavation_angle_sv, self._encocoders_sub: self._widget.encoders_sv } def stop_updating_sensors(self): self._widget.stop_updating_button.setEnabled(False) self._widget.start_updating_button.setEnabled(True) for subscriber, sv_widget in self._subscriber_widget_map.items(): subscriber.unregister() sv_widget.setText('') def shutdown_plugin(self): # TODO unregister all publishers here #self._update_parameter_timer.stop() self._unregister_publisher() 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