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))
