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
